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;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
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 );
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = ( iA + iB == 0.0f );
_mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
_mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
_mass.ez.X = -_rA.Y * iA - _rB.Y * iB;
_mass.ex.Y = _mass.ey.X;
_mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
_mass.ez.Y = _rA.X * iA + _rB.X * iB;
_mass.ex.Z = _mass.ez.X;
_mass.ey.Z = _mass.ez.Y;
_mass.ez.Z = iA + iB;
_motorMass = iA + iB;
if( _motorMass > 0.0f )
{
_motorMass = 1.0f / _motorMass;
}
if( _enableMotor == false || fixedRotation )
{
_motorImpulse = 0.0f;
}
if( _enableLimit && fixedRotation == false )
{
float jointAngle = aB - aA - referenceAngle;
if( Math.Abs( _upperAngle - _lowerAngle ) < 2.0f * Settings.angularSlop )
{
_limitState = LimitState.Equal;
}
else if( jointAngle <= _lowerAngle )
{
if( _limitState != LimitState.AtLower )
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if( jointAngle >= _upperAngle )
{
if( _limitState != LimitState.AtUpper )
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if( Settings.enableWarmstarting )
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
Vector2 P = new Vector2( _impulse.X, _impulse.Y );
vA -= mA * P;
wA -= iA * ( MathUtils.cross( _rA, P ) + motorImpulse + _impulse.Z );
vB += mB * P;
wB += iB * ( MathUtils.cross( _rB, P ) + motorImpulse + _impulse.Z );
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}