Update Pathfinder.cpp