Commit of initial C++ pathfinding implementation