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 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_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 _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
