new waypoint calculation code