Waypoint distance calc