public void UpdateThrustControl(MatrixD orientation, Vector3D linearVelocityWorld)
{
Vector3D velocity = Vector3D.TransformNormal(linearVelocityWorld, MatrixD.Transpose(orientation));
Vector3D destinationRelative = Vector3D.TransformNormal(destination - orientation.Translation, MatrixD.Transpose(orientation));
float mass = navigation.Mass;
Vector3D availableBrakingAcceleration = navigation.MaxThrustInDirection(-velocity) / mass;
Vector3D predictedStopPoint = -0.5 * velocity * velocity / availableBrakingAcceleration;
Vector3D positionError = destinationRelative - predictedStopPoint;
Vector3D desiredVelocity = Vector3D.ClampToSphere(thrustPID.Filter(positionError), SpeedCap);
Vector3D velocityError = velocity - desiredVelocity;
Vector3D desiredThrustPercentage = Vector3D.Clamp(0.5 * velocityError, -Vector3D.One, Vector3D.One);
thrustDebug?.Reset();
thrustDebug?.WriteLine($"x = {DebugPanel.ToString(destinationRelative)}");
thrustDebug?.WriteLine($"v = {DebugPanel.ToString(velocity)}");
thrustDebug?.WriteLine($"as = {DebugPanel.ToString(availableBrakingAcceleration)}");
thrustDebug?.WriteLine($"xs = {DebugPanel.ToString(predictedStopPoint)}");
thrustDebug?.WriteLine($"vt = {DebugPanel.ToString(desiredVelocity)}");
thrustDebug?.WriteLine($"v_err = {DebugPanel.ToString(velocityError)}");
thrustDebug?.WriteLine($"t = {DebugPanel.ToString(desiredThrustPercentage)}");
navigation.SetThrustPercentage(desiredThrustPercentage);
}
