class FibPQ(PriorityQueue): def __init__(self): self.heap = FibHeap() def __len__(self): return self.heap.count def insert(self, node): self.heap.insert(node) def minimum(self): return self.heap.minimum() def removeminimum(self): return self.heap.removeminimum() def decreasekey(self, node, new_priority): self.heap.decreasekey(node, new_priority)
def solve(maze): # Width is used for indexing, total is used for array sizes width = maze.width total = maze.width * maze.height # Start node, end node start = maze.start startpos = start.Position end = maze.end endpos = end.Position # Visited holds true/false on whether a node has been seen already. Used to stop us returning to nodes multiple times visited = [False] * total # Previous holds a link to the previous node in the path. Used at the end for reconstructing the route prev = [None] * total # Distances holds the distance to any node taking the best known path so far. Better paths replace worse ones as we find them. # We start with all distances at infinity infinity = float("inf") distances = [infinity] * total # The priority queue. We are using a Fibonacci heap in this case. unvisited = FibHeap() # This index holds all priority queue nodes as they are created. We use this to decrease the key of a specific node when a shorter path is found. # This isn't hugely memory efficient, but likely to be faster than a dictionary or similar. nodeindex = [None] * total # To begin, we set the distance to the start to zero (we're there) and add it into the unvisited queue distances[start.Position[0] * width + start.Position[1]] = 0 startnode = FibHeap.Node(0, start) nodeindex[start.Position[0] * width + start.Position[1]] = startnode unvisited.insert(startnode) # Zero nodes visited, and not completed yet. count = 0 completed = False # Begin Dijkstra - continue while there are unvisited nodes in the queue while unvisited.count > 0: count += 1 # Find current shortest path point to explore n = unvisited.minimum() unvisited.removeminimum() # Current node u, all neighbours will be v u = n.value upos = u.Position uposindex = upos[0] * width + upos[1] if distances[uposindex] == infinity: break # If upos == endpos, we're done! if upos == endpos: completed = True break for v in u.Neighbours: if v != None: vpos = v.Position vposindex = vpos[0] * width + vpos[1] if visited[vposindex] == False: # The extra distance from where we are (upos) to the neighbour (vpos) - this is manhattan distance # https://en.wikipedia.org/wiki/Taxicab_geometry d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1]) # New path cost to v is distance to u + extra newdistance = distances[uposindex] + d # If this new distance is the new shortest path to v if newdistance < distances[vposindex]: vnode = nodeindex[vposindex] # v isn't already in the priority queue - add it if vnode == None: vnode = FibHeap.Node(newdistance, v) unvisited.insert(vnode) nodeindex[vposindex] = vnode distances[vposindex] = newdistance prev[vposindex] = u # v is already in the queue - decrease its key to re-prioritise it else: unvisited.decreasekey(vnode, newdistance) distances[vposindex] = newdistance prev[vposindex] = u visited[uposindex] = True # We want to reconstruct the path. We start at end, and then go prev[end] and follow all the prev[] links until we're back at the start from collections import deque path = deque() current = end while (current != None): path.appendleft(current) current = prev[current.Position[0] * width + current.Position[1]] return [path, [count, len(path), completed]]
def solve(maze): width = maze.width total = maze.width * maze.height start = maze.start startpos = start.Position end = maze.end endpos = end.Position visited = [False] * total prev = [None] * total infinity = float("inf") distances = [infinity] * total unvisited = FibHeap() nodeindex = [None] * total distances[start.Position[0] * width + start.Position[1]] = 0 startnode = FibHeap.Node(0, start) nodeindex[start.Position[0] * width + start.Position[1]] = startnode unvisited.insert(startnode) count = 0 completed = False while unvisited.count > 0: count += 1 n = unvisited.minimum() unvisited.removeminimum() u = n.value upos = u.Position uposindex = upos[0] * width + upos[1] if distances[uposindex] == infinity: break if upos == endpos: completed = True break for v in u.Neighbours: if v != None: vpos = v.Position vposindex = vpos[0] * width + vpos[1] if visited[vposindex] == False: d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1]) # New path cost to v is distance to u + extra. Some descriptions of A* call this the g cost. # New distance is the distance of the path from the start, through U, to V. newdistance = distances[uposindex] + d # A* includes a remaining cost, the f cost. In this case we use manhattan distance to calculate the distance from # V to the end. We use manhattan again because A* works well when the g cost and f cost are balanced. # https://en.wikipedia.org/wiki/Taxicab_geometry remaining = abs(vpos[0] - endpos[0]) + abs(vpos[1] - endpos[1]) # Notice that we don't include f cost in this first check. We want to know that the path *to* our node V is shortest if newdistance < distances[vposindex]: vnode = nodeindex[vposindex] if vnode == None: # V goes into the priority queue with a cost of g + f. So if it's moving closer to the end, it'll get higher # priority than some other nodes. The order we visit nodes is a trade-off between a short path, and moving # closer to the goal. vnode = FibHeap.Node(newdistance + remaining, v) unvisited.insert(vnode) nodeindex[vposindex] = vnode # The distance *to* the node remains just g, no f included. distances[vposindex] = newdistance prev[vposindex] = u else: # As above, we decrease the node since we've found a new path. But we include the f cost, the distance remaining. unvisited.decreasekey(vnode, newdistance + remaining) # The distance *to* the node remains just g, no f included. distances[vposindex] = newdistance prev[vposindex] = u visited[uposindex] = True from collections import deque path = deque() current = end while (current != None): path.appendleft(current) current = prev[current.Position[0] * width + current.Position[1]] return [path, [count, len(path), completed]]