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 tSVector3 = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA); TSVector2 tSVector4 = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB); Mat33 mat = default(Mat33); mat.ex.X = invMassA + invMassB + tSVector3.y * tSVector3.y * invIA + tSVector4.y * tSVector4.y * invIB; mat.ey.X = -tSVector3.y * tSVector3.x * invIA - tSVector4.y * tSVector4.x * invIB; mat.ez.X = -tSVector3.y * invIA - tSVector4.y * invIB; mat.ex.Y = mat.ey.X; mat.ey.Y = invMassA + invMassB + tSVector3.x * tSVector3.x * invIA + tSVector4.x * tSVector4.x * invIB; mat.ez.Y = tSVector3.x * invIA + tSVector4.x * invIB; mat.ex.Z = mat.ez.X; mat.ey.Z = mat.ez.Y; mat.ez.Z = invIA + invIB; bool flag = this.FrequencyHz > 0f; FP magnitude; FP x; if (flag) { TSVector2 b = tSVector2 + tSVector4 - tSVector - tSVector3; magnitude = b.magnitude; x = 0f; TSVector2 tSVector5 = -mat.Solve22(b); tSVector -= invMassA * tSVector5; fP -= invIA * MathUtils.Cross(tSVector3, tSVector5); tSVector2 += invMassB * tSVector5; fP2 += invIB * MathUtils.Cross(tSVector4, tSVector5); } else { TSVector2 tSVector6 = tSVector2 + tSVector4 - tSVector - tSVector3; FP fP3 = fP2 - fP - this.ReferenceAngle; magnitude = tSVector6.magnitude; x = FP.Abs(fP3); Vector3 b2 = new Vector3(tSVector6.x, tSVector6.y, fP3); Vector3 vector = -mat.Solve33(b2); TSVector2 tSVector7 = new TSVector2(vector.X, vector.Y); tSVector -= invMassA * tSVector7; fP -= invIA * (MathUtils.Cross(tSVector3, tSVector7) + vector.Z); tSVector2 += invMassB * tSVector7; fP2 += invIB * (MathUtils.Cross(tSVector4, tSVector7) + vector.Z); } 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 void SolveVelocityConstraints(ref SolverData data) { TSVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; TSVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { FP Cdot = TSVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; FP impulse = _motorMass * (_motorSpeed - Cdot); FP oldImpulse = MotorImpulse; FP maxImpulse = data.step.dt * _maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; TSVector2 P = impulse * _axis; FP LA = impulse * _a1; FP LB = impulse * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } TSVector2 Cdot1 = new TSVector2(); Cdot1.x = TSVector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA; Cdot1.y = wB - wA; if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. FP Cdot2; Cdot2 = TSVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; TSVector Cdot = new TSVector(Cdot1.x, Cdot1.y, Cdot2); TSVector f1 = _impulse; TSVector df = _K.Solve33(Cdot * -1); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.z = TrueSync.TSMath.Max(_impulse.z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.z = TrueSync.TSMath.Min(_impulse.z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) TSVector2 b = -Cdot1 - (_impulse.z - f1.z) * new TSVector2(_K.ez.x, _K.ez.y); TSVector2 f2r = _K.Solve22(b) + new TSVector2(f1.x, f1.y); _impulse.x = f2r.x; _impulse.y = f2r.y; df = _impulse - f1; TSVector2 P = df.x * _perp + df.z * _axis; FP LA = df.x * _s1 + df.y + df.z * _a1; FP LB = df.x * _s2 + df.y + df.z * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { // Limit is inactive, just solve the prismatic constraint in block form. TSVector2 df = _K.Solve22(-Cdot1); _impulse.x += df.x; _impulse.y += df.y; TSVector2 P = df.x * _perp; FP LA = df.x * _s1 + df.y; FP LB = df.x * _s2 + df.y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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); }