public override void Update(float dt)
{
//Transform local axes into world space
Matrix3x3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1);
Matrix3x3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2);
Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor);
Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor);
Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection);
//Transform local
Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB);
Vector3.Add(ref rB, ref connectionB.position, out worldPoint);
//Find the point on the line closest to the world point.
Vector3 offset;
Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset);
float distanceAlongAxis;
Vector3.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis);
Vector3 worldNearPoint;
Vector3.Multiply(ref worldLineDirection, distanceAlongAxis, out offset);
Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint);
Vector3.Subtract(ref worldNearPoint, ref connectionA.position, out rA);
//Error
Vector3 error3D;
Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D);
Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X);
Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y);
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
float bias = -errorReduction;
biasVelocity.X = bias * error.X;
biasVelocity.Y = bias * error.Y;
//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;
}
//Set up the jacobians
Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1);
Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1);
Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2);
Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2);
float m11 = 0, m22 = 0, m1221 = 0;
float inverseMass;
Vector3 intermediate;
//Compute the effective mass matrix.
if (connectionA.isDynamic)
{
inverseMass = connectionA.inverseMass;
Matrix3x3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularA1, out m11);
m11 += inverseMass;
Vector3.Dot(ref intermediate, ref angularA2, out m1221);
Matrix3x3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularA2, out m22);
m22 += inverseMass;
}
#region Mass Matrix B
if (connectionB.isDynamic)
{
float extra;
inverseMass = connectionB.inverseMass;
Matrix3x3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularB1, out extra);
m11 += inverseMass + extra;
Vector3.Dot(ref intermediate, ref angularB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate);
Vector3.Dot(ref intermediate, ref angularB2, out extra);
m22 += inverseMass + extra;
}
#endregion
negativeEffectiveMassMatrix.M11 = m11 + softness;
negativeEffectiveMassMatrix.M12 = m1221;
negativeEffectiveMassMatrix.M21 = m1221;
negativeEffectiveMassMatrix.M22 = m22 + softness;
Matrix2x2.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix);
}
