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