dijkstra.py

from FibonacciHeap import FibHeap

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]]