def condition_slide_right(self):
heading_gtg = self.supervisor.go_to_goal_controller.gtg_heading_vector
heading_ao = self.supervisor.avoid_obstacles_controller.ao_heading_vector
heading_fwr = self.supervisor.follow_wall_controller.r_fw_heading_vector
ao_cross_fwr = linalg.cross(heading_ao, heading_fwr)
fwr_cross_gtg = linalg.cross(heading_fwr, heading_gtg)
ao_cross_gtg = linalg.cross(heading_ao, heading_gtg)
return ((ao_cross_gtg > 0.0 and ao_cross_fwr > 0.0 and fwr_cross_gtg > 0.0) or
(ao_cross_gtg <= 0.0 and ao_cross_fwr <= 0.0 and fwr_cross_gtg <= 0.0))
def condition_slide_right( self ):
heading_gtg = self.supervisor.go_to_goal_controller.gtg_heading_vector
heading_ao = self.supervisor.avoid_obstacles_controller.ao_heading_vector
heading_fwr = self.supervisor.follow_wall_controller.r_fw_heading_vector
ao_cross_fwr = linalg.cross( heading_ao, heading_fwr )
fwr_cross_gtg = linalg.cross( heading_fwr, heading_gtg )
ao_cross_gtg = linalg.cross( heading_ao, heading_gtg )
return( ( ao_cross_gtg > 0.0 and ao_cross_fwr > 0.0 and fwr_cross_gtg > 0.0 ) or
( ao_cross_gtg <= 0.0 and ao_cross_fwr <= 0.0 and fwr_cross_gtg <= 0.0 ) )
def line_segment_intersection(line1, line2):
# see http://stackoverflow.com/questions/563198
nointersect_symbol = (False, None, None)
p1, r1 = line1[0], linalg.sub(line1[1], line1[0])
p2, r2 = line2[0], linalg.sub(line2[1], line2[0])
r1xr2 = float(linalg.cross(r1, r2))
if r1xr2 == 0.0: return nointersect_symbol
p2subp1 = linalg.sub(p2, p1)
d1 = linalg.cross(p2subp1, r2) / r1xr2
d2 = linalg.cross(p2subp1, r1) / r1xr2
if d1 >= 0.0 and d1 <= 1.0 and d2 >= 0.0 and d2 <= 1.0:
return True, linalg.add(p1, linalg.scale(r1, d1)), d1
else:
return nointersect_symbol
def line_segment_intersection( line1, line2 ):
# see http://stackoverflow.com/questions/563198
nointersect_symbol = ( False, None, None )
p1, r1 = line1[0], linalg.sub( line1[1], line1[0] )
p2, r2 = line2[0], linalg.sub( line2[1], line2[0] )
r1xr2 = float( linalg.cross( r1, r2 ) )
if r1xr2 == 0.0: return nointersect_symbol
p2subp1 = linalg.sub( p2, p1 )
d1 = linalg.cross( p2subp1, r2 ) / r1xr2
d2 = linalg.cross( p2subp1, r1 ) / r1xr2
if d1 >= 0.0 and d1 <= 1.0 and d2 >= 0.0 and d2 <= 1.0:
return True, linalg.add( p1, linalg.scale( r1, d1 ) ), d1
else:
return nointersect_symbol
