Update pathfinding.cpp

Small changes, removed useless shit