def get_required_bounce_dir(self, backward_source_edge, backward_target_edge):
p1, p2 = self.vertices_of_edges[backward_source_edge]
p3, p4 = self.vertices_of_edges[backward_target_edge]
dir_p1p3 = cal_angle(p1, p3)
dir_p1p4 = cal_angle(p1, p4)
dir_p2p3 = cal_angle(p2, p3)
dir_p2p4 = cal_angle(p2, p4)
bouncing_dir_p1p3 = self.updated_bouncing_direction(dir_p1p3, self.vertices_of_edges[backward_target_edge])
bouncing_dir_p1p4 = self.updated_bouncing_direction(dir_p1p4, self.vertices_of_edges[backward_target_edge])
bouncing_dir_p2p3 = self.updated_bouncing_direction(dir_p2p3, self.vertices_of_edges[backward_target_edge])
bouncing_dir_p2p4 = self.updated_bouncing_direction(dir_p2p4, self.vertices_of_edges[backward_target_edge])
return
def compute_initial_direction_exact_position(self, man_pos, target_edge):
p1, p2 = target_edge
angle_1 = cal_angle(man_pos, p1)
angle_2 = cal_angle(man_pos, p2)
if angle_1 > angle_2:
if angle_1 - angle_2 > pi:
return (angle_1 - pi_2, angle_2)
#return [(0, angle_2), (angle_1, pi)]
else:
return (angle_2, angle_1)
elif angle_2 - angle_1 >pi:
return (angle_2 - pi_2, angle_1)
else:
return (angle_1, angle_2)
def can_bounce(self, pos, surface):
required_direction_1, required_direction_2 = self.get_required_direction_from_other_edges(surface)
p1, p2 = self.vertices_of_edges[surface]
angle1 = cal_angle(pos, p1)
angle2 = cal_angle(pos, p2)
if angle1 > angle2:
if angle1 - angle2 > pi:
direction_range = (angle1 - pi_2, angle2)
else:
direction_range = (angle2, angle1)
else:
if angle2 - angle1 > pi:
direction_range = (angle2 - pi_2, angle1)
else:
direction_range = (angle1, angle2)
if self.update_direction(direction_range, required_direction_1) != None:
return True
if self.update_direction(direction_range, required_direction_2) != None:
return True
return False
def reflection_angle(self, direction1, surface_vec):
def is_intersection(hit_dir, surface_dir):
if hit_dir > surface_dir:
difference = hit_dir - surface_dir
else:
difference = surface_dir - hit_dir
_flag = False
if difference > pi:
difference = pi + pi - difference
_flag = True
if difference <= pi/2:
result = surface_dir - hit_dir
#print "pre plus: ", result
if result < 0 and _flag:
result = pi + pi + result
#print "result: ", result
return result
else:
return -1
surface_angle1 = cal_angle(*surface_vec)
if surface_angle1 > pi:
surface_angle2 = surface_angle1 - pi
else:
surface_angle2 = surface_angle1 + pi
if direction1 < 0:
direction1 = direction1 + pi + pi
difference = is_intersection(direction1, surface_angle1)
if difference != -1:
reflected_angle = surface_angle1 + difference
else:
#print is_intersection(direction1, surface_angle2)
reflected_angle = surface_angle2 + is_intersection(direction1, surface_angle2)
if reflected_angle >= pi + pi:
reflected_angle = reflected_angle - pi - pi
elif reflected_angle < 0:
reflected_angle = reflected_angle + pi + pi
return reflected_angle
def infer_next_states(self, current_state, pre_node):
next_states = []
#print "test: ", current_state
current_node, entering_edge, current_direction, current_motion, is_collided, give_impulse_direction, obj_id = current_state
#if current_node == 5 and entering_edge == frozenset([10,11]) and current_motion == SLIDING and not is_collided:
# print " " ," ", current_state
#direction_x, direction_y = current_direction
#print "current_node: ", current_node
edges = self.edges_by_tri[current_node]
#if current_node == 40:
# print "cdir before apply gravity: ", current_direction
if current_motion == FLYING:
######### Add Gravity Here ##############################
current_direction = self.apply_gravity(current_direction)
######### Add Gravity Here ##############################
#if current_node == 5 and entering_edge == frozenset([10, 11]):
# print "after apply gravity: ", current_direction
for edge in edges:
#print self.is_edge_surface[edge]
if edge != entering_edge and self.is_edge_surface[edge] != -1:
required_direction = self.get_required_direction(entering_edge, edge)
updated_direction = self.update_direction(current_direction, required_direction)
#if current_node == 5 and entering_edge == frozenset([10,11]):
# print current_node, " cdir: ", current_direction, "rdir: ", required_direction," udir: ", updated_direction, " entering edge: ", entering_edge, " arrive edge", edge
direction_change_flag = False
############## ADD CODE FOR GRAVITY #######################
## consider gravity:
'''
if len(updated_direction)==0:
#merge_direction = self.merge_with_graivty_dir(current_direction)
#updated_direction = self.update_direction(merge_direction, required_direction)
updated_direction = self.update_direction([(GRAVITY_DIR,GRAVITY_DIR)], required_direction)
if len(updated_direction) != 0:
#print "use gravity at state: ", current_state, " rdir ", required_direction, " merge_dir: ", merge_direction
direction_change_flag = True
'''
############## ADD CODE FOR GRAVITY #######################
if updated_direction == None:
#print " entering_edge: ", entering_edge, " current_direction: ", current_direction
continue
else:
if self.is_edge_surface[edge] == 1:
# does not allow moveable object id to bounce [unless it is circle]
if obj_id != self.initial_obj_id:
state = (current_node, edge, updated_direction, FLYING, False, updated_direction, obj_id)
next_states.append(state)
else:
## CAN be SLIDING, or FLYING BACK
## For Flying BACK. Flying Back is DANGEROUS, CAUSE LOOP
#required_direction_fly_back = self.get_required_direction(edge, entering_edge)
intersection_angle_1 = self.get_intersection_angle_from_directions(updated_direction[0], cal_angle(*self.vertices_of_edges[edge]))
intersection_angle_2 = self.get_intersection_angle_from_directions(updated_direction[1], cal_angle(*self.vertices_of_edges[edge]))
if (intersection_angle_1 != 0 and intersection_angle_1 != pi) or (intersection_angle_2 != 0 and intersection_angle_2 != pi):
updated_bouncing_direction = self.update_reflection_angles(updated_direction, self.vertices_of_edges[edge])
#print current_node, "current_direction: ", current_direction," bouncing direction:", updated_bouncing_direction
## Do not allow jump between two surfaces (additional checked by isloop)
state = (current_node, edge, updated_bouncing_direction, FLYING, True, updated_direction, obj_id)
#if current_node == 40:
# print "note 40: state before bouncing: ", current_direction
# print "node 40: state after bouncing: ", state, "\n"
next_states.append(state)
'''
if self.is_edge_surface[entering_edge] != 1:
state = (self.tri_neighbor_by_edge[current_node][entering_edge], entering_edge, required_direction_fly_back, FLYING)
next_states.append(state)
#print "current_node: ", current_node, " flying back: ", state
'''
### SLIDING on this edge. It may not necessarily slide. if it cannot, it will be terminated at the next expanding anyway. can be changed later
######### BUG AREA [Seems Fixed] ########################
connected_surfaces = self.surface_rel_dic[edge]
for surface_info in connected_surfaces:
connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
for angle in current_direction:
if self.get_intersection_angle_from_directions(angle, on_surface_direction[0]) >= pi/2:
continue
else:
state = (current_node, edge, on_surface_direction, SLIDING, True, updated_direction, obj_id)
next_states.append(state)
break
'''
for direction_range in current_direction:
if not can_slide:
for angle in direction_range:
#if current_node == 32:
# print angle, " ", on_surface_direction, " ", self.get_intersection_angle_from_directions(angle, on_surface_direction[0][0])
if self.get_intersection_angle_from_directions(angle, on_surface_direction[0][0]) >= pi/2:
continue
else:
state = (current_node, edge, on_surface_direction, SLIDING, True, updated_direction, obj_id)
next_states.append(state)
can_slide = True
'''
######### BUG AREA ########################
else:
if self.edges_length[edge] > self.object_size:
#print self.tri_neighbor_by_edge, " ", current_node
#for neighbor in neighbors:
# print neighbor, " ", graph.edges(current_node), " current_node: ", current_node
state = (self.tri_neighbor_by_edge[current_node][edge] , edge, updated_direction, FLYING, direction_change_flag, updated_direction, obj_id)
#print " next state: ", state
next_states.append(state)
#print "current_direction, ", current_direction, " required_direction: ", required_direction, " updated_direction: ", updated_direction, " new state: ", state
elif current_motion == SLIDING:
#print entering_edge
#print self.is_edge_surface[entering_edge]
### IMPORTANT: NEED TO IMPLEMNT TRASITION FROM SLIDE TO FLY
connected_surfaces = self.surface_rel_dic[entering_edge]
for surface_info in connected_surfaces:
connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
required_directions = self.get_required_direction_to_other_edges(connected_surface)
#print "test sliding relation: ", current_node, "current direction: ", current_direction, " entering_direction: ", entering_direction, " required_directions: ", required_directions, " connected_tri: ", self.tri_by_surface[connected_surface], " can slide: ", self.can_slide(intersection_angle, required_directions[0],required_directions[1])
#if current_node == 5:
# print current_direction, " ", on_surface_direction
if current_direction == on_surface_direction:
#print "current_direction : ", current_direction, " on_surface_direction: ", on_surface_direction
if self.can_slide(intersection_angle, required_directions[0],required_directions[1]):
state = (self.tri_by_surface[connected_surface], connected_surface, exiting_direction, SLIDING, True, current_direction , obj_id)
next_states.append(state)
else:
## IMPORTABNT, POTENTIAL BUG, the ball may only collide the connected surface when the surface is upwards in front of the ball
updated_bouncing_direction = self.update_reflection_angles(current_direction, self.vertices_of_edges[connected_surface])
#print current_node, "current_direction: ", current_direction," bouncing direction:", updated_bouncing_direction
## Do not allow jump between two surfaces (additional checked by isloop)
state = (current_node, connected_surface, updated_bouncing_direction, SLIDING, True, current_direction, obj_id)
#print "bouncing state: ", state
next_states.append(state)
## Add EVENT
#if current_node == 5:
# print "before ", current_direction, " ", on_surface_direction
#state = (current_node, entering_edge, current_direction, FLYING, False, current_direction, obj_id)
#if current_node == 5:
# print " after ", state, current_direction
#next_states.append(state)
#print " inferred states: ", next_states
return next_states
def get_normal(v1, v2, v3):
angle1 = cal_angle(v1, v2)
angle2 = cal_angle(v2, v3)
return (angle1 + angle2)/2
