astar.py

from FibonacciHeap import FibHeap;

# This implementatoin of A* is almost identical to the Dijkstra implementation. So for clarity I've removed all comments, and only added those
# Specifically showing the difference between dijkstra and A*

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