public override void Update(float dt)
{
//Transform the axes into world space.
Matrix3x3.Transform(ref localHingeAxis, ref connectionA.orientationMatrix, out worldHingeAxis);
Matrix3x3.Transform(ref localTwistAxis, ref connectionB.orientationMatrix, out worldTwistAxis);
//****** VELOCITY BIAS ******//
Vector3.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
//Compute the correction velocity.
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);
//Compute the jacobian
Vector3.Cross(ref worldHingeAxis, ref worldTwistAxis, out jacobianA);
float length = jacobianA.LengthSquared();
if (length > Toolbox.Epsilon)
Vector3.Divide(ref jacobianA, (float)Math.Sqrt(length), out jacobianA);
else
jacobianA = new Vector3();
jacobianB.X = -jacobianA.X;
jacobianB.Y = -jacobianA.Y;
jacobianB.Z = -jacobianA.Z;
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
}
else
entryA = 0;
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
entryB = 0;
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}
