Removed some cppcheck warnings
Simplified pathfinding
pathfinder.cpp: Removed outdated commented-out code