Update pathfinder.cpp