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;
}
public static TSVector2 MulT(ref Mat22 A, TSVector2 v)
{
return(MathUtils.MulT(ref A, ref v));
}
public static TSVector2 MulT(ref Mat22 A, ref TSVector2 v)
{
return(new TSVector2(v.x * A.ex.x + v.y * A.ex.y, v.x * A.ey.x + v.y * A.ey.y));
}
public static TSVector2 Mul(ref Mat22 A, ref TSVector2 v)
{
return(new TSVector2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y));
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
TSVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
TSVector2 d = cB + rB - cA - rA;
TSVector2 axis = MathUtils.Mul(qA, LocalXAxis);
FP a1 = MathUtils.Cross(d + rA, axis);
FP a2 = MathUtils.Cross(rB, axis);
TSVector2 perp = MathUtils.Mul(qA, _localYAxisA);
FP s1 = MathUtils.Cross(d + rA, perp);
FP s2 = MathUtils.Cross(rB, perp);
TSVector impulse;
TSVector2 C1 = new TSVector2();
C1.x = TSVector2.Dot(perp, d);
C1.y = aB - aA - ReferenceAngle;
FP linearError = FP.Abs(C1.x);
FP angularError = FP.Abs(C1.y);
bool active = false;
FP C2 = 0.0f;
if (_enableLimit)
{
FP translation = TSVector2.Dot(axis, d);
if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = TrueSync.TSMath.Max(linearError, FP.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = TrueSync.TSMath.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = TrueSync.TSMath.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
FP k12 = iA * s1 + iB * s2;
FP k13 = iA * s1 * a1 + iB * s2 * a2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
FP k23 = iA * a1 + iB * a2;
FP k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new TSVector(k11, k12, k13);
K.ey = new TSVector(k12, k22, k23);
K.ez = new TSVector(k13, k23, k33);
TSVector C = new TSVector();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = K.Solve33(C * -1);
}
else
{
FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
FP k12 = iA * s1 + iB * s2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new TSVector2(k11, k12);
K.ey = new TSVector2(k12, k22);
TSVector2 impulse1 = K.Solve(-C1);
impulse = new TSVector();
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
TSVector2 P = impulse.x * perp + impulse.z * axis;
FP LA = impulse.x * s1 + impulse.y + impulse.z * a1;
FP LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
TSVector2 tSVector = data.positions[this._indexA].c;
FP fP = data.positions[this._indexA].a;
TSVector2 tSVector2 = data.positions[this._indexB].c;
FP fP2 = data.positions[this._indexB].a;
Rot q = new Rot(fP);
Rot q2 = new Rot(fP2);
FP invMassA = this._invMassA;
FP invMassB = this._invMassB;
FP invIA = this._invIA;
FP invIB = this._invIB;
TSVector2 value = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
TSVector2 tSVector3 = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
TSVector2 tSVector4 = tSVector2 + tSVector3 - tSVector - value;
TSVector2 tSVector5 = MathUtils.Mul(q, this.LocalXAxis);
FP y = MathUtils.Cross(tSVector4 + value, tSVector5);
FP y2 = MathUtils.Cross(tSVector3, tSVector5);
TSVector2 tSVector6 = MathUtils.Mul(q, this._localYAxisA);
FP y3 = MathUtils.Cross(tSVector4 + value, tSVector6);
FP y4 = MathUtils.Cross(tSVector3, tSVector6);
TSVector2 tSVector7 = default(TSVector2);
tSVector7.x = TSVector2.Dot(tSVector6, tSVector4);
tSVector7.y = fP2 - fP - this.ReferenceAngle;
FP fP3 = FP.Abs(tSVector7.x);
FP x = FP.Abs(tSVector7.y);
bool flag = false;
FP z = 0f;
bool enableLimit = this._enableLimit;
if (enableLimit)
{
FP fP4 = TSVector2.Dot(tSVector5, tSVector4);
bool flag2 = FP.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop;
if (flag2)
{
z = MathUtils.Clamp(fP4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
fP3 = TSMath.Max(fP3, FP.Abs(fP4));
flag = true;
}
else
{
bool flag3 = fP4 <= this._lowerTranslation;
if (flag3)
{
z = MathUtils.Clamp(fP4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
fP3 = TSMath.Max(fP3, this._lowerTranslation - fP4);
flag = true;
}
else
{
bool flag4 = fP4 >= this._upperTranslation;
if (flag4)
{
z = MathUtils.Clamp(fP4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
fP3 = TSMath.Max(fP3, fP4 - this._upperTranslation);
flag = true;
}
}
}
}
bool flag5 = flag;
Vector3 vector;
if (flag5)
{
FP x2 = invMassA + invMassB + invIA * y3 * y3 + invIB * y4 * y4;
FP fP5 = invIA * y3 + invIB * y4;
FP fP6 = invIA * y3 * y + invIB * y4 * y2;
FP fP7 = invIA + invIB;
bool flag6 = fP7 == 0f;
if (flag6)
{
fP7 = 1f;
}
FP fP8 = invIA * y + invIB * y2;
FP z2 = invMassA + invMassB + invIA * y * y + invIB * y2 * y2;
Mat33 mat = default(Mat33);
mat.ex = new Vector3(x2, fP5, fP6);
mat.ey = new Vector3(fP5, fP7, fP8);
mat.ez = new Vector3(fP6, fP8, z2);
vector = mat.Solve33(-new Vector3
{
X = tSVector7.x,
Y = tSVector7.y,
Z = z
});
}
else
{
FP x3 = invMassA + invMassB + invIA * y3 * y3 + invIB * y4 * y4;
FP fP9 = invIA * y3 + invIB * y4;
FP fP10 = invIA + invIB;
bool flag7 = fP10 == 0f;
if (flag7)
{
fP10 = 1f;
}
Mat22 mat2 = default(Mat22);
mat2.ex = new TSVector2(x3, fP9);
mat2.ey = new TSVector2(fP9, fP10);
TSVector2 tSVector8 = mat2.Solve(-tSVector7);
vector = default(Vector3);
vector.X = tSVector8.x;
vector.Y = tSVector8.y;
vector.Z = 0f;
}
TSVector2 value2 = vector.X * tSVector6 + vector.Z * tSVector5;
FP y5 = vector.X * y3 + vector.Y + vector.Z * y;
FP y6 = vector.X * y4 + vector.Y + vector.Z * y2;
tSVector -= invMassA * value2;
fP -= invIA * y5;
tSVector2 += invMassB * value2;
fP2 += invIB * y6;
data.positions[this._indexA].c = tSVector;
data.positions[this._indexA].a = fP;
data.positions[this._indexB].c = tSVector2;
data.positions[this._indexB].a = fP2;
return(fP3 <= Settings.LinearSlop && x <= Settings.AngularSlop);
}
public static TSVector2 Mul(ref Mat22 A, TSVector2 v)
{
return(Mul(ref A, ref v));
}
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;
}