Update PathFinding.cpp