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;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA);
Rot qB = new Rot(aB);
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, -_localCenterA);
_rB = MathUtils.Mul(qB, -_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]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.x = mA + mB + iA * _rA.y * _rA.y + iB * _rB.y * _rB.y;
K.ex.y = -iA * _rA.x * _rA.y - iB * _rB.x * _rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * _rA.x * _rA.x + iB * _rB.x * _rB.x;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
_linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
_angularError = aB - aA - _angularOffset;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
FPVector2 P = new FPVector2(_linearImpulse.x, _linearImpulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = FPVector2.zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public static FPVector2 MulT(ref Mat22 A, ref FPVector2 v)
{
return(new FPVector2(v.x * A.ex.x + v.y * A.ex.y, v.x * A.ey.x + v.y * A.ey.y));
}
public static FPVector2 Mul(ref Mat22 A, ref FPVector2 v)
{
return(new FPVector2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y));
}
public static FPVector2 MulT(ref Mat22 A, FPVector2 v)
{
return(MulT(ref A, ref v));
}
public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R)
{
R.ex = A.ex + B.ex;
R.ey = A.ey + B.ey;
}
