public override float SolveIteration()
{
float lambda;
float lambdaTotal = 0;
float velocityA, velocityB;
float previousAccumulatedImpulse;
if (minIsActive)
{
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out velocityA);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out velocityB);
//Add in the constraint space bias velocity
lambda = -(velocityA + velocityB) + biasVelocity.X - softness * accumulatedImpulse.X;
//Transform to an impulse
lambda *= velocityToImpulse.X;
//Clamp accumulated impulse (can't go negative)
previousAccumulatedImpulse = accumulatedImpulse.X;
accumulatedImpulse.X = MathHelper.Max(accumulatedImpulse.X + lambda, 0);
lambda = accumulatedImpulse.X - previousAccumulatedImpulse;
//Apply the impulse
Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3.Multiply(ref jacobianMinA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Multiply(ref jacobianMinB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
lambdaTotal += Math.Abs(lambda);
}
if (maxIsActive)
{
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out velocityA);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out velocityB);
//Add in the constraint space bias velocity
lambda = -(velocityA + velocityB) + biasVelocity.Y - softness * accumulatedImpulse.Y;
//Transform to an impulse
lambda *= velocityToImpulse.Y;
//Clamp accumulated impulse (can't go negative)
previousAccumulatedImpulse = accumulatedImpulse.Y;
accumulatedImpulse.Y = MathHelper.Max(accumulatedImpulse.Y + lambda, 0);
lambda = accumulatedImpulse.Y - previousAccumulatedImpulse;
//Apply the impulse
Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3.Multiply(ref jacobianMaxA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Multiply(ref jacobianMaxB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
lambdaTotal += Math.Abs(lambda);
}
return lambdaTotal;
}