public override void Update(float dt)
{
Quaternion quaternionA;
Quaternion.Multiply(ref connectionA.orientation, ref initialQuaternionConjugateA, out quaternionA);
Quaternion quaternionB;
Quaternion.Multiply(ref connectionB.orientation, ref initialQuaternionConjugateB, out quaternionB);
Quaternion.Conjugate(ref quaternionB, out quaternionB);
Quaternion intermediate;
Quaternion.Multiply(ref quaternionA, ref quaternionB, out intermediate);
float angle;
Vector3 axis;
Toolbox.GetAxisAngleFromQuaternion(ref intermediate, out axis, out angle);
error.X = axis.X * angle;
error.Y = axis.Y * angle;
error.Z = axis.Z * angle;
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
errorReduction = -errorReduction;
biasVelocity.X = errorReduction * error.X;
biasVelocity.Y = errorReduction * error.Y;
biasVelocity.Z = errorReduction * error.Z;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > maxCorrectiveVelocitySquared)
{
float multiplier = maxCorrectiveVelocity / (float) Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix);
effectiveMassMatrix.M11 += softness;
effectiveMassMatrix.M22 += softness;
effectiveMassMatrix.M33 += softness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
}
