public void Update(Pose pose, Seconds elapsed)
{
if (this.accumulator == 0)
{
this.Initiate(pose);
}
this.accumulator += elapsed;
var progress = this.accumulator / this.Duration;
pose.Rotate(
AngleMath.LerpRadians(this.startYaw, this.TargetYaw, progress),
AngleMath.LerpRadians(this.startPitch, this.TargetPitch, progress),
pose.Roll);
pose.Move(Vector3.Lerp(this.startPosition, this.TargetPosition, progress));
if (this.accumulator >= this.Duration)
{
this.Completed = true;
}
}
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);
}
}