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); }