def shortest_path(self, origin, destination): """Find and return shortest path between origin and destination. Will return in-order list of Points of the shortest path found. If origin or destination are not in the visibility graph, their respective visibility edges will be found, but only kept temporarily for finding the shortest path. """ origin_exists = origin in self.visgraph dest_exists = destination in self.visgraph if origin_exists and dest_exists: return shortest_path(self.visgraph, origin, destination) orgn = None if origin_exists else origin dest = None if dest_exists else destination add_to_visg = Graph([]) if not origin_exists: for v in visible_vertices(origin, self.graph, destination=dest, con=True): add_to_visg.add_edge(Edge(origin, v)) if not dest_exists: for v in visible_vertices(destination, self.graph, origin=orgn, con=True): add_to_visg.add_edge(Edge(destination, v)) return shortest_path(self.visgraph, origin, destination, add_to_visg)
def shortest_path_sequential(self, agents): """ Sequentially runs A* on each agent, with collision constraint. """ paths = [] occupied = [] for (origin, destination) in agents: origin_exists = origin in self.visgraph dest_exists = destination in self.visgraph if origin_exists and dest_exists: paths.append( shortest_path_single(self.visgraph, origin, destination, occupied)) continue orgn = None if origin_exists else origin dest = None if dest_exists else destination add_to_visg = Graph([]) if not origin_exists: for v in visible_vertices(origin, self.graph, destination=dest): add_to_visg.add_edge(Edge(origin, v)) if not dest_exists: for v in visible_vertices(destination, self.graph, origin=orgn): add_to_visg.add_edge(Edge(destination, v)) paths.append( shortest_path_single(self.visgraph, origin, destination, add_to_visg, occupied)) return paths
def build_graph(graph, origin, destination): origin_exists = origin in graph.visgraph dest_exists = destination in graph.visgraph if origin_exists and dest_exists: return graph.visgraph orgn = None if origin_exists else origin dest = None if dest_exists else destination if not origin_exists: for v in visible_vertices(origin, graph.graph, destination = dest): graph.visgraph.add_edge(Edge(origin, v)) if not dest_exists: for v in visible_vertices(destination, graph.graph, origin = orgn): graph.visgraph.add_edge(Edge(destination, v)) return graph.visgraph
def test_collin4(self): graph = Graph([[Point(1, 1), Point(2, 3), Point(3, 1), Point(2, 2)], [Point(2, 4)]]) visible = visible_vertices(Point(2, 1), graph, None, None) assert visible == [Point(3, 1), Point(2, 2), Point(1, 1)]
def test_collin1(self): graph = Graph([[self.point_a, self.point_b, self.point_c], [self.point_d, self.point_e, self.point_f]]) visible = visible_vertices(Point(1, 4), graph, None, None) assert visible == [ self.point_a, self.point_c, self.point_d, self.point_f ]
def update(self, points, origin=None, destination=None): """Update visgraph by checking visibility of Points in list points.""" for p in points: for v in visible_vertices(p, self.graph, origin=origin, destination=destination): self.visgraph.add_edge(Edge(p, v))
def test_collin2(self): self.point_g = Point(2, 5) self.point_h = Point(3, 5) graph = Graph([[self.point_g, self.point_h, self.point_c], [self.point_d, self.point_e, self.point_f]]) visible = visible_vertices(Point(1, 4), graph, None, None) assert visible == [self.point_g, self.point_e, self.point_c]
def test_collin2(self): self.point_g = Point(2.0, 5.0) self.point_h = Point(3.0, 5.0) graph = Graph([[self.point_g, self.point_h, self.point_c], [self.point_d, self.point_e, self.point_f]]) visible = visible_vertices(Point(1,4), graph, None, None) assert visible == [self.point_g, self.point_e, self.point_c, self.point_d]
def test_collin3(self): point_g = Point(2.0, 2.0) point_h = Point(3.5, 5.0) point_i = Point(2.5, 2.0) graph = Graph([[self.point_a, self.point_b, self.point_c], [point_g, point_h, point_i], [self.point_d, self.point_e, self.point_f]]) visible = visible_vertices(Point(1,4), graph, None, None) assert visible == [point_h, self.point_a, self.point_c]
def test_collin3(self): point_g = Point(2.0, 2.0) point_h = Point(3.5, 5.0) point_i = Point(2.5, 2.0) graph = Graph([[self.point_a, self.point_b, self.point_c], [point_g, point_h, point_i], [self.point_d, self.point_e, self.point_f]]) visible = visible_vertices(Point(1, 4), graph, None, None) assert visible == [point_h, self.point_a, self.point_c]
def construct_path(self): origin_exists = self.origin in self.g.visgraph dest_exists = self.destination in self.g.visgraph if origin_exists and dest_exists: return self.shortest_path(self.g.visgraph, self.origin, self.destination) orgn = None if origin_exists else self.origin dest = None if dest_exists else self.destination add_to_visg = Graph([]) if not origin_exists: for v in visible_vertices(self.origin, self.g.graph, destination=dest): add_to_visg.add_edge(Edge(self.origin, v)) if not dest_exists: for v in visible_vertices(self.destination, self.g.graph, origin=orgn): add_to_visg.add_edge(Edge(self.destination, v)) return self.shortest_path(self.g.visgraph, self.origin, self.destination, add_to_visg)
def shortest_path_parallel(self, agents, h=lambda x, y: 0): """ Runs A* on all agents in parallel, with collision constraint. """ add_to_visg = Graph([]) for (origin, destination) in agents: origin_exists = origin in self.visgraph dest_exists = destination in self.visgraph if origin_exists and dest_exists: continue orgn = None if origin_exists else origin dest = None if dest_exists else destination if not origin_exists: for v in visible_vertices(origin, self.graph, destination=dest): add_to_visg.add_edge(Edge(origin, v)) if not dest_exists: for v in visible_vertices(destination, self.graph, origin=orgn): add_to_visg.add_edge(Edge(destination, v)) return shortest_path_parallel(self.visgraph, agents, h, add_to_visg)
def test_collin3(self): graph = Graph([[Point(0, 1), Point(1, 0), Point(2, 3)], [Point(2, 2), Point(3.5, 5), Point(2.5, 2)], [Point(3, 2), Point(3.5, 0.5), Point(4.5, 3.5)]]) visible = visible_vertices(Point(1, 4), graph, None, None) assert visible == [ Point(3.50, 5.00), Point(0.00, 1.00), Point(2.00, 3.00) ]
def shortest_path(self, origin, destination): """Find and return shortest path between origin and destination. Will return in-order list of Points of the shortest path found. If origin or destination are not in the visibility graph, their respective visibility edges will be found, but only kept temporarily for finding the shortest path. """ origin_exists = origin in self.visgraph dest_exists = destination in self.visgraph if origin_exists and dest_exists: return shortest_path(self.visgraph, origin, destination) orgn = None if origin_exists else origin dest = None if dest_exists else destination add_to_visg = Graph([]) if not origin_exists: for v in visible_vertices(origin, self.graph, destination=dest): add_to_visg.add_edge(Edge(origin, v)) if not dest_exists: for v in visible_vertices(destination, self.graph, origin=orgn): add_to_visg.add_edge(Edge(destination, v)) return shortest_path(self.visgraph, origin, destination, add_to_visg)
def _vis_graph(graph, points, worker, status): total_points = len(points) visible_edges = [] if status: t0 = default_timer() points_done = 0 for p1 in points: for p2 in visible_vertices(p1, graph, scan='half'): visible_edges.append(Edge(p1, p2)) if status: points_done += 1 avg_time = round((default_timer() - t0) / points_done, 3) time_stat = (points_done, total_points-points_done, avg_time) status = '\r\033[' + str(21*worker) + 'C[{:4}][{:4}][{:5.3f}] \r' stdout.write(status.format(*time_stat)) stdout.flush() return visible_edges
def test_collin5(self): r = 0.2 # Radius of polygon n = 4 # Sides of polygon c = Point(1.0, 1.0) # Center of polygon verts = [] for i in range(n): verts.append( Point(r * cos(2 * pi * i / n - pi / 4) + c.x, r * sin(2 * pi * i / n - pi / 4) + c.y)) g = vg.VisGraph() g.build([verts]) s = Point(0, 0) t = Point(1.7, 1.7) shortest = g.shortest_path(s, t) visible = visible_vertices(t, g.graph, s, None) assert verts[3] not in visible assert verts[1] not in shortest assert verts[3] not in shortest
nWithin = len(pts) - 1 if nWithin >= threshold: # Centroid of all points & circle center g = np.mean(np.array(pts), axis=0) # Above often lands in polygons, # so now take the centroid between g and circle center g = np.mean(np.array([c, g]), axis=0) generated.append(g) # Switch order (x, y) --> (y, x) to match visibility graph evg = [vg.Point(g[1], g[0]) for g in generated] # Add points to visgraph added = [] for g in evg: try: for v in visible_vertices(g, graph.graph): if point_in_polygon(g, graph.graph) < 0 and point_in_polygon( v, graph.graph) < 0: graph.visgraph.add_edge(Edge(g, v)) except: continue print("End extending visibility graph") # Save evg graph.save(graphFileOut) # Plot points ax = plt.subplot(111) plt.scatter([c[0] for c in generated], [c[1] for c in generated], c='blue', alpha=1.0)
def test_collin4(self): graph = Graph([[Point(1,1), Point(2,3), Point(3,1),Point(2,2)], [Point(2,4)]]) visible = visible_vertices(Point(2,1), graph, None, None) assert visible == [Point(3,1), Point(2,2), Point(1,1)]
# Add the start and stop point to grid origin = vg.Point(round((round(startPoint[1], 10) - minx) / (maxx - minx) * rangeWidth, 6), round((round(startPoint[0], 10) - miny) / (maxy - miny) * rangeWidth, 6)) destination = vg.Point(round((round(endPoint[1], 10) - minx) / (maxx - minx) * rangeWidth, 6), round((round(endPoint[0], 10) - miny) / (maxy - miny) * rangeWidth, 6)) print("start:", startPoint, "scaled:", origin) print("destination", endPoint, "scaled:", destination) origin_exists = origin in graph.visgraph dest_exists = destination in graph.visgraph orgn = None if origin_exists else origin dest = None if dest_exists else destination if not origin_exists: for v in visible_vertices(origin, graph.graph, destination=dest): graph.visgraph.add_edge(Edge(origin, v)) if not dest_exists: for v in visible_vertices(destination, graph.graph, origin=orgn): graph.visgraph.add_edge(Edge(destination, v)) # Convert to simple dictionary graph dgraph = {} for v in graph.visgraph.get_points(): v_latlon = ( (v.y / rangeWidth) * (maxy - miny) + miny, (v.x / rangeWidth) * (maxx - minx) + minx ) v_rowcol = world2grid(v_latlon[0], v_latlon[1], regionTransform, regionExtent["rows"]) v_edges = [] for edge in graph.visgraph[v]: w = edge.get_adjacent(v) w_latlon = ( (w.y / rangeWidth) * (maxy - miny) + miny, (w.x / rangeWidth) * (maxx - minx) + minx )
def find_visible(self, point): """Find vertices visible from point.""" return visible_vertices(point, self.graph)
def test_collin1(self): graph = Graph([[self.point_a, self.point_b, self.point_c], [self.point_d, self.point_e, self.point_f]]) visible = visible_vertices(Point(1,4), graph, None, None) assert visible == [self.point_a, self.point_c, self.point_d, self.point_f]
def _vis_graph(graph, points): visible_edges = [] for p1 in points: for p2 in visible_vertices(p1, graph, scan='half'): visible_edges.append(Edge(p1, p2)) return visible_edges