/// <summary> Apply the impulse caused by the joint to the bodies attached.</summary>
public virtual void ApplyImpulse()
{
Vector2f dv = new Vector2f(body2.Velocity);
dv.Add(MathUtil.Cross(body2.AngularVelocity, r2));
dv.Sub(body1.Velocity);
dv.Sub(MathUtil.Cross(body1.AngularVelocity, r1));
dv.Scale(- 1);
dv.Add(bias); // TODO: is this baumgarte stabilization?
if (dv.LengthSquared() == 0)
{
return ;
}
Vector2f impulse = MathUtil.Mul(M, dv);
if (!body1.Static)
{
Vector2f delta1 = new Vector2f(impulse);
delta1.Scale(- body1.InvMass);
body1.AdjustVelocity(delta1);
body1.AdjustAngularVelocity((- body1.InvI) * MathUtil.Cross(r1, impulse));
}
if (!body2.Static)
{
Vector2f delta2 = new Vector2f(impulse);
delta2.Scale(body2.InvMass);
body2.AdjustVelocity(delta2);
body2.AdjustAngularVelocity(body2.InvI * MathUtil.Cross(r2, impulse));
}
accumulatedImpulse.Add(impulse);
}