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 setup_method(self, method):
self.point_a = Point(0.0, 1.0)
self.point_b = Point(1.0, 2.0)
self.point_c = Point(0.0, 1.0)
self.point_d = Point(1.0, 2.0)
self.edge_a = Edge(self.point_a, self.point_b)
self.edge_b = Edge(self.point_b, self.point_a)
self.edge_c = Edge(self.point_c, self.point_d)
self.edge_d = Edge(self.point_d, self.point_c)
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 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_point_edge_distance_function():
point_a = Point(3.0, 1.0)
point_b = Point(3.0, 5.0)
point_c = Point(2.0, 2.0)
point_d = Point(4.0, 4.0)
point_e = Point(1.0, 1.0)
point_f = Point(1.0, 2.0)
point_g = Point(3.0, 4.0)
point_h = Point(2.0, 5.0)
edge = Edge(point_a, point_b)
edge2 = Edge(point_c, point_d)
edge3 = Edge(point_e, point_b)
assert point_edge_distance(point_c, point_d, edge) == 1.4142135623730951
assert point_edge_distance(point_a, point_b, edge2) == 2.0
assert point_edge_distance(point_f, point_g, edge3) == 1.4142135623730951
assert point_edge_distance(point_h, point_g, edge3) == 0.9428090415820635
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 _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_edge_intersect_function():
point_a = Point(3.0, 5.0)
point_b = Point(5.0, 3.0)
point_c = Point(4.0, 2.0)
point_d = Point(4.0, 5.0)
point_e = Point(5.0, 4.0)
point_f = Point(3.0, 4.0)
point_g = Point(4.0, 1.0)
point_h = Point(6.0, 4.0)
point_i = Point(4.0, 4.0)
edge = Edge(point_a, point_b)
assert edge_intersect(point_c, point_d, edge) is True
assert edge_intersect(point_c, point_e, edge) is True
assert edge_intersect(point_f, point_e, edge) is True
assert edge_intersect(point_g, point_b, edge) is True
assert edge_intersect(point_c, point_h, edge) is True
assert edge_intersect(point_h, point_i, edge) is True
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
# 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 )
w_rowcol = world2grid(w_latlon[0], w_latlon[1], regionTransform, regionExtent["rows"])
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)
plt.scatter(gx, gy, c='red')
ax.set_xlim(0, rangeWidth)
ax.set_ylim(0, rangeWidth)
def test_closest_point_length(self):
pid = self.g.point_in_polygon(self.point_d)
cp = self.g.closest_point(self.point_d, pid, length=0.5)
ip = intersect_point(self.point_d, cp, Edge(self.point_a,
self.point_b))
assert edge_distance(ip, cp) == 0.5