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 x = 0f;
bool flag = this._invIA + this._invIB == 0f;
bool flag2 = this._enableLimit && this._limitState != LimitState.Inactive && !flag;
if (flag2)
{
FP x2 = fP2 - fP - this.ReferenceAngle;
FP y = 0f;
bool flag3 = this._limitState == LimitState.Equal;
if (flag3)
{
FP fP3 = MathUtils.Clamp(x2 - this._lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
y = -this._motorMass * fP3;
x = FP.Abs(fP3);
}
else
{
bool flag4 = this._limitState == LimitState.AtLower;
if (flag4)
{
FP fP4 = x2 - this._lowerAngle;
x = -fP4;
fP4 = MathUtils.Clamp(fP4 + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0f);
y = -this._motorMass * fP4;
}
else
{
bool flag5 = this._limitState == LimitState.AtUpper;
if (flag5)
{
FP fP5 = x2 - this._upperAngle;
x = fP5;
fP5 = MathUtils.Clamp(fP5 - Settings.AngularSlop, 0f, Settings.MaxAngularCorrection);
y = -this._motorMass * fP5;
}
}
}
fP -= this._invIA * y;
fP2 += this._invIB * y;
}
q.Set(fP);
q2.Set(fP2);
TSVector2 tSVector3 = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
TSVector2 tSVector4 = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
TSVector2 b = tSVector2 + tSVector4 - tSVector - tSVector3;
FP magnitude = b.magnitude;
FP invMassA = this._invMassA;
FP invMassB = this._invMassB;
FP invIA = this._invIA;
FP invIB = this._invIB;
Mat22 mat = default(Mat22);
mat.ex.x = invMassA + invMassB + invIA * tSVector3.y * tSVector3.y + invIB * tSVector4.y * tSVector4.y;
mat.ex.y = -invIA * tSVector3.x * tSVector3.y - invIB * tSVector4.x * tSVector4.y;
mat.ey.x = mat.ex.y;
mat.ey.y = invMassA + invMassB + invIA * tSVector3.x * tSVector3.x + invIB * tSVector4.x * tSVector4.x;
TSVector2 tSVector5 = -mat.Solve(b);
tSVector -= invMassA * tSVector5;
fP -= invIA * MathUtils.Cross(tSVector3, tSVector5);
tSVector2 += invMassB * tSVector5;
fP2 += invIB * MathUtils.Cross(tSVector4, tSVector5);
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(magnitude <= Settings.LinearSlop && x <= 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);
}
