def build(self, input, workers=1, status=True):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status=False to turn off the statusbar when building.
"""
self.graph = Graph(input)
self.visgraph = Graph([])
points = self.graph.get_points()
batch_size = 10
if workers == 1:
for batch in tqdm([points[i:i + batch_size]
for i in xrange(0, len(points), batch_size)],
disable=not status):
for edge in _vis_graph(self.graph, batch):
self.visgraph.add_edge(edge)
else:
pool = Pool(workers)
batches = [(self.graph, points[i:i + batch_size])
for i in xrange(0, len(points), batch_size)]
results = list(tqdm(pool.imap(_vis_graph_wrapper, batches), total=len(batches),
disable=not status))
for result in results:
for edge in result:
self.visgraph.add_edge(edge)
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 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_collin8(self):
graph = Graph(
[[Point(0, 0),
Point(2, 0),
Point(2, 2),
Point(1, 1),
Point(0, 2)]])
pip = point_in_polygon(Point(0.5, 1), graph)
assert pip > -1
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 build(self, input, workers=1, status=False):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status to True to see progress information for each subprocess:
[Points done][Points remaining][average time per Point].
"""
self.graph = Graph(input)
self.visgraph = Graph([])
if status: print(" " + "[Done][Rem.][Avg t] " * workers)
if workers == 1:
#print(type(ed))
for edge in _vis_graph(self.graph, self.graph.get_points(), 0, status):
self.visgraph.add_edge(edge)
if status: print("")
ed = _vis_graph(self.graph, self.graph.get_points(), 0, status)
return ed
points = self.graph.get_points()
batch_size = int(len(points) / workers)
batches = [(self.graph, points[i:i + batch_size], i/batch_size, status)
for i in xrange(0, len(points), batch_size)]
pool = Pool(workers)
results = pool.map_async(_vis_graph_wrapper, batches)
try:
for result in results.get():
for edge in result:
self.visgraph.add_edge(edge)
except KeyboardInterrupt:
if status: print("")
raise
if status: print("")
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 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 build(self, input, workers=1, status=False):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status to True to see progress information for each subprocess:
[Points done][Points remaining][average time per Point].
"""
self.graph = Graph(input)
self.visgraph = Graph([])
if status: print(" " + "[Done][Rem.][Avg t] " * workers)
if workers == 1:
for edge in _vis_graph(self.graph, self.graph.get_points(), 0, status):
self.visgraph.add_edge(edge)
if status: print("")
return None
points = self.graph.get_points()
batch_size = int(len(points) / workers)
batches = [(self.graph, points[i:i + batch_size], i/batch_size, status)
for i in xrange(0, len(points), batch_size)]
pool = Pool(workers)
results = pool.map_async(_vis_graph_wrapper, batches)
try:
for result in results.get():
for edge in result:
self.visgraph.add_edge(edge)
except KeyboardInterrupt:
if status: print("")
raise
if status: print("")
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 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 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)
class VisGraph(object):
def __init__(self):
self.graph = None
self.visgraph = None
def load(self, filename):
"""Load obstacle graph and visibility graph. """
with open(filename, 'rb') as load:
self.graph, self.visgraph = pickle.load(load)
def save(self, filename):
"""Save obstacle graph and visibility graph. """
with open(filename, 'wb') as output:
pickle.dump((self.graph, self.visgraph), output, -1)
def build(self, input, workers=1, status=True):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status=False to turn off the statusbar when building.
"""
self.graph = Graph(input)
self.visgraph = Graph([])
points = self.graph.get_points()
batch_size = 10
if workers == 1:
for batch in tqdm([
points[i:i + batch_size]
for i in xrange(0, len(points), batch_size)
],
disable=not status):
for edge in _vis_graph(self.graph, batch):
self.visgraph.add_edge(edge)
else:
pool = Pool(workers)
batches = [(self.graph, points[i:i + batch_size])
for i in xrange(0, len(points), batch_size)]
results = list(
tqdm(pool.imap(_vis_graph_wrapper, batches),
total=len(batches),
disable=not status))
for result in results:
for edge in result:
self.visgraph.add_edge(edge)
def find_visible(self, point):
"""Find vertices visible from point."""
return visible_vertices(point, self.graph)
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 shortest_path_single(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_single(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_single(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 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 shortest_path_multi(self, origins, destinations):
# """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.
# """
# paths = dict()
# zipped = dict(zip(destinations, origins))
# counter = 0
# add_to_visg = Graph([])
#
# #check that all origins and destinations exist, if not, add them to the graph
# for (destination, origin) in zipped.items():
#
# origin_exists = origin in self.visgraph
# dest_exists = destination in self.visgraph
# if origin_exists and dest_exists:
# paths[destination] = [origin]
# 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))
#
# paths[destination] = [origin]
#
# flag = 0
# start = True
# g_dict = {}
# f_dict = {}
# while flag < 5:
# occupied = set()
# counter = 0
# for (destination, origin) in zipped.items():
# if destination == origin:
# counter += 1
# continue
#
# P, g_score, frontier = shortest_path_multi(self.visgraph, origin, destination, occupied,
# g_dict.get(destination, {}), f_dict.get(destination, heapdict()), add_to_visg, start)
# g_dict[destination] = g_score
# f_dict[destination] = frontier
#
# if P != None:
# occupied.add(P)
# zipped[destination] = P
# (paths[destination]).append(P)
#
# start = False
# if counter >= len(zipped):
# break
# flag +=1
#
# res = []
# for i in range(len(destinations)):
# res.append(paths[destinations[i]])
# return res
################################################################################################
def point_in_polygon(self, point):
"""Return polygon_id if point in a polygon, -1 otherwise."""
return point_in_polygon(point, self.graph)
def closest_point(self, point, polygon_id, length=0.001):
"""Return closest Point outside polygon from point.
Note method assumes point is inside the polygon, no check is
performed.
"""
return closest_point(point, self.graph, polygon_id, length)
class VisGraph(object):
def __init__(self):
self.graph = None
self.visgraph = None
def load(self, filename):
"""Load obstacle graph and visibility graph. """
with open(filename, 'rb') as load:
self.graph, self.visgraph = pickle.load(load)
def save(self, filename):
"""Save obstacle graph and visibility graph. """
with open(filename, 'wb') as output:
pickle.dump((self.graph, self.visgraph), output, -1)
def build(self, input, workers=1, status=False):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status to True to see progress information for each subprocess:
[Points done][Points remaining][average time per Point].
"""
self.graph = Graph(input)
self.visgraph = Graph([])
if status: print(" " + "[Done][Rem.][Avg t] " * workers)
if workers == 1:
for edge in _vis_graph(self.graph, self.graph.get_points(), 0, status):
self.visgraph.add_edge(edge)
if status: print("")
return None
points = self.graph.get_points()
batch_size = int(len(points) / workers)
batches = [(self.graph, points[i:i + batch_size], i/batch_size, status)
for i in xrange(0, len(points), batch_size)]
pool = Pool(workers)
results = pool.map_async(_vis_graph_wrapper, batches)
try:
for result in results.get():
for edge in result:
self.visgraph.add_edge(edge)
except KeyboardInterrupt:
if status: print("")
raise
if status: print("")
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 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 point_in_polygon(self, point):
"""Return polygon_id if point in a polygon, -1 otherwise."""
return point_in_polygon(point, self.graph)
def closest_point(self, point, polygon_id, length=0.001):
"""Return closest Point outside polygon from point.
Note method assumes point is inside the polygon, no check is
performed.
"""
return closest_point(point, self.graph, polygon_id, length)
class VisGraph(object):
def __init__(self, precision=2):
self.precision = precision
self.graph = None
self.visgraph = None
def load(self, filename):
"""Load obstacle graph and visibility graph. """
with open(filename, 'rb') as load:
self.graph, self.visgraph = pickle.load(load)
def save(self, filename):
"""Save obstacle graph and visibility graph. """
with open(filename, 'wb') as output:
pickle.dump((self.graph, self.visgraph), output, -1)
def round_point(self, point):
return Point(round(point.x, self.precision), round(point.y, self.precision))
def build(self, input, workers=1, status=True):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status=False to turn off the statusbar when building.
"""
# round points to desired precision
for i, polygon in enumerate(input):
input[i] = [self.round_point(point) for point in input[i]]
self.graph = Graph(input)
self.visgraph = Graph([])
points = self.graph.get_points()
batch_size = 10
if workers == 1:
for batch in tqdm([points[i:i + batch_size]
for i in xrange(0, len(points), batch_size)],
disable=not status):
for edge in _vis_graph(self.graph, batch):
self.visgraph.add_edge(edge)
else:
pool = Pool(workers)
batches = [(self.graph, points[i:i + batch_size])
for i in xrange(0, len(points), batch_size)]
results = list(tqdm(pool.imap(_vis_graph_wrapper, batches), total=len(batches),
disable=not status))
for result in results:
for edge in result:
self.visgraph.add_edge(edge)
def find_visible(self, point):
"""Find vertices visible from point."""
return visible_vertices(point, self.graph)
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 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 point_in_polygon(self, point):
"""Return polygon_id if point in a polygon, -1 otherwise."""
return point_in_polygon(point, self.graph)
def closest_point(self, point, polygon_id, length=0.001):
"""Return closest Point outside polygon from point.
Note method assumes point is inside the polygon, no check is
performed.
"""
return closest_point(point, self.graph, polygon_id, length)
class VisGraph(object):
def __init__(self):
self.graph = None
self.visgraph = None
def load(self, filename):
"""Load obstacle graph and visibility graph. """
with open(filename, 'rb') as load:
self.graph, self.visgraph = pickle.load(load)
def save(self, filename):
"""Save obstacle graph and visibility graph. """
with open(filename, 'wb') as output:
pickle.dump((self.graph, self.visgraph), output, -1)
def build(self, input, workers=1, status=False):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status to True to see progress information for each subprocess:
[Points done][Points remaining][average time per Point].
"""
self.graph = Graph(input)
self.visgraph = Graph([])
if status: print(" " + "[Done][Rem.][Avg t] " * workers)
if workers == 1:
#print(type(ed))
for edge in _vis_graph(self.graph, self.graph.get_points(), 0, status):
self.visgraph.add_edge(edge)
if status: print("")
ed = _vis_graph(self.graph, self.graph.get_points(), 0, status)
return ed
points = self.graph.get_points()
batch_size = int(len(points) / workers)
batches = [(self.graph, points[i:i + batch_size], i/batch_size, status)
for i in xrange(0, len(points), batch_size)]
pool = Pool(workers)
results = pool.map_async(_vis_graph_wrapper, batches)
try:
for result in results.get():
for edge in result:
self.visgraph.add_edge(edge)
except KeyboardInterrupt:
if status: print("")
raise
if status: print("")
#def get_vis(self,ed):
#return ed
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 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 point_in_polygon(self, point):
"""Return polygon_id if point in a polygon, -1 otherwise."""
return point_in_polygon(point, self.graph)
def closest_point(self, point, polygon_id, length=0.001):
"""Return closest Point outside polygon from point.
Note method assumes point is inside the polygon, no check is
performed.
"""
return closest_point(point, self.graph, polygon_id, length)
def build_mod(self, input, workers=1, status=True, ref_point=None):
"""Build visibility graph based on a list of polygons.
Modified version. Takes into account the case where one polygon surrounds the rest
To test this, a reference-point is given to check the existance of such polygon
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status=False to turn off the statusbar when building.
"""
self.graph = Graph(input)
self.visgraph = Graph([])
# Check if surrounding polygon exists
surPoly = []
if ref_point:
sPoly = pointVec_in_polygon(ref_point, self.graph)
surPoly = sPoly
if sPoly.__len__() == 2: #choose the outer polygon
pp0 = list(self.graph.polygons[sPoly[0]])
pp1 = list(self.graph.polygons[sPoly[1]])
x0=[]
for e in pp0:
x0.append(e.p1.x)
x0.append(e.p2.x)
x1=[]
for e in pp1:
x1.append(e.p1.x)
x1.append(e.p2.x)
if min(x0)<min(x1):
surPoly = sPoly[0]
else: surPoly = sPoly[1]
if surPoly:
surPoly = surPoly[0]
surPolyEdges = list(self.graph.polygons[surPoly])
points = self.graph.get_points()
batch_size = 10
if workers == 1:
for batch in tqdm([points[i:i + batch_size]
for i in xrange(0, len(points), batch_size)],
disable=not status):
for edge in _vis_graph(self.graph, batch):
if surPoly != -1: # skip an edge outside the surrounding polygon
edgePtsInPoly = 0
for n in range(surPolyEdges.__len__()):
if surPolyEdges[n].__contains__(edge.p1) or surPolyEdges[n].__contains__(edge.p2):
edgePtsInPoly += 1
if edgePtsInPoly == 4: # both edge points on surrounding polygon, but not same edge
midP = Point((edge.p1.x + edge.p2.x)/2. , (edge.p1.y + edge.p2.y)/2. )
if not polygon_crossing(midP, self.graph.polygons[surPoly]):
continue
self.visgraph.add_edge(edge)
else:
pool = Pool(workers)
batches = [(self.graph, points[i:i + batch_size])
for i in xrange(0, len(points), batch_size)]
results = list(tqdm(pool.imap(_vis_graph_wrapper, batches), total=len(batches),
disable=not status))
for result in results:
for edge in result:
self.visgraph.add_edge(edge)
class VisGraph(object):
def __init__(self):
self.graph = None
self.visgraph = None
def load(self, filename):
"""Load obstacle graph and visibility graph. """
with open(filename, 'rb') as load:
self.graph, self.visgraph = pickle.load(load)
def save(self, filename):
"""Save obstacle graph and visibility graph. """
with open(filename, 'wb') as output:
pickle.dump((self.graph, self.visgraph), output, -1)
def build(self, input, workers=1, status=True):
"""Build visibility graph based on a list of polygons.
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status=False to turn off the statusbar when building.
"""
self.graph = Graph(input)
self.visgraph = Graph([])
points = self.graph.get_points()
batch_size = 10
if workers == 1:
for batch in tqdm([points[i:i + batch_size]
for i in xrange(0, len(points), batch_size)],
disable=not status):
for edge in _vis_graph(self.graph, batch):
self.visgraph.add_edge(edge)
else:
pool = Pool(workers)
batches = [(self.graph, points[i:i + batch_size])
for i in xrange(0, len(points), batch_size)]
results = list(tqdm(pool.imap(_vis_graph_wrapper, batches), total=len(batches),
disable=not status))
for result in results:
for edge in result:
self.visgraph.add_edge(edge)
def build_mod(self, input, workers=1, status=True, ref_point=None):
"""Build visibility graph based on a list of polygons.
Modified version. Takes into account the case where one polygon surrounds the rest
To test this, a reference-point is given to check the existance of such polygon
The input must be a list of polygons, where each polygon is a list of
in-order (clockwise or counter clockwise) Points. It only one polygon,
it must still be a list in a list, i.e. [[Point(0,0), Point(2,0),
Point(2,1)]].
Take advantage of processors with multiple cores by setting workers to
the number of subprocesses you want. Defaults to 1, i.e. no subprocess
will be started.
Set status=False to turn off the statusbar when building.
"""
self.graph = Graph(input)
self.visgraph = Graph([])
# Check if surrounding polygon exists
surPoly = []
if ref_point:
sPoly = pointVec_in_polygon(ref_point, self.graph)
surPoly = sPoly
if sPoly.__len__() == 2: #choose the outer polygon
pp0 = list(self.graph.polygons[sPoly[0]])
pp1 = list(self.graph.polygons[sPoly[1]])
x0=[]
for e in pp0:
x0.append(e.p1.x)
x0.append(e.p2.x)
x1=[]
for e in pp1:
x1.append(e.p1.x)
x1.append(e.p2.x)
if min(x0)<min(x1):
surPoly = sPoly[0]
else: surPoly = sPoly[1]
if surPoly:
surPoly = surPoly[0]
surPolyEdges = list(self.graph.polygons[surPoly])
points = self.graph.get_points()
batch_size = 10
if workers == 1:
for batch in tqdm([points[i:i + batch_size]
for i in xrange(0, len(points), batch_size)],
disable=not status):
for edge in _vis_graph(self.graph, batch):
if surPoly != -1: # skip an edge outside the surrounding polygon
edgePtsInPoly = 0
for n in range(surPolyEdges.__len__()):
if surPolyEdges[n].__contains__(edge.p1) or surPolyEdges[n].__contains__(edge.p2):
edgePtsInPoly += 1
if edgePtsInPoly == 4: # both edge points on surrounding polygon, but not same edge
midP = Point((edge.p1.x + edge.p2.x)/2. , (edge.p1.y + edge.p2.y)/2. )
if not polygon_crossing(midP, self.graph.polygons[surPoly]):
continue
self.visgraph.add_edge(edge)
else:
pool = Pool(workers)
batches = [(self.graph, points[i:i + batch_size])
for i in xrange(0, len(points), batch_size)]
results = list(tqdm(pool.imap(_vis_graph_wrapper, batches), total=len(batches),
disable=not status))
for result in results:
for edge in result:
self.visgraph.add_edge(edge)
def find_visible(self, point):
"""Find vertices visible from point."""
return visible_vertices(point, self.graph)
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 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 point_in_polygon(self, point):
"""Return polygon_id if point in a polygon, -1 otherwise."""
return point_in_polygon(point, self.graph)
def pointVec_in_polygon(self, point):
"""Return list of polygon_id if point in a polygon(s), -1 otherwise."""
return pointVec_in_polygon(point, self.graph)
def closest_point(self, point, polygon_id, length=0.001):
"""Return closest Point outside polygon from point.
Note method assumes point is inside the polygon, no check is
performed.
"""
return closest_point(point, self.graph, polygon_id, length)
