Pathfinding now uses the minimum amount of waypoints