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);
}
/// <summary>
/// Set this based on the position and angle.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="angle">The angle.</param>
public void Set(TSVector2 position, FP angle)
{
p = position;
q.Set(angle);
}