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;
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
TSVector2 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;
TSVector2 P = new TSVector2(_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 = TSVector2.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 Transform(ref TSVector2 position, ref Rot rotation)
{
this.p = position;
this.q = rotation;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
this._indexA = base.BodyA.IslandIndex;
this._indexB = base.BodyB.IslandIndex;
this._localCenterA = base.BodyA._sweep.LocalCenter;
this._localCenterB = base.BodyB._sweep.LocalCenter;
this._invMassA = base.BodyA._invMass;
this._invMassB = base.BodyB._invMass;
this._invIA = base.BodyA._invI;
this._invIB = base.BodyB._invI;
FP invMassA = this._invMassA;
FP invMassB = this._invMassB;
FP invIA = this._invIA;
FP invIB = this._invIB;
TSVector2 c = data.positions[this._indexA].c;
FP a = data.positions[this._indexA].a;
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 c2 = data.positions[this._indexB].c;
FP a2 = data.positions[this._indexB].a;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
Rot q = new Rot(a);
Rot q2 = new Rot(a2);
TSVector2 value = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
TSVector2 tSVector3 = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
TSVector2 value2 = c2 + tSVector3 - c - value;
this._ay = MathUtils.Mul(q, this._localYAxis);
this._sAy = MathUtils.Cross(value2 + value, this._ay);
this._sBy = MathUtils.Cross(tSVector3, this._ay);
this._mass = invMassA + invMassB + invIA * this._sAy * this._sAy + invIB * this._sBy * this._sBy;
bool flag = this._mass > 0f;
if (flag)
{
this._mass = 1f / this._mass;
}
this._springMass = 0f;
this._bias = 0f;
this._gamma = 0f;
bool flag2 = this.Frequency > 0f;
if (flag2)
{
this._ax = MathUtils.Mul(q, this.LocalXAxis);
this._sAx = MathUtils.Cross(value2 + value, this._ax);
this._sBx = MathUtils.Cross(tSVector3, this._ax);
FP fP3 = invMassA + invMassB + invIA * this._sAx * this._sAx + invIB * this._sBx * this._sBx;
bool flag3 = fP3 > 0f;
if (flag3)
{
this._springMass = 1f / fP3;
FP x = TSVector2.Dot(value2, this._ax);
FP y = 2f * Settings.Pi * this.Frequency;
FP x2 = 2f * this._springMass * this.DampingRatio * y;
FP y2 = this._springMass * y * y;
FP dt = data.step.dt;
this._gamma = dt * (x2 + dt * y2);
bool flag4 = this._gamma > 0f;
if (flag4)
{
this._gamma = 1f / this._gamma;
}
this._bias = x * dt * y2 * this._gamma;
this._springMass = fP3 + this._gamma;
bool flag5 = this._springMass > 0f;
if (flag5)
{
this._springMass = 1f / this._springMass;
}
}
}
else
{
this._springImpulse = 0f;
}
bool enableMotor = this._enableMotor;
if (enableMotor)
{
this._motorMass = invIA + invIB;
bool flag6 = this._motorMass > 0f;
if (flag6)
{
this._motorMass = 1f / this._motorMass;
}
}
else
{
this._motorMass = 0f;
this._motorImpulse = 0f;
}
this._impulse *= data.step.dtRatio;
this._springImpulse *= data.step.dtRatio;
this._motorImpulse *= data.step.dtRatio;
TSVector2 value3 = this._impulse * this._ay + this._springImpulse * this._ax;
FP y3 = this._impulse * this._sAy + this._springImpulse * this._sAx + this._motorImpulse;
FP y4 = this._impulse * this._sBy + this._springImpulse * this._sBx + this._motorImpulse;
tSVector -= this._invMassA * value3;
fP -= this._invIA * y3;
tSVector2 += this._invMassB * value3;
fP2 += this._invIB * y4;
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}