public override void Update(float dt)
{
basis.rotationMatrix = entity.orientationMatrix;
basis.ComputeWorldSpaceAxes();
if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo.
{
Quaternion currentRelativeOrientation;
Matrix worldTransform = Matrix3x3.ToMatrix4X4(basis.WorldTransform);
Quaternion.CreateFromRotationMatrix(ref worldTransform, out currentRelativeOrientation);
//Compute the relative orientation R' between R and the target relative orientation.
Quaternion errorOrientation;
Quaternion.Conjugate(ref currentRelativeOrientation, out errorOrientation);
Quaternion.Multiply(ref settings.servo.goal, ref errorOrientation, out errorOrientation);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness);
//Turn this into an axis-angle representation.
Toolbox.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle);
//Scale the axis by the desired velocity if the angle is sufficiently large (epsilon).
if (angle > Toolbox.BigEpsilon)
{
float velocity = MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle / dt) + angle * errorReduction;
biasVelocity.X = axis.X * velocity;
biasVelocity.Y = axis.Y * velocity;
biasVelocity.Z = axis.Z * velocity;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > settings.servo.maxCorrectiveVelocitySquared)
{
float multiplier = settings.servo.maxCorrectiveVelocity / (float) Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
else
{
//Wouldn't want an old frame's bias velocity to sneak in.
biasVelocity = new Vector3();
}
}
else
{
usedSoftness = settings.velocityMotor.softness / dt;
angle = 0; //Zero out the error;
Matrix3x3 transform = basis.WorldTransform;
Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity);
}
//Compute effective mass
effectiveMassMatrix = entity.inertiaTensorInverse;
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}