/// <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); }