Stupid bug in A* pathfinding and faster heuristic.

Note: the heuristic is still admissible. It is the 3D manhattan distance.