public override float SolveIteration()
{
float velocityA, velocityB;
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB);
//Add in the constraint space bias velocity
float lambda = -(velocityA + velocityB) - biasVelocity - softness * accumulatedImpulse;
//Transform to an impulse
lambda *= velocityToImpulse;
//Accumulate the impulse
accumulatedImpulse += lambda;
//Apply the impulse
Vector3 impulse;
if (connectionA.isDynamic)
{
Vector3.Multiply(ref jacobianA, lambda, out impulse);
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3.Multiply(ref jacobianB, lambda, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return (Math.Abs(lambda));
}