public override void Update(float dt)
{
//Transform the anchors and offsets into world space.
Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out offsetA);
Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out offsetB);
Vector3.Add(ref connectionA.position, ref offsetA, out anchorA);
Vector3.Add(ref connectionB.position, ref offsetB, out anchorB);
//Compute the distance.
Vector3 separation;
Vector3.Subtract(ref anchorB, ref anchorA, out separation);
float currentDistance = separation.Length();
//Compute jacobians
if (currentDistance > Toolbox.Epsilon)
{
jLinearB.X = separation.X / currentDistance;
jLinearB.Y = separation.Y / currentDistance;
jLinearB.Z = separation.Z / currentDistance;
}
else
jLinearB = Toolbox.ZeroVector;
jLinearA.X = -jLinearB.X;
jLinearA.Y = -jLinearB.Y;
jLinearA.Z = -jLinearB.Z;
Vector3.Cross(ref offsetA, ref jLinearB, out jAngularA);
//Still need to negate angular A. It's done after the effective mass matrix.
Vector3.Cross(ref offsetB, ref jLinearB, out jAngularB);
//Compute effective mass matrix
if (connectionA.isDynamic && connectionB.isDynamic)
{
Vector3 aAngular;
Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular);
Vector3.Cross(ref aAngular, ref offsetA, out aAngular);
Vector3 bAngular;
Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular);
Vector3.Cross(ref bAngular, ref offsetB, out bAngular);
Vector3.Add(ref aAngular, ref bAngular, out aAngular);
Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse);
velocityToImpulse += connectionA.inverseMass + connectionB.inverseMass;
}
else if (connectionA.isDynamic)
{
Vector3 aAngular;
Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular);
Vector3.Cross(ref aAngular, ref offsetA, out aAngular);
Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse);
velocityToImpulse += connectionA.inverseMass;
}
else if (connectionB.isDynamic)
{
Vector3 bAngular;
Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular);
Vector3.Cross(ref bAngular, ref offsetB, out bAngular);
Vector3.Dot(ref bAngular, ref jLinearB, out velocityToImpulse);
velocityToImpulse += connectionB.inverseMass;
}
else
{
//No point in trying to solve with two kinematics.
isActiveInSolver = false;
accumulatedImpulse = 0;
return;
}
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness);
velocityToImpulse = 1 / (softness + velocityToImpulse);
//Finish computing jacobian; it's down here as an optimization (since it didn't need to be negated in mass matrix)
jAngularA.X = -jAngularA.X;
jAngularA.Y = -jAngularA.Y;
jAngularA.Z = -jAngularA.Z;
//Compute bias velocity
error = distance - currentDistance;
biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity);
}
