def computeDistances(self, src_node, dest_node_list): # uses Dijkstra
g = self._graph
dest_node_ids = set([a.getLeafId() if a is not None else -1 for a in dest_nodes ])
if src_node is None or not any(dest_nodes):
return [None]*len(dest_list)
search_front_queue = SortedList()
search_front_queue.add((0,src_node.getLeafId()))
search_front_p = {}
target_distances = {}
frozen = set()
loops = 0
while search_front_queue:
loops += 1
d,a = search_front_queue.pop(0)
if a in frozen: # ignore duplicates
continue
# target found
if a == dest_node_ids:
target_distances[a] = d
dest_node_ids.remove(a)
if len(dest_node_ids) == 0:
break
frozen.add(a)
node = g.getNode(a)
for adj in node.getAdjacencies().keys():
if adj in frozen: # don't try to even check nodes that have been already frozen
continue
new_d = d + node.distanceTo(g.getNode(adj))
# route to adj through a is longer than what we already had. Dismiss
if adj in search_front_p and new_d > search_front_p[adj]:
continue
search_front_p[adj] = new_d
# this might add duplicate adj items (ideally we would delete any previous insertion)
# because we can't easily erase from the queue.
search_front_queue.add((new_d, adj))
distances = []
for t in dest_node_ids:
try:
distances.append(target_distances[t])
except:
distances.append(None)
return distances
def computeRoute(self, src_node, dest_node): # uses A*
graph = self._graph
src_node_id = src_node.getLeafId()
dest_node_id = dest_node.getLeafId()
frontier = SortedList()
frontier.add((0,src_node_id,src_node))
cost_so_far = {src_node_id:0}
came_from = {}
#loops = 0
while frontier:
#loops += 1
g,node_id,node = frontier.pop(0)
if node == dest_node:
break
for adj_id,portal in node.getAdjacencies().iteritems():
adj = graph.getNode(adj_id)
new_cost = cost_so_far[node_id] + node.distanceTo(adj)
if adj_id not in cost_so_far or new_cost < cost_so_far[adj_id]:
cost_so_far[adj_id] = new_cost
h = node.distanceTo(dest_node) # the A* heuristic
priority = new_cost + h
frontier.add((priority,adj_id,adj))
came_from[adj_id] = (node_id,portal)
p = dest_node_id
route = [(p,None)] # stores tuples: node_number, exit portal
while p in came_from:
prev,portal = came_from[p]
route.append((prev, portal))
p = prev
route.reverse()
return route
