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