Fix several bugs in waypoint computations