internal override void SolveVelocityConstraints(TimeStep step)
{
//B2_NOT_USED(step);
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection( _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity);
Vector2 v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity);
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * r1.Cross(P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * r2.Cross(P);
}