public override void Update(float dt)
{
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB);
float errorReductionParameter;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReductionParameter, out softness);
//Mass Matrix
Matrix3x3 k;
Matrix3x3 linearComponent;
Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct);
Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct);
if (connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentA, angularComponentB;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
Matrix3x3.Subtract(ref k, ref angularComponentB, out k);
}
else if (connectionA.isDynamic && !connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent);
Matrix3x3 angularComponentA;
Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA);
Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k);
}
else if (!connectionA.isDynamic && connectionB.isDynamic)
{
Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent);
Matrix3x3 angularComponentB;
Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB);
Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB);
Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k);
}
else
{
throw new InvalidOperationException("Cannot constrain two kinematic bodies.");
}
k.M11 += softness;
k.M22 += softness;
k.M33 += softness;
Matrix3x3.Invert(ref k, out massMatrix);
Vector3.Add(ref connectionB.position, ref worldOffsetB, out error);
Vector3.Subtract(ref error, ref connectionA.position, out error);
Vector3.Subtract(ref error, ref worldOffsetA, out error);
Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity);
//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;
}
}