internal override void solveVelocityConstraints( ref SolverData data )
{
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
Vector2 vpA = vA + MathUtils.cross( wA, _rA );
Vector2 vpB = vB + MathUtils.cross( wB, _rB );
float Cdot = Vector2.Dot( _u, vpB - vpA );
float impulse = -_mass * ( Cdot + _bias + _gamma * _impulse );
_impulse += impulse;
Vector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.cross( _rA, P );
vB += _invMassB * P;
wB += _invIB * MathUtils.cross( _rB, P );
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}