internal override void SolveVelocityConstraints(TimeStep step)
{
Body b = _body2;
Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());
// Cdot = v + cross(w, r)
Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse));
Vec2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = step.Dt * _maxForce;
if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.Length();
}
impulse = _impulse - oldImpulse;
b._linearVelocity += b._invMass * impulse;
b._angularVelocity += b._invI * Vec2.Cross(r, impulse);
}