Fixed one of the path finding bugs. Cleaned PathFinder.cpp.