def compute_initial_direction_exact_position(self, man_pos, target_edge):
p1, p2 = target_edge
angle_1 = __calAngle__(man_pos, p1)
angle_2 = __calAngle__(man_pos, p2)
if angle_1 > angle_2:
if angle_1 - angle_2 > pi:
return [(0, angle_2), (angle_1, pi + pi)]
else:
return [(angle_2, angle_1)]
elif angle_2 - angle_1 >pi:
return [(0, angle_1), (angle_2, pi + pi)]
else:
return [(angle_1, angle_2)]
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 = __calAngle__(*surface_vec)
if surface_angle1 > pi:
surface_angle2 = surface_angle1 - pi
else:
surface_angle2 = surface_angle1 + 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
