Testing distance functions. Fixed bug.

Bug in the heuristic distance. Astar should run much faster now.
Note: This heuristic distance is not admissible.