public override void Update(float dt)
{
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo.
{
//The error is computed using this equation:
//GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
//GoalRelativeOrientation is the original rotation from A to B in A's local space.
//Multiplying by A's orientation gives us where B *should* be.
//Of course, B won't be exactly where it should be after initialization.
//The Error component holds the difference between what is and what should be.
//Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
//ConnectionA.Orientation is replaced in the above by the world space basis orientation.
Quaternion worldBasis = Matrix3x3.CreateQuaternion(basis.WorldTransform);
Quaternion bTarget;
Quaternion.Concatenate(ref settings.servo.goal, ref worldBasis, out bTarget);
Quaternion bTargetConjugate;
Quaternion.Conjugate(ref bTarget, out bTargetConjugate);
Quaternion error;
Quaternion.Concatenate(ref bTargetConjugate, ref connectionB.orientation, out error);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness);
//Turn this into an axis-angle representation.
Toolbox.GetAxisAngleFromQuaternion(ref error, 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
{
biasVelocity.X = 0;
biasVelocity.Y = 0;
biasVelocity.Z = 0;
}
}
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
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}
