Update Pathfinding.cpp