public override float SolveIteration()
{
//Compute relative velocity
Vector3 lambda;
Vector3.Cross(ref r, ref entity.angularVelocity, out lambda);
Vector3.Subtract(ref lambda, ref entity.linearVelocity, out lambda);
//Add in bias velocity
Vector3.Add(ref biasVelocity, ref lambda, out lambda);
//Add in softness
Vector3 softnessVelocity;
Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity);
Vector3.Subtract(ref lambda, ref softnessVelocity, out lambda);
//In terms of an impulse (an instantaneous change in momentum), what is it?
Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
//Sum the impulse.
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse += lambda;
//If the impulse it takes to get to the goal is too high for the motor to handle, scale it back.
float sumImpulseLengthSquared = accumulatedImpulse.LengthSquared();
if (sumImpulseLengthSquared > maxForceDtSquared)
{
//max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum.
accumulatedImpulse *= maxForceDt / (float)Math.Sqrt(sumImpulseLengthSquared);
//Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained.
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
entity.ApplyLinearImpulse(ref lambda);
Vector3 taImpulse;
Vector3.Cross(ref r, ref lambda, out taImpulse);
entity.ApplyAngularImpulse(ref taImpulse);
return (Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z));
}
