Cleaned up the pathfinding methods and tests.