internal override void SolveVelocityConstraints(ref TimeStep step) { Body bA = BodyA; Body bB = BodyB; Vector2 vA = bA.LinearVelocityInternal; float wA = bA.AngularVelocityInternal; Vector2 vB = bB.LinearVelocityInternal; float wB = bB.AngularVelocityInternal; float mA = bA.InvMass, mB = bB.InvMass; float iA = bA.InvI, iB = bB.InvI; Transform xfA, xfB; bA.GetTransform(out xfA); bB.GetTransform(out xfB); Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter); // Solve point-to-point constraint Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA); float Cdot2 = wB - wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); _impulse += impulse; Vector2 P = new Vector2(impulse.X, impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(rB, P) + impulse.Z); bA.LinearVelocityInternal = vA; bA.AngularVelocityInternal = wA; bB.LinearVelocityInternal = vB; bB.AngularVelocityInternal = wB; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); float positionError, angularError; Mat33 K = new Mat33(); K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse; if (K.ez.Z <= 0.0f) { Vector2 impulse2 = -K.Solve22(C1); impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); } else { impulse = -K.Solve33(C); } Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vec2 v1 = b1._linearVelocity; float w1 = b1._angularVelocity; Vec2 v2 = b2._linearVelocity; float w2 = b2._angularVelocity; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.Dt * _maxMotorForce; _motorImpulse = Box2DNet.Common.MathB2.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; Vec2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } Vec2 Cdot1; Cdot1.X = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; Cdot1.Y = w2 - w1; if (_enableLimit && _limitState != LimitState.InactiveLimit) { // Solve prismatic and limit constraint in block form. var Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2); Vec3 f1 = _impulse; Vec3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLowerLimit) { _impulse.Z = Box2DNet.Common.MathB2.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpperLimit) { _impulse.Z = Box2DNet.Common.MathB2.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) Vec2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vec2(_K.Col3.X, _K.Col3.Y); Vec2 f2r = _K.Solve22(b) + new Vec2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vec2 P = df.X * _perp + df.Z * _axis; float L1 = df.X * _s1 + df.Y + df.Z * _a1; float L2 = df.X * _s2 + df.Y + df.Z * _a2; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vec2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vec2 P = df.X * _perp; float L1 = df.X * _s1 + df.Y; float L2 = df.X * _s2 + df.Y; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } b1._linearVelocity = v1; b1._angularVelocity = w1; b2._linearVelocity = v2; b2._angularVelocity = w2; }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = Vector2.Zero; const float w2 = 0; float m1 = b1.InvMass; float i1 = b1.InvI; // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; // Solve point-to-point constraint Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } Vector2 P = new Vector2(impulse.X, impulse.Y); v1 -= m1 * P; w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.Z); } else { Transform xf1; b1.GetTransform(out xf1); Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = _worldAnchor; // Solve point-to-point constraint Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; v1 -= m1 * impulse; w1 -= i1 * MathUtils.Cross(r1, impulse); } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 v1 = b1._linearVelocity; float w1 = b1._angularVelocity; Vector2 v2 = b2._linearVelocity; float w2 = b2._angularVelocity; float m1 = b1._invMass, m2 = b2._invMass; float i1 = b1._invI, i2 = b2._invI; //Solve motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.Dt * _maxMotorTorque; _motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } //Solve limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1); float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.EqualLimits) { _impulse += impulse; } else if (_limitState == LimitState.AtLowerLimit) { float newImpulse = _impulse.z + impulse.z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } } else if (_limitState == LimitState.AtUpperLimit) { float newImpulse = _impulse.z + impulse.z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } } Vector2 P = impulse.ToVector2(); v1 -= m1 * P; w1 -= i1 * (r1.Cross(P) + impulse.z); v2 += m2 * P; w2 += i2 * (r2.Cross(P) + impulse.z); } else { Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.x += impulse.x; _impulse.y += impulse.y; v1 -= m1 * impulse; w1 -= i1 * r1.Cross(impulse); v2 += m2 * impulse; w2 += i2 * r2.Cross(impulse); } b1._linearVelocity = v1; b1._angularVelocity = w1; b2._linearVelocity = v2; b2._angularVelocity = w2; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d = cB + rB - cA - rA; Vector2 axis = Complex.Multiply(ref _localXAxis, ref qA); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(ref rB, ref axis); Vector2 perp = Complex.Multiply(ref _localYAxisA, ref qA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(ref rB, ref perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = Math.Abs(C1.X); float angularError = Math.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (_enableLimit) { float translation = Vector2.Dot(axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Max(linearError, Math.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 = Math.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 = Math.Max(linearError, translation - _upperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float 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 void SolveVelocityConstraints(ref TimeStep step) { Body b1 = BodyA; Body b2 = BodyB; Vector2 v1 = b1.LinearVelocityInternal; float w1 = b1.AngularVelocityInternal; Vector2 v2 = b2.LinearVelocityInternal; float w2 = b2.AngularVelocityInternal; float m1 = b1.InvMass, m2 = b2.InvMass; float i1 = b1.InvI, i2 = b2.InvI; // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { /*Transform xf1, xf2; * b1.GetTransform(out xf1); * b2.GetTransform(out xf2);*/ Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); // Solve point-to-point constraint MathUtils.Cross(w2, ref r2, out _tmpVector2); MathUtils.Cross(w1, ref r1, out _tmpVector1); Vector2 Cdot1 = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } Vector2 P = new Vector2(impulse.X, impulse.Y); v1 -= m1 * P; MathUtils.Cross(ref r1, ref P, out _tmpFloat1); w1 -= i1 * (/* r1 x P */ _tmpFloat1 + impulse.Z); v2 += m2 * P; MathUtils.Cross(ref r2, ref P, out _tmpFloat1); w2 += i2 * (/* r2 x P */ _tmpFloat1 + impulse.Z); } else { /*Transform xf1, xf2; * b1.GetTransform(out xf1); * b2.GetTransform(out xf2);*/ _tmpVector1 = LocalAnchorA - b1.LocalCenter; _tmpVector2 = LocalAnchorB - b2.LocalCenter; Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, ref _tmpVector1); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, ref _tmpVector2); // Solve point-to-point constraint MathUtils.Cross(w2, ref r2, out _tmpVector2); MathUtils.Cross(w1, ref r1, out _tmpVector1); Vector2 Cdot = v2 + /* w2 x r2 */ _tmpVector2 - v1 - /* w1 x r1 */ _tmpVector1; Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; v1 -= m1 * impulse; MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); w1 -= i1 * /* r1 x impulse */ _tmpFloat1; v2 += m2 * impulse; MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); w2 += i2 * /* r2 x impulse */ _tmpFloat1; } b1.LinearVelocityInternal = v1; b1.AngularVelocityInternal = w1; b2.LinearVelocityInternal = v2; b2.AngularVelocityInternal = w2; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); float positionError, angularError; Mat33 K = new Mat33(); K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse = -K.Solve33(C); Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.Positions[_indexA].C; GGame.Math.Fix64 aA = data.Positions[_indexA].A; Vector2 cB = data.Positions[_indexB].C; GGame.Math.Fix64 aB = data.Positions[_indexB].A; Rot qA = new Rot(aA), qB = new Rot(aB); GGame.Math.Fix64 mA = _invMassA, mB = _invMassB; GGame.Math.Fix64 iA = _invIA, iB = _invIB; Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); GGame.Math.Fix64 positionError, angularError; Mat33 K = new Mat33(); K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; GGame.Math.Fix64 C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = GGame.Math.Fix64.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse; if (K.ez.Z > 0.0f) { impulse = -K.Solve33(C); } else { Vector2 impulse2 = -K.Solve22(C1); impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); } Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.Positions[_indexA].C = cA; data.Positions[_indexA].A = aA; data.Positions[_indexB].C = cB; data.Positions[_indexB].A = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); float positionError, angularError; Mat33 K = new Mat33(); K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (m_frequencyHz > 0.0f) { FVector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; FVector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { FVector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); FVector3 C = new FVector3(C1.X, C1.Y, C2); FVector3 impulse = -K.Solve33(C); FVector2 P = new FVector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void SolveVelocityConstraints(ref SolverData data) { FPVector2 vA = data.velocities[_indexA].v; FP wA = data.velocities[_indexA].w; FPVector2 vB = data.velocities[_indexB].v; FP wB = data.velocities[_indexB].w; FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) { FP Cdot = wB - wA - _motorSpeed; FP impulse = _motorMass * (-Cdot); FP oldImpulse = _motorImpulse; FP maxImpulse = data.step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { FPVector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); FP Cdot2 = wB - wA; FPVector Cdot = new FPVector(Cdot1.x, Cdot1.y, Cdot2); FPVector impulse = _mass.Solve33(Cdot) * -1; if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { FP newImpulse = _impulse.z + impulse.z; if (newImpulse < 0.0f) { FPVector2 rhs = -Cdot1 + _impulse.z * new FPVector2(_mass.ez.x, _mass.ez.y); FPVector2 reduced = _mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } else { _impulse += impulse; } } else if (_limitState == LimitState.AtUpper) { FP newImpulse = _impulse.z + impulse.z; if (newImpulse > 0.0f) { FPVector2 rhs = -Cdot1 + _impulse.z * new FPVector2(_mass.ez.x, _mass.ez.y); FPVector2 reduced = _mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -_impulse.z; _impulse.x += reduced.x; _impulse.y += reduced.y; _impulse.z = 0.0f; } else { _impulse += impulse; } } FPVector2 P = new FPVector2(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + impulse.z); } else { // Solve point-to-point constraint FPVector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); FPVector2 impulse = _mass.Solve22(-Cdot); _impulse.x += impulse.x; _impulse.y += impulse.y; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } 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) { Vector2 cA = data.Positions[_indexA].C; GGame.Math.Fix64 aA = data.Positions[_indexA].A; Vector2 cB = data.Positions[_indexB].C; GGame.Math.Fix64 aB = data.Positions[_indexB].A; Rot qA = new Rot(aA), qB = new Rot(aB); GGame.Math.Fix64 mA = _invMassA, mB = _invMassB; GGame.Math.Fix64 iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, LocalXAxis); GGame.Math.Fix64 a1 = MathUtils.Cross(d + rA, axis); GGame.Math.Fix64 a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, _localYAxisA); GGame.Math.Fix64 s1 = MathUtils.Cross(d + rA, perp); GGame.Math.Fix64 s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; GGame.Math.Fix64 linearError = GGame.Math.Fix64.Abs(C1.X); GGame.Math.Fix64 angularError = GGame.Math.Fix64.Abs(C1.Y); bool active = false; GGame.Math.Fix64 C2 = 0.0f; if (_enableLimit) { GGame.Math.Fix64 translation = Vector2.Dot(axis, d); if (GGame.Math.Fix64.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Max((float)linearError, (float)GGame.Math.Fix64.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 = Math.Max((float)linearError, (float)(_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 = Math.Max((float)linearError, (float)(translation - _upperTranslation)); active = true; } } if (active) { GGame.Math.Fix64 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; GGame.Math.Fix64 k12 = iA * s1 + iB * s2; GGame.Math.Fix64 k13 = iA * s1 * a1 + iB * s2 * a2; GGame.Math.Fix64 k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } GGame.Math.Fix64 k23 = iA * a1 + iB * a2; GGame.Math.Fix64 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } else { GGame.Math.Fix64 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; GGame.Math.Fix64 k12 = iA * s1 + iB * s2; GGame.Math.Fix64 k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; GGame.Math.Fix64 LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; GGame.Math.Fix64 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 void SolveVelocityConstraints() { Vector2 vA = _bodyA.velocity; float wA = _bodyA.angularVelocity * Mathf.Deg2Rad; Vector2 vB = _bodyB.velocity; float wB = _bodyB.angularVelocity * Mathf.Deg2Rad; float mA = m_InvMassA, mB = m_InvMassB; float iA = m_InvIA, iB = m_InvIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (m_EnableMotor && limitState != JointLimitState2D.EqualLimits && fixedRotation == false) { float Cdot = wB - wA - m_MotorSpeed; float impulse = m_MotorMass * (-Cdot); float oldImpulse = m_MotorImpulse; float maxImpulse = Time.deltaTime * m_MaxMotorTorque; m_MotorImpulse = Mathf.Clamp(m_MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_MotorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (m_EnableLimit && limitState != JointLimitState2D.Inactive && fixedRotation == false) { Vector2 Cdot1 = vB + MathUtils.Cross(wB, m_RB) - vA - MathUtils.Cross(wA, m_RA); float Cdot2 = wB - wA; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 impulse = -m_Mass.Solve33(Cdot); if (limitState == JointLimitState2D.EqualLimits) { m_Impulse += impulse; } else if (limitState == JointLimitState2D.LowerLimit) { float newImpulse = m_Impulse.z + impulse.z; if (newImpulse < 0.0f) { Vector2 rhs = -Cdot1 + m_Impulse.z * new Vector2(m_Mass.ez.x, m_Mass.ez.y); Vector2 reduced = m_Mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_Impulse.z; m_Impulse.x += reduced.x; m_Impulse.y += reduced.y; m_Impulse.z = 0.0f; } else { m_Impulse += impulse; } } else if (limitState == JointLimitState2D.UpperLimit) { float newImpulse = m_Impulse.z + impulse.z; if (newImpulse > 0.0f) { Vector2 rhs = -Cdot1 + m_Impulse.z * new Vector2(m_Mass.ez.x, m_Mass.ez.y); Vector2 reduced = m_Mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_Impulse.z; m_Impulse.x += reduced.x; m_Impulse.y += reduced.y; m_Impulse.z = 0.0f; } else { m_Impulse += impulse; } } Vector2 P = new Vector2(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (MathUtils.Cross(m_RA, P) + impulse.z); vB += mB * P; wB += iB * (MathUtils.Cross(m_RB, P) + impulse.z); } else { // Solve point-to-point constraint Vector2 Cdot = vB + MathUtils.Cross(wB, m_RB) - vA - MathUtils.Cross(wA, m_RA); Vector2 impulse = m_Mass.Solve22(-Cdot); m_Impulse.x += impulse.x; m_Impulse.y += impulse.y; vA -= mA * impulse; wA -= iA * MathUtils.Cross(m_RA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(m_RB, impulse); } if (!_bodyA.isKinematic) { _bodyA.velocity = vA; if (!_bodyA.fixedAngle) { _bodyA.angularVelocity = wA * Mathf.Rad2Deg; } } if (!_bodyB.isKinematic) { _bodyB.velocity = vB; if (!_bodyB.fixedAngle) { _bodyB.angularVelocity = wB * Mathf.Rad2Deg; } } }
internal override bool SolvePositionConstraints() { Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA); Vector2 cA0 = cA; float aA = _bodyA.rotation * Mathf.Deg2Rad; Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB); Vector2 cB0 = cB; float aB = _bodyB.rotation * Mathf.Deg2Rad; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = m_InvMassA, mB = m_InvMassB; float iA = m_InvIA, iB = m_InvIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_LocalCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, localXAxisA); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, m_LocalYAxisA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.x = Vector2.Dot(perp, d); C1.y = aB - aA - referenceAngle; float linearError = Mathf.Abs(C1.x); float angularError = Mathf.Abs(C1.y); bool active = false; float C2 = 0.0f; if (m_EnableLimit) { float translation = Vector2.Dot(axis, d); m_Translation = translation; if (Mathf.Abs(m_UpperTranslation - m_LowerTranslation) < 2.0f * Constants.linearSlop) { // Prevent large angular corrections C2 = Mathf.Clamp(translation, -Constants.maxLinearCorrection, Constants.maxLinearCorrection); linearError = Mathf.Max(linearError, Mathf.Abs(translation)); active = true; } else if (translation <= m_LowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = Mathf.Clamp(translation - m_LowerTranslation + Constants.linearSlop, -Constants.maxLinearCorrection, 0.0f); linearError = Mathf.Max(linearError, m_LowerTranslation - translation); active = true; } else if (translation >= m_UpperTranslation) { // Prevent large linear corrections and allow some slop. C2 = Mathf.Clamp(translation - m_UpperTranslation - Constants.linearSlop, 0.0f, Constants.maxLinearCorrection); linearError = Mathf.Max(linearError, translation - m_UpperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.x = C1.x; C.y = C1.y; C.z = C2; impulse = K.Solve33(-C); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } Vector2 P = impulse.x * perp + impulse.z * axis; float LA = impulse.x * s1 + impulse.y + impulse.z * a1; float LB = impulse.x * s2 + impulse.y + impulse.z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; if (!_bodyA.isKinematic) { var dcA = cA0 - cA; _bodyA.position -= dcA; if (!_bodyA.fixedAngle) { _bodyA.rotation = aA * Mathf.Rad2Deg; } } if (!_bodyB.isKinematic) { var dcB = cB0 - cB; _bodyB.position -= dcB; if (!_bodyB.fixedAngle) { _bodyB.rotation = aB * Mathf.Rad2Deg; } } return(linearError < Constants.linearSlop && angularError < Constants.angularSlop); }
internal override void SolveVelocityConstraints() { Vector2 vA = _bodyA.velocity; float wA = _bodyA.angularVelocity * Mathf.Deg2Rad; Vector2 vB = _bodyB.velocity; float wB = _bodyB.angularVelocity * Mathf.Deg2Rad; float mA = m_InvMassA, mB = m_InvMassB; float iA = m_InvIA, iB = m_InvIB; // Solve linear motor constraint. if (m_EnableMotor && m_LimitState != JointLimitState2D.EqualLimits) { float Cdot = Vector2.Dot(m_Axis, vB - vA) + m_A2 * wB - m_A1 * wA; float impulse = m_MotorMass * (m_MotorSpeed - Cdot); float oldImpulse = m_MotorImpulse; float maxImpulse = Time.deltaTime * m_MaxMotorForce; m_MotorImpulse = Mathf.Clamp(m_MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_MotorImpulse - oldImpulse; Vector2 P = impulse * m_Axis; float LA = impulse * m_A1; float LB = impulse * m_A2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } Vector2 Cdot1 = new Vector2(); Cdot1.x = Vector2.Dot(m_Perp, vB - vA) + m_S2 * wB - m_S1 * wA; Cdot1.y = wB - wA; if (m_EnableLimit && m_LimitState != JointLimitState2D.Inactive) { // Solve slider and limit constraint in block form. float Cdot2; Cdot2 = Vector2.Dot(m_Axis, vB - vA) + m_A2 * wB - m_A1 * wA; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 f1 = m_Impulse; Vector3 df = m_K.Solve33(-Cdot); m_Impulse += df; if (m_LimitState == JointLimitState2D.LowerLimit) { m_Impulse.z = Mathf.Max(m_Impulse.z, 0.0f); } else if (m_LimitState == JointLimitState2D.UpperLimit) { m_Impulse.z = Mathf.Min(m_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) Vector2 b = -Cdot1 - (m_Impulse.z - f1.z) * new Vector2(m_K.ez.x, m_K.ez.y); Vector2 f2r = m_K.Solve22(b) + new Vector2(f1.x, f1.y); m_Impulse.x = f2r.x; m_Impulse.y = f2r.y; df = m_Impulse - f1; Vector2 P = df.x * m_Perp + df.z * m_Axis; float LA = df.x * m_S1 + df.y + df.z * m_A1; float LB = df.x * m_S2 + df.y + df.z * m_A2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { // Limit is inactive, just solve the slider constraint in block form. Vector2 df = m_K.Solve22(-Cdot1); m_Impulse.x += df.x; m_Impulse.y += df.y; Vector2 P = df.x * m_Perp; float LA = df.x * m_S1 + df.y; float LB = df.x * m_S2 + df.y; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; //FVector2 Cdot10 = Cdot1; Cdot1.x = Vector2.Dot(m_Perp, vB - vA) + m_S2 * wB - m_S1 * wA; Cdot1.y = wB - wA; if (Mathf.Abs(Cdot1.x) > 0.01f || Mathf.Abs(Cdot1.y) > 0.01f) { //FVector2 test = MathUtils.Mul22(m_K, df); Cdot1.x += 0.0f; } } if (!_bodyA.isKinematic) { _bodyA.velocity = vA; if (!_bodyA.fixedAngle) { _bodyA.angularVelocity = wA * Mathf.Rad2Deg; } } if (!_bodyB.isKinematic) { _bodyB.velocity = vB; if (!_bodyB.fixedAngle) { _bodyB.angularVelocity = wB * Mathf.Rad2Deg; } } }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vec2 v1 = b1._linearVelocity; float w1 = b1._angularVelocity; Vec2 v2 = b2._linearVelocity; float w2 = b2._angularVelocity; float m1 = b1._invMass, m2 = b2._invMass; float i1 = b1._invI, i2 = b2._invI; //Solve motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.Dt * _maxMotorTorque; _motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } //Solve limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1); float Cdot2 = w2 - w1; Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2); Vec3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.EqualLimits) { _impulse += impulse; } else if (_limitState == LimitState.AtLowerLimit) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vec2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpperLimit) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vec2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } Vec2 P = new Vec2(impulse.X, impulse.Y); v1 -= m1 * P; w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z); v2 += m2 * P; w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z); } else { Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1); Vec2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; v1 -= m1 * impulse; w1 -= i1 * Vec2.Cross(r1, impulse); v2 += m2 * impulse; w2 += i2 * Vec2.Cross(r2, impulse); } b1._linearVelocity = v1; b1._angularVelocity = w1; b2._linearVelocity = v2; b2._angularVelocity = w2; }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Vector2 vB = data.Velocities[_indexB].V; float wB = data.Velocities[_indexB].W; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (_enableMotor && _limitState != LimitState.Equal && fixedRotation == false) { float Cdot = wB - wA - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = data.Step.dt * _maxMotorTorque; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); float Cdot2 = wB - wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = -_mass.Solve33(Cdot); if (_limitState == LimitState.Equal) { _impulse += impulse; } else if (_limitState == LimitState.AtLower) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); Vector2 reduced = _mass.Solve22(rhs); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } else { _impulse += impulse; } } else if (_limitState == LimitState.AtUpper) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 rhs = -Cdot1 + _impulse.Z * new Vector2(_mass.ez.X, _mass.ez.Y); Vector2 reduced = _mass.Solve22(rhs); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } else { _impulse += impulse; } } Vector2 P = new Vector2(impulse.X, impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z); } else { // Solve point-to-point constraint Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; data.Velocities[_indexB].V = vB; data.Velocities[_indexB].W = wB; }
internal override bool SolvePositionConstraints() { Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA); Vector2 cA0 = cA; float aA = _bodyA.rotation * Mathf.Deg2Rad; Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB); Vector2 cB0 = cB; float aB = _bodyB.rotation * Mathf.Deg2Rad; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = m_InvMassA, mB = m_InvMassB; float iA = m_InvIA, iB = m_InvIB; Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_LocalCenterB); float positionError, angularError; Mat33 K = new Mat33(); K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; K.ez.x = -rA.y * iA - rB.y * iB; K.ex.y = K.ey.x; K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; K.ez.y = rA.x * iA + rB.x * iB; K.ex.z = K.ez.x; K.ey.z = K.ez.y; K.ez.z = iA + iB; if (frequency > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.magnitude; angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - referenceAngle; positionError = C1.magnitude; angularError = Mathf.Abs(C2); Vector3 C = new Vector3(C1.x, C1.y, C2); Vector3 impulse = -K.Solve33(C); Vector2 P = new Vector2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.z); } if (!_bodyA.isKinematic) { var dcA = cA0 - cA; _bodyA.position -= dcA; if (!_bodyA.fixedAngle) { _bodyA.rotation = aA * Mathf.Rad2Deg; } } if (!_bodyB.isKinematic) { var dcB = cB0 - cB; _bodyB.position -= dcB; if (!_bodyB.fixedAngle) { _bodyB.rotation = aB * Mathf.Rad2Deg; } } return(positionError <= Constants.linearSlop && angularError <= Constants.angularSlop); }
/// <summary> /// Solves the velocity constraints using the specified step /// </summary> /// <param name="step">The step</param> internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = Body1; Body b2 = Body2; Vec2 v1 = b1.LinearVelocity; float w1 = b1.AngularVelocity; Vec2 v2 = b2.LinearVelocity; float w2 = b2.AngularVelocity; // Solve linear motor constraint. if (IsMotorEnabled && LimitState != LimitState.EqualLimits) { float cdot = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1; float impulse = MotorMass * (motorSpeedx - cdot); float oldImpulse = MotorForce; float maxImpulse = step.Dt * MaxMotorForce; MotorForce = Box2DXMath.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse); impulse = MotorForce - oldImpulse; Vec2 p = impulse * Axis; float l1 = impulse * a1; float l2 = impulse * A2; v1 -= InvMass1 * p; w1 -= InvI1 * l1; v2 += InvMass2 * p; w2 += InvI2 * l2; } Vec2 cdot1; cdot1.X = Vec2.Dot(Perp, v2 - v1) + s2 * w2 - s1 * w1; cdot1.Y = w2 - w1; if (IsLimitEnabled && LimitState != LimitState.InactiveLimit) { // Solve prismatic and limit constraint in block form. float cdot2; cdot2 = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1; Vec3 cdot = new Vec3(cdot1.X, cdot1.Y, cdot2); Vec3 f1 = Impulse; Vec3 df = K.Solve33(-cdot); Impulse += df; if (LimitState == LimitState.AtLowerLimit) { Impulse.Z = Box2DXMath.Max(Impulse.Z, 0.0f); } else if (LimitState == LimitState.AtUpperLimit) { Impulse.Z = Box2DXMath.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) Vec2 b = -cdot1 - (Impulse.Z - f1.Z) * new Vec2(K.Col3.X, K.Col3.Y); Vec2 f2R = K.Solve22(b) + new Vec2(f1.X, f1.Y); Impulse.X = f2R.X; Impulse.Y = f2R.Y; df = Impulse - f1; Vec2 p = df.X * Perp + df.Z * Axis; float l1 = df.X * s1 + df.Y + df.Z * a1; float l2 = df.X * s2 + df.Y + df.Z * A2; v1 -= InvMass1 * p; w1 -= InvI1 * l1; v2 += InvMass2 * p; w2 += InvI2 * l2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vec2 df = K.Solve22(-cdot1); Impulse.X += df.X; Impulse.Y += df.Y; Vec2 p = df.X * Perp; float l1 = df.X * s1 + df.Y; float l2 = df.X * s2 + df.Y; v1 -= InvMass1 * p; w1 -= InvI1 * l1; v2 += InvMass2 * p; w2 += InvI2 * l2; } b1.LinearVelocity = v1; b1.AngularVelocity = w1; b2.LinearVelocity = v2; b2.AngularVelocity = w2; }
internal override bool SolvePositionConstraints(ref SolverData data) { FPVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; FPVector2 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; FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FP positionError, angularError; Mat33 K = new Mat33(); K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; K.ez.x = -rA.y * iA - rB.y * iB; K.ex.y = K.ey.x; K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; K.ez.y = rA.x * iA + rB.x * iB; K.ex.z = K.ez.x; K.ey.z = K.ez.y; K.ez.z = iA + iB; if (FrequencyHz > 0.0f) { FPVector2 C1 = cB + rB - cA - rA; positionError = C1.magnitude; angularError = 0.0f; FPVector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { FPVector2 C1 = cB + rB - cA - rA; FP C2 = aB - aA - ReferenceAngle; positionError = C1.magnitude; angularError = FP.Abs(C2); FPVector C = new FPVector(C1.x, C1.y, C2); FPVector impulse = K.Solve33(C) * -1; FPVector2 P = new FPVector2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = MotorImpulse; float maxImpulse = data.step.dt * _maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; Vector2 P = impulse * _axis; float LA = impulse * _a1; float LB = impulse * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } Vector2 Cdot1 = new Vector2(); Cdot1.X = Vector2.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. float Cdot2; Cdot2 = Vector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Z = Math.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Z = Math.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) Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.ez.X, _K.ez.Y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vector2 P = df.X * _perp + df.Z * _axis; float LA = df.X * _s1 + df.Y + df.Z * _a1; float 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. Vector2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vector2 P = df.X * _perp; float LA = df.X * _s1 + df.Y; float 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) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, LocalXAxis); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, _localYAxisA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = Math.Abs(C1.X); float angularError = Math.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (_enableLimit) { float translation = Vector2.Dot(axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Max(linearError, Math.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 = Math.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 = Math.Max(linearError, translation - _upperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float 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 void SolveVelocityConstraints(ref TimeStep step) { Body b2 = BodyB; Vector2 v1 = Vector2.Zero; float w1 = 0.0f; Vector2 v2 = b2.LinearVelocityInternal; float w2 = b2.AngularVelocityInternal; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.Equal) { float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.dt * _maxMotorForce; _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; Vector2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= InvMassA * P; w1 -= InvIA * L1; v2 += InvMassB * P; w2 += InvIB * L2; } Vector2 Cdot1 = new Vector2(Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1, w2 - w1); if (_enableLimit && _limitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLower) { _impulse.Z = Math.Max(_impulse.Z, 0.0f); } else if (_limitState == LimitState.AtUpper) { _impulse.Z = Math.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) Vector2 b = -Cdot1 - (_impulse.Z - f1.Z) * new Vector2(_K.col3.X, _K.col3.Y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.X, f1.Y); _impulse.X = f2r.X; _impulse.Y = f2r.Y; df = _impulse - f1; Vector2 P = df.X * _perp + df.Z * _axis; float L2 = df.X * _s2 + df.Y + df.Z * _a2; v2 += InvMassB * P; w2 += InvIB * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vector2 df = _K.Solve22(-Cdot1); _impulse.X += df.X; _impulse.Y += df.Y; Vector2 P = df.X * _perp; float L2 = df.X * _s2 + df.Y; v2 += InvMassB * P; w2 += InvIB * L2; } b2.LinearVelocityInternal = v2; b2.AngularVelocityInternal = w2; }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 v1 = b1._linearVelocity; float w1 = b1._angularVelocity; Vector2 v2 = b2._linearVelocity; float w2 = b2._angularVelocity; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; float impulse = _motorMass * (_motorSpeed - Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.Dt * _maxMotorForce; _motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; Vector2 P = impulse * _axis; float L1 = impulse * _a1; float L2 = impulse * _a2; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } Vector2 Cdot1; Cdot1.x = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1; Cdot1.y = w2 - w1; if (_enableLimit && _limitState != LimitState.InactiveLimit) { // Solve prismatic and limit constraint in block form. float Cdot2; Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1; Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2); Vector3 f1 = _impulse; Vector3 df = _K.Solve33(-Cdot); _impulse += df; if (_limitState == LimitState.AtLowerLimit) { _impulse.z = Mathf.Max(_impulse.z, 0.0f); } else if (_limitState == LimitState.AtUpperLimit) { _impulse.z = Mathf.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) Vector2 b = -Cdot1 - (_impulse.z - f1.z) * new Vector2(_K.Col3.x, _K.Col3.y); Vector2 f2r = _K.Solve22(b) + new Vector2(f1.x, f1.y); _impulse.x = f2r.x; _impulse.y = f2r.y; df = _impulse - f1; Vector2 P = df.x * _perp + df.z * _axis; float L1 = df.x * _s1 + df.y + df.z * _a1; float L2 = df.x * _s2 + df.y + df.z * _a2; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vector2 df = _K.Solve22(-Cdot1); _impulse.x += df.x; _impulse.y += df.y; Vector2 P = df.x * _perp; float L1 = df.x * _s1 + df.y; float L2 = df.x * _s2 + df.y; v1 -= _invMass1 * P; w1 -= _invI1 * L1; v2 += _invMass2 * P; w2 += _invI2 * L2; } b1._linearVelocity = v1; b1._angularVelocity = w1; b2._linearVelocity = v2; b2._angularVelocity = w2; }