goalNode = (4,1)
frontier = Queue.PriorityQueue()
frontier.put(startNode, 0)
came_from = {}
cost_so_far = {}
came_from[startNode] = None #Python version of "null"
cost_so_far[startNode] = 0
# Construct a map of all possible paths for the startNode across the map
while not frontier.empty():
current = frontier.get() # Get instead of peek, dequeues the item
for neighbour in graph.getNeighbours(current):
new_cost = cost_so_far[current] + graph.getCost(neighbour)
if neighbour not in cost_so_far or new_cost < cost_so_far[neighbour]:
cost_so_far[neighbour] = new_cost
priority = new_cost + graph.heuristic(goalNode, neighbour)
frontier.put(neighbour, priority)
came_from[neighbour] = current
# Create the path between the startNode and goalNode
currentNode = goalNode
path = [currentNode]
while currentNode != startNode:
currentNode = came_from[currentNode]
path.append(currentNode)
