Fixed A* heuristic calculation bug