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 C = _length - maxLength;
float Cdot = Vector2.Dot( _u, vpB - vpA );
// Predictive constraint.
if( C < 0.0f )
{
Cdot += data.step.inv_dt * C;
}
float impulse = -_mass * Cdot;
float oldImpulse = _impulse;
_impulse = Math.Min( 0.0f, _impulse + impulse );
impulse = _impulse - oldImpulse;
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;
}