public override float SolveIteration()
{
float linearSpeed = entity.linearVelocity.LengthSquared();
if (linearSpeed > maximumSpeedSquared)
{
linearSpeed = (float) Math.Sqrt(linearSpeed);
Vector3 impulse;
//divide by linearSpeed to normalize the velocity.
//Multiply by linearSpeed - maximumSpeed to get the 'velocity change vector.'
Vector3.Multiply(ref entity.linearVelocity, -(linearSpeed - maximumSpeed) / linearSpeed, out impulse);
//incorporate softness
Vector3 softnessImpulse;
Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse);
Vector3.Subtract(ref impulse, ref softnessImpulse, out impulse);
//Transform into impulse
Vector3.Multiply(ref impulse, effectiveMassMatrix, out impulse);
//Accumulate
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
Vector3.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse);
float forceMagnitude = accumulatedImpulse.LengthSquared();
if (forceMagnitude > maxForceDtSquared)
{
//max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum.
float multiplier = maxForceDt / (float) Math.Sqrt(forceMagnitude);
accumulatedImpulse.X *= multiplier;
accumulatedImpulse.Y *= multiplier;
accumulatedImpulse.Z *= multiplier;
//Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained.
impulse.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
impulse.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
impulse.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z;
}
entity.ApplyLinearImpulse(ref impulse);
return (Math.Abs(impulse.X) + Math.Abs(impulse.Y) + Math.Abs(impulse.Z));
}
return 0;
}
