public static void PlanMoveTo(Queue <IManeuver> maneuvers, Vector3 position, float yaw, float pitch, Vector3 target, float maxLinearAcceleration, float maxAngularAcceleration)
{
// 1. Point in the right direction
// 2. Accelerate by performing a prograde burn
// 3. Rotate so that the rocket points in the opposite direction of the velocity vector
// 4. Decelerate by performing a retrograde burn
// 5. Make tiny adjustments to correct floating point math errors
PlanPointAt(maneuvers, position, yaw, pitch, target, maxAngularAcceleration);
var distance = Vector3.Distance(position, target);
if (distance > MinMoveDistance)
{
var direction = Vector3.Normalize(target - position);
var targetYaw = AngleMath.YawFromVector(-direction);
var yawDistance = Math.PI;
var yawRotationDuration = ComputeRotationDuration(yawDistance, maxAngularAcceleration);
var currentPitch = AngleMath.PitchFromVector(direction);
var targetPitch = AngleMath.PitchFromVector(-direction);
var pitchDistance = AngleMath.DistanceRadians(currentPitch, targetPitch);
var pitchRotationDuration = ComputeRotationDuration(pitchDistance, maxAngularAcceleration);
var burnDuration = ComputeBurnDuration(distance, maxLinearAcceleration, yawRotationDuration);
var velocityAfterBurn = maxLinearAcceleration * burnDuration * direction;
var progradeBurn = new BurnManeuver(direction, Vector3.Zero, maxLinearAcceleration, burnDuration);
var rotation = new RotationManeuver(velocityAfterBurn, Math.Sign(yawDistance), yawRotationDuration, Math.Sign(pitchDistance), pitchRotationDuration, maxAngularAcceleration);
var retrogradeBurn = new BurnManeuver(-direction, velocityAfterBurn, maxLinearAcceleration, burnDuration);
var adjust = new LerpManeuver(target, targetYaw, targetPitch, new Seconds(1.0f));
maneuvers.Enqueue(progradeBurn);
maneuvers.Enqueue(rotation);
maneuvers.Enqueue(retrogradeBurn);
maneuvers.Enqueue(adjust);
}
}
public static void PlanPointAt(Queue <IManeuver> maneuvers, Vector3 position, float yaw, float pitch, Vector3 target, float maxAngularAcceleration)
{
if (Vector3.DistanceSquared(target, position) > 0)
{
var targetDirection = Vector3.Normalize(target - position);
var targetYaw = AngleMath.YawFromVector(targetDirection);
var yawDistance = AngleMath.DistanceRadians(yaw, targetYaw);
var targetPitch = AngleMath.PitchFromVector(targetDirection);
var pitchDistance = AngleMath.DistanceRadians(pitch, targetPitch);
if (Math.Abs(yawDistance) > MinRotateDistance || Math.Abs(pitchDistance) > MinRotateDistance)
{
var yawRotationDuration = ComputeRotationDuration(yawDistance, maxAngularAcceleration);
var pitchRotationDuration = ComputeRotationDuration(pitchDistance, maxAngularAcceleration);
var rotation = new RotationManeuver(Vector3.Zero, Math.Sign(yawDistance), yawRotationDuration, Math.Sign(pitchDistance), pitchRotationDuration, maxAngularAcceleration);
maneuvers.Enqueue(rotation);
}
var adjust = new LerpManeuver(position, targetYaw, targetPitch, new Seconds(0.2f));
maneuvers.Enqueue(adjust);
}
}