public static TSVector2 Mul22(Mat33 A, 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));
}
public static Vector3 Mul(Mat33 A, Vector3 v)
{
return(v.X * A.ex + v.Y * A.ey + v.Z * A.ez);
}
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 a = data.positions[this._indexA].a;
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
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);
this._rA = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
this._rB = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
FP invMassA = this._invMassA;
FP invMassB = this._invMassB;
FP invIA = this._invIA;
FP invIB = this._invIB;
Mat33 mat = default(Mat33);
mat.ex.X = invMassA + invMassB + this._rA.y * this._rA.y * invIA + this._rB.y * this._rB.y * invIB;
mat.ey.X = -this._rA.y * this._rA.x * invIA - this._rB.y * this._rB.x * invIB;
mat.ez.X = -this._rA.y * invIA - this._rB.y * invIB;
mat.ex.Y = mat.ey.X;
mat.ey.Y = invMassA + invMassB + this._rA.x * this._rA.x * invIA + this._rB.x * this._rB.x * invIB;
mat.ez.Y = this._rA.x * invIA + this._rB.x * invIB;
mat.ex.Z = mat.ez.X;
mat.ey.Z = mat.ez.Y;
mat.ez.Z = invIA + invIB;
bool flag = this.FrequencyHz > 0f;
if (flag)
{
mat.GetInverse22(ref this._mass);
FP fP3 = invIA + invIB;
FP fP4 = (fP3 > 0f) ? (1f / fP3) : 0f;
FP x = a2 - a - this.ReferenceAngle;
FP y = 2f * Settings.Pi * this.FrequencyHz;
FP x2 = 2f * fP4 * this.DampingRatio * y;
FP y2 = fP4 * y * y;
FP dt = data.step.dt;
this._gamma = dt * (x2 + dt * y2);
this._gamma = ((this._gamma != 0f) ? (1f / this._gamma) : 0f);
this._bias = x * dt * y2 * this._gamma;
fP3 += this._gamma;
this._mass.ez.Z = ((fP3 != 0f) ? (1f / fP3) : 0f);
}
else
{
mat.GetSymInverse33(ref this._mass);
this._gamma = 0f;
this._bias = 0f;
}
this._impulse *= data.step.dtRatio;
TSVector2 tSVector3 = new TSVector2(this._impulse.X, this._impulse.Y);
tSVector -= invMassA * tSVector3;
fP -= invIA * (MathUtils.Cross(this._rA, tSVector3) + this._impulse.Z);
tSVector2 += invMassB * tSVector3;
fP2 += invIB * (MathUtils.Cross(this._rB, tSVector3) + this._impulse.Z);
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}
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);
}
/// Multiply a matrix times a vector.
public static TSVector Mul(Mat33 A, TSVector v)
{
return(v.x * A.ex + v.y * A.ey + v.z * A.ez);
}