Waypoint constrained guidance for entry vehicles