internal override void initVelocityConstraints( ref SolverData data )
{
_indexA = bodyA.islandIndex;
_indexB = bodyB.islandIndex;
_localCenterA = bodyA._sweep.LocalCenter;
_localCenterB = bodyB._sweep.LocalCenter;
_invMassA = bodyA._invMass;
_invMassB = bodyB._invMass;
_invIA = bodyA._invI;
_invIB = bodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot( aA ), qB = new Rot( aB );
_rA = MathUtils.mul( qA, localAnchorA - _localCenterA );
_rB = MathUtils.mul( qB, localAnchorB - _localCenterB );
_u = cB + _rB - cA - _rA;
// Handle singularity.
float length = _u.Length();
if( length > Settings.linearSlop )
{
_u *= 1.0f / length;
}
else
{
_u = Vector2.Zero;
}
float crAu = MathUtils.cross( _rA, _u );
float crBu = MathUtils.cross( _rB, _u );
float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu;
// Compute the effective mass matrix.
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if( frequency > 0.0f )
{
float C = length - this.length;
// Frequency
float omega = 2.0f * Settings.pi * frequency;
// Damping coefficient
float d = 2.0f * _mass * dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
float h = data.step.dt;
_gamma = h * ( d + h * k );
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invMass += _gamma;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
}
else
{
_gamma = 0.0f;
_bias = 0.0f;
}
if( Settings.enableWarmstarting )
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.cross( _rA, P );
vB += _invMassB * P;
wB += _invIB * MathUtils.cross( _rB, P );
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}