Added PathFinder.cpp to the actual project and did a little work on it