fix goal distance estimate in findpath
fix goal distance estimate in findpath

justinhj/astar-algorithm-cpp,gerram/astar-algorithm-cpp,gerram/astar-algorithm-cpp,justinhj/astar-algorithm-cpp,majinjing3/astar-algorithm-cpp,majinjing3/astar-algorithm-cpp