def find_walk_path(self, dest):
if self.is_line_walkable( geometry.LineSegment( self.walker_position, dest ) ):
return [ self.walker_position, dest ]
vertices = self.vertices + [ self.walker_position, dest ]
vertice_count = len( vertices )
edges = self.edges.copy( )
for i in range( vertice_count - 2 ):
if self.is_line_walkable( geometry.LineSegment( vertices[ i ], self.walker_position ) ):
edges.append( ( i, vertice_count - 2 ) )
if self.is_line_walkable( geometry.LineSegment( vertices[ i ], dest ) ):
edges.append( ( i, vertice_count - 1 ) )
graph = { i : dict( ) for i in range( len( vertices ) ) }
for edge in edges:
distance = geometry.distance( vertices[ edge[0] ], vertices[ edge[1] ] )
graph[ edge[0] ][ edge[1] ] = distance
graph[ edge[1] ][ edge[0] ] = distance
start_time = time.time()
path = pathfinder.find_cheapest_path_dijkstra( graph, vertice_count-2, vertice_count-1 )
end_time = time.time()
print( "dijkstra algorithm took {} seconds".format( end_time - start_time ) )
if path is None:
return None
path_coordinates = [ vertices[ vertice_i ] for vertice_i in path["path"] ]
return path_coordinates
def is_line_walkable(self, walk_line_segment):
if self.quad_tree is None:
return False
start_circle = geometry.Circle( walk_line_segment.start, self.WALKER_COMFORT_ZONE )
end_circle = geometry.Circle( walk_line_segment.end, self.WALKER_COMFORT_ZONE )
start_points = geometry.intersect_line_and_circle( start_circle,
walk_line_segment.perpendicular( walk_line_segment.start ) )
end_points = geometry.intersect_line_and_circle( end_circle,
walk_line_segment.perpendicular( walk_line_segment.end ) )
if not walk_line_segment.are_on_the_same_side( start_points[0], end_points[0] ):
end_points[0], end_points[1] = end_points[1], end_points[0]
trajectory_bounding_rect_segments = [
geometry.LineSegment( start_points[0], start_points[1] ),
geometry.LineSegment( start_points[1], end_points[1] ),
geometry.LineSegment( end_points[1], end_points[0] ),
geometry.LineSegment( end_points[0], start_points[0] )
]
potentially_interfering_walls = []
for bounding_box in get_line_segment_bounding_boxes( walk_line_segment ):
potentially_interfering_walls += self.quad_tree.get( bounding_box.expand( self.WALKER_COMFORT_ZONE + 1 ) )
for wall_line_segment in potentially_interfering_walls:
if ( does_line_segment_interfere_with_circle( wall_line_segment, start_circle ) or
does_line_segment_interfere_with_circle( wall_line_segment, end_circle ) ):
return False
if does_line_segment_interfere_with_rect( wall_line_segment, trajectory_bounding_rect_segments ):
return False
return True
def test_line_segment_interference_with_rect(self):
point1 = geometry.Point( 2, 1 )
point2 = geometry.Point( 1, 3 )
point3 = geometry.Point( 9, 7 )
point4 = geometry.Point( 10, 5 )
rect_segments = [
geometry.LineSegment( point1, point2 ),
geometry.LineSegment( point2, point3 ),
geometry.LineSegment( point3, point4 ),
geometry.LineSegment( point4, point1 )
]
self.assertTrue( does_line_segment_interfere_with_rect( geometry.LineSegment( geometry.Point( 5, 0 ),
geometry.Point( 5, 10 ) ),
rect_segments ) )
self.assertTrue( does_line_segment_interfere_with_rect( geometry.LineSegment( geometry.Point( 5, 4 ),
geometry.Point( 5, 10 ) ),
rect_segments ) )
self.assertTrue( does_line_segment_interfere_with_rect( geometry.LineSegment( geometry.Point( 5, 4 ),
geometry.Point( 5, 3 ) ),
rect_segments ) )
self.assertTrue( does_line_segment_interfere_with_rect( geometry.LineSegment( geometry.Point( 2, 2 ),
geometry.Point( 4, 3 ) ),
rect_segments ) )
self.assertFalse( does_line_segment_interfere_with_rect( geometry.LineSegment( geometry.Point( 3, 10 ),
geometry.Point( 3, 20 ) ),
rect_segments ) )
def generate_edges(self):
for i in range( len( self.vertices ) ):
for j in range( i+1, len( self.vertices ) ):
new_edge = geometry.LineSegment( self.vertices[ i ], self.vertices[ j ] )
if self.is_line_walkable( new_edge ):
self.edges.append( (i, j) )
def test_line_segment_interference_with_cirlce(self):
circle = geometry.Circle( geometry.Point( 0, 0 ), 10 )
self.assertTrue( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -20, 4 ),
geometry.Point( 20, 1 ) ),
circle ) )
self.assertTrue( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -5, 4 ),
geometry.Point( 20, 1 ) ),
circle ) )
self.assertTrue( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -5, 4 ),
geometry.Point( 4, 1 ) ),
circle ) )
self.assertFalse( does_line_segment_interfere_with_circle( geometry.LineSegment( geometry.Point( -50, 11 ),
geometry.Point( 50, 11 ) ),
circle ) )
def rebuild_quad_tree(self):
self.quad_tree = QuadTree( geometry.BoundingBox( 0, self.SCREEN_SIZE[0], 0, self.SCREEN_SIZE[1] ), max_elems=4 )
for wall in self.walls:
for wall_segment in pairwise( wall ):
self.quad_tree.add( geometry.LineSegment( wall_segment[0], wall_segment[1] ) )
self.quad_tree_rects = []
def get_quadrants_rects( quadrant ):
if quadrant.subquadrants is not None:
for subquadrant in quadrant.subquadrants:
get_quadrants_rects( subquadrant )
else:
self.quad_tree_rects.append( pygame.Rect( quadrant.bounding_box.x_lower, quadrant.bounding_box.y_lower,
quadrant.bounding_box.x_upper - quadrant.bounding_box.x_lower + 1,
quadrant.bounding_box.y_upper - quadrant.bounding_box.y_lower + 1 ) )
get_quadrants_rects( self.quad_tree.main_quadrant )