public override float SolveIteration()
{
//Compute the current relative velocity.
float lambda, dot;
Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda);
Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot);
lambda += dot;
Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot);
lambda += dot;
Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot);
lambda += dot;
//Add in the constraint space bias velocity
lambda = -lambda + biasVelocity - softness * accumulatedImpulse;
//Transform to an impulse
lambda *= velocityToImpulse;
//Clamp accumulated impulse (can't go negative)
float previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
//Apply the impulse
Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3.Multiply(ref jLinearA, lambda, out impulse);
connectionA.ApplyLinearImpulse(ref impulse);
Vector3.Multiply(ref jAngularA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Multiply(ref jLinearB, lambda, out impulse);
connectionB.ApplyLinearImpulse(ref impulse);
Vector3.Multiply(ref jAngularB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return (Math.Abs(lambda));
}