public override float SolveIteration()
{
#if !WINDOWS
Vector3 lambda = new Vector3();
#else
Vector3 lambda;
#endif
Vector3 aVel = entity.angularVelocity;
lambda.X = -aVel.X + biasVelocity.X - usedSoftness * accumulatedImpulse.X;
lambda.Y = -aVel.Y + biasVelocity.Y - usedSoftness * accumulatedImpulse.Y;
lambda.Z = -aVel.Z + biasVelocity.Z - usedSoftness * accumulatedImpulse.Z;
Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
Vector3 previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse.X += lambda.X;
accumulatedImpulse.Y += lambda.Y;
accumulatedImpulse.Z += lambda.Z;
float sumLengthSquared = accumulatedImpulse.LengthSquared();
if (sumLengthSquared > 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(sumLengthSquared);
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.
lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X;
lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y;
lambda.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z;
}
entity.ApplyAngularImpulse(ref lambda);
return Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z);
}