def findPath(self, startID, endID):
Queue = PriorityQueue()
G = self.asAdjacencyList()
visited = set()
parent = {}
cost_array = np.zeros(len(self.nodes))
Queue.add_update(startID, 0)
found = False
while (not Queue.empty()):
node = Queue.pop()
if (node == endID):
found = True
break
visited.add(node)
for nextNode in G[node]:
if (nextNode in visited):
continue
newcost = cost_array[node] + np.linalg.norm(
self.nodes[node] - self.nodes[nextNode])
if (nextNode in Queue and newcost >= cost_array[nextNode]):
continue
parent[nextNode] = node
cost_array[nextNode] = newcost
f = newcost + np.linalg.norm(self.nodes[endID] -
self.nodes[nextNode])
Queue.add_update(nextNode, f)
if (not found):
return []
path = []
node = endID
while (node != startID):
path.append(node)
node = parent[node]
path.append(node)
path.reverse()
return path
