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); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d = (cB - cA) + rB - rA; Vector2 ay = Complex.Multiply(ref _localYAxis, ref qA); float sAy = MathUtils.Cross(d + rA, ay); float sBy = MathUtils.Cross(ref rB, ref ay); float C = Vector2.Dot(d, ay); float k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy; float impulse; if (k != 0.0f) { impulse = -C / k; } else { impulse = 0.0f; } Vector2 P = impulse * ay; float LA = impulse * sAy; float LB = impulse * sBy; cA -= _invMassA * P; aA -= _invIA * LA; cB += _invMassB * P; aB += _invIB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(Math.Abs(C) <= Settings.LinearSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { if (Frequency > 0.0f) { // There is no position correction for soft distance constraints. return(true); } 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); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 u = cB + rB - cA - rA; float length = u.Length(); u = Vector2.Normalize(u); float C = length - Length; C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); float impulse = -_mass * C; Vector2 P = impulse * u; cA -= _invMassA * P; aA -= _invIA * MathUtils.Cross(ref rA, ref P); cB += _invMassB * P; aB += _invIB * MathUtils.Cross(ref rB, ref P); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(Math.Abs(C) < Settings.LinearSlop); }
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; Vector2 cC = data.positions[_indexC].c; float aC = data.positions[_indexC].a; Vector2 cD = data.positions[_indexD].c; float aD = data.positions[_indexD].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Complex qC = Complex.FromAngle(aC); Complex qD = Complex.FromAngle(aD); const float linearError = 0.0f; float coordinateA, coordinateB; Vector2 JvAC, JvBD; float JwA, JwB, JwC, JwD; float mass = 0.0f; if (_typeA == JointType.Revolute) { JvAC = Vector2.Zero; JwA = 1.0f; JwC = 1.0f; mass += _iA + _iC; coordinateA = aA - aC - _referenceAngleA; } else { Vector2 u = Complex.Multiply(ref _localAxisC, ref qC); Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC); Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA); JvAC = u; JwC = MathUtils.Cross(ref rC, ref u); JwA = MathUtils.Cross(ref rA, ref u); mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; Vector2 pC = _localAnchorC - _lcC; Vector2 pA = Complex.Divide(rA + (cA - cC), ref qC); coordinateA = Vector2.Dot(pA - pC, _localAxisC); } if (_typeB == JointType.Revolute) { JvBD = Vector2.Zero; JwB = _ratio; JwD = _ratio; mass += _ratio * _ratio * (_iB + _iD); coordinateB = aB - aD - _referenceAngleB; } else { Vector2 u = Complex.Multiply(ref _localAxisD, ref qD); Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD); Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB); JvBD = _ratio * u; JwD = _ratio * MathUtils.Cross(ref rD, ref u); JwB = _ratio * MathUtils.Cross(ref rB, ref u); mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; Vector2 pD = _localAnchorD - _lcD; Vector2 pB = Complex.Divide(rB + (cB - cD), ref qD); coordinateB = Vector2.Dot(pB - pD, _localAxisD); } float C = (coordinateA + _ratio * coordinateB) - _constant; float impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; } cA += _mA * impulse * JvAC; aA += _iA * impulse * JwA; cB += _mB * impulse * JvBD; aB += _iB * impulse * JwB; cC -= _mC * impulse * JvAC; aC -= _iC * impulse * JwC; cD -= _mD * impulse * JvBD; aD -= _iD * impulse * JwD; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; data.positions[_indexC].c = cC; data.positions[_indexC].a = aC; data.positions[_indexD].c = cD; data.positions[_indexD].a = aD; // TODO_ERIN not implemented return(linearError < Settings.LinearSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = _bodyA.IslandIndex; _indexB = _bodyB.IslandIndex; _indexC = _bodyC.IslandIndex; _indexD = _bodyD.IslandIndex; _lcA = _bodyA._sweep.LocalCenter; _lcB = _bodyB._sweep.LocalCenter; _lcC = _bodyC._sweep.LocalCenter; _lcD = _bodyD._sweep.LocalCenter; _mA = _bodyA._invMass; _mB = _bodyB._invMass; _mC = _bodyC._invMass; _mD = _bodyD._invMass; _iA = _bodyA._invI; _iB = _bodyB._invI; _iC = _bodyC._invI; _iD = _bodyD._invI; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; float aC = data.positions[_indexC].a; Vector2 vC = data.velocities[_indexC].v; float wC = data.velocities[_indexC].w; float aD = data.positions[_indexD].a; Vector2 vD = data.velocities[_indexD].v; float wD = data.velocities[_indexD].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Complex qC = Complex.FromAngle(aC); Complex qD = Complex.FromAngle(aD); _mass = 0.0f; if (_typeA == JointType.Revolute) { _JvAC = Vector2.Zero; _JwA = 1.0f; _JwC = 1.0f; _mass += _iA + _iC; } else { Vector2 u = Complex.Multiply(ref _localAxisC, ref qC); Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC); Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA); _JvAC = u; _JwC = MathUtils.Cross(ref rC, ref u); _JwA = MathUtils.Cross(ref rA, ref u); _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA; } if (_typeB == JointType.Revolute) { _JvBD = Vector2.Zero; _JwB = _ratio; _JwD = _ratio; _mass += _ratio * _ratio * (_iB + _iD); } else { Vector2 u = Complex.Multiply(ref _localAxisD, ref qD); Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD); Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB); _JvBD = _ratio * u; _JwD = _ratio * MathUtils.Cross(ref rD, ref u); _JwB = _ratio * MathUtils.Cross(ref rB, ref u); _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB; } // Compute effective mass. _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f; if (data.step.warmStarting) { vA += (_mA * _impulse) * _JvAC; wA += _iA * _impulse * _JwA; vB += (_mB * _impulse) * _JvBD; wB += _iB * _impulse * _JwB; vC -= (_mC * _impulse) * _JvAC; wC -= _iC * _impulse * _JwC; vD -= (_mD * _impulse) * _JvBD; wD -= _iD * _impulse * _JwD; } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; data.velocities[_indexC].v = vC; data.velocities[_indexC].w = wC; data.velocities[_indexD].v = vD; data.velocities[_indexD].w = wD; }
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; float angularError = 0.0f; float positionError; bool fixedRotation = (_invIA + _invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false) { float angle = aB - aA - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } aA -= _invIA * limitImpulse; aB += _invIB * limitImpulse; } // Solve point-to-point constraint. { Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = _invMassA, mB = _invMassB; float 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; Vector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(ref rA, ref impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(ref rB, ref impulse); } 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 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; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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] float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; bool fixedRotation = (iA + iB == 0.0f); _mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB; _mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB; _mass.ez.X = -_rA.Y * iA - _rB.Y * iB; _mass.ex.Y = _mass.ey.X; _mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB; _mass.ez.Y = _rA.X * iA + _rB.X * iB; _mass.ex.Z = _mass.ez.X; _mass.ey.Z = _mass.ez.Y; _mass.ez.Z = iA + iB; _motorMass = iA + iB; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } if (_enableMotor == false || fixedRotation) { _motorImpulse = 0.0f; } if (_enableLimit && fixedRotation == false) { float jointAngle = aB - aA - ReferenceAngle; if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) { _limitState = LimitState.Equal; } else if (jointAngle <= _lowerAngle) { if (_limitState != LimitState.AtLower) { _impulse.Z = 0.0f; } _limitState = LimitState.AtLower; } else if (jointAngle >= _upperAngle) { if (_limitState != LimitState.AtUpper) { _impulse.Z = 0.0f; } _limitState = LimitState.AtUpper; } else { _limitState = LimitState.Inactive; _impulse.Z = 0.0f; } } else { _limitState = LimitState.Inactive; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(ref _rA, ref P) + MotorImpulse + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref P) + MotorImpulse + _impulse.Z); } else { _impulse = Vector3.Zero; _motorImpulse = 0.0f; } 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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // Get the pulley axes. Vector2 uA = cA + rA - WorldAnchorA; Vector2 uB = cB + rB - WorldAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(ref rA, ref uA); float ruB = MathUtils.Cross(ref rB, ref uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; float mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = Constant - lengthA - Ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; Vector2 PA = -impulse * uA; Vector2 PB = -Ratio * impulse * uB; cA += _invMassA * PA; aA += _invIA * MathUtils.Cross(ref rA, ref PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(ref rB, ref PB); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(linearError < Settings.LinearSlop); }
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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // Get the pulley axes. _uA = cA + _rA - WorldAnchorA; _uB = cB + _rB - WorldAnchorB; float lengthA = _uA.Length(); float lengthB = _uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { _uA *= 1.0f / lengthA; } else { _uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { _uB *= 1.0f / lengthB; } else { _uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(ref _rA, ref _uA); float ruB = MathUtils.Cross(ref _rB, ref _uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; _mass = mA + Ratio * Ratio * mB; if (_mass > 0.0f) { _mass = 1.0f / _mass; } if (data.step.warmStarting) { // Scale impulses to support variable time steps. _impulse *= data.step.dtRatio; // Warm starting. Vector2 PA = -(_impulse) * _uA; Vector2 PB = (-Ratio * _impulse) * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(ref _rA, ref PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(ref _rB, ref PB); } else { _impulse = 0.0f; } 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; 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 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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); _u = cB + _rB - cA - _rA; // Handle singularity. float length = _u.Length(); if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = Vector2.Zero; } float crAu = MathUtils.Cross(ref _rA, ref _u); float crBu = MathUtils.Cross(ref _rB, ref _u); float invMass = _invMassA + _invIA * crAu * crAu + _invMassB + _invIB * crBu * crBu; // Compute the effective mass matrix. _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Frequency > 0.0f) { float C = length - Length; // Frequency float omega = Constant.Tau * Frequency; // Damping coefficient float d = 2.0f * _mass * DampingRatio * omega; // Spring stiffness float k = _mass * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invMass += _gamma; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; } else { _gamma = 0.0f; _bias = 0.0f; } if (data.step.warmStarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(ref _rA, ref P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(ref _rB, ref P); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _invMassA = BodyA._invMass; _invIA = BodyA._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Complex qA = Complex.FromAngle(aA); float mass = BodyA.Mass; // Frequency float omega = Constant.Tau * Frequency; // Damping coefficient float d = 2.0f * mass * DampingRatio * omega; // Spring stiffness float k = mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. float h = data.step.dt; Debug.Assert(d + h * k > Settings.Epsilon); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y] // [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X] Mat22 K = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; if (data.step.warmStarting) { _impulse *= data.step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(ref _rA, ref _impulse); } else { _impulse = Vector2.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
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 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; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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] float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; 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) { K.GetInverse22(ref _mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - ReferenceAngle; // Frequency float omega = Constant.Tau * FrequencyHz; // Damping coefficient float d = 2.0f * m * DampingRatio * omega; // Spring stiffness float k = m * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * h * k * _gamma; invM += _gamma; _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else if (K.ez.Z == 0.0f) { K.GetInverse22(ref _mass); _gamma = 0.0f; _bias = 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref P) + _impulse.Z); } else { _impulse = Vector3.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); // Compute the effective masses. Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d1 = cB + rB - cA - rA; // Point to line constraint { _ay = Complex.Multiply(ref _localYAxis, ref qA); _sAy = MathUtils.Cross(d1 + rA, _ay); _sBy = MathUtils.Cross(ref rB, ref _ay); _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; if (_mass > 0.0f) { _mass = 1.0f / _mass; } } // Spring constraint _springMass = 0.0f; _bias = 0.0f; _gamma = 0.0f; if (Frequency > 0.0f) { _ax = Complex.Multiply(ref _localXAxis, ref qA); _sAx = MathUtils.Cross(d1 + rA, _ax); _sBx = MathUtils.Cross(ref rB, ref _ax); float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx; if (invMass > 0.0f) { _springMass = 1.0f / invMass; float C = Vector2.Dot(d1, _ax); // Frequency float omega = Constant.Tau * Frequency; // Damping coefficient float d = 2.0f * _springMass * DampingRatio * omega; // Spring stiffness float k = _springMass * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); if (_gamma > 0.0f) { _gamma = 1.0f / _gamma; } _bias = C * h * k * _gamma; _springMass = invMass + _gamma; if (_springMass > 0.0f) { _springMass = 1.0f / _springMass; } } } else { _springImpulse = 0.0f; } // Rotational motor if (_enableMotor) { _motorMass = iA + iB; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } else { _motorMass = 0.0f; _motorImpulse = 0.0f; } if (data.step.warmStarting) { // Account for variable time step. _impulse *= data.step.dtRatio; _springImpulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; Vector2 P = _impulse * _ay + _springImpulse * _ax; float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; vA -= _invMassA * P; wA -= _invIA * LA; vB += _invMassB * P; wB += _invIB * LB; } else { _impulse = 0.0f; _springImpulse = 0.0f; _motorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); // Compute the effective mass matrix. _rA = -Complex.Multiply(ref _localCenterA, ref qA); _rB = -Complex.Multiply(ref _localCenterB, ref qB); // 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] float mA = _invMassA, mB = _invMassB; float 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 - Complex.Multiply(ref _linearOffset, ref qA); _angularError = aB - aA - _angularOffset; if (data.step.warmStarting) { // Scale impulses to support a variable time step. _linearImpulse *= data.step.dtRatio; _angularImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(ref _rA, ref P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); _u = cB + _rB - cA - _rA; _length = _u.Length(); float C = _length - MaxLength; if (C > 0.0f) { State = LimitState.AtUpper; } else { State = LimitState.Inactive; } if (_length > Settings.LinearSlop) { _u *= 1.0f / _length; } else { _u = Vector2.Zero; _mass = 0.0f; _impulse = 0.0f; return; } // Compute effective mass. float crA = MathUtils.Cross(ref _rA, ref _u); float crB = MathUtils.Cross(ref _rB, ref _u); float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (data.step.warmStarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(ref _rA, ref P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(ref _rB, ref P); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); // Compute the effective masses. Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d = (cB - cA) + rB - rA; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute motor Jacobian and effective mass. { _axis = Complex.Multiply(ref _localXAxis, ref qA); _a1 = MathUtils.Cross(d + rA, _axis); _a2 = MathUtils.Cross(ref rB, ref _axis); _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } // Prismatic constraint. { _perp = Complex.Multiply(ref _localYAxisA, ref qA); _s1 = MathUtils.Cross(d + rA, _perp); _s2 = MathUtils.Cross(ref rB, ref _perp); 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 bodies with fixed rotation. k22 = 1.0f; } float k23 = iA * _a1 + iB * _a2; float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; _K.ex = new Vector3(k11, k12, k13); _K.ey = new Vector3(k12, k22, k23); _K.ez = new Vector3(k13, k23, k33); } // Compute motor and limit terms. if (_enableLimit) { float jointTranslation = Vector2.Dot(_axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { _limitState = LimitState.Equal; } else if (jointTranslation <= _lowerTranslation) { if (_limitState != LimitState.AtLower) { _limitState = LimitState.AtLower; _impulse.Z = 0.0f; } } else if (jointTranslation >= _upperTranslation) { if (_limitState != LimitState.AtUpper) { _limitState = LimitState.AtUpper; _impulse.Z = 0.0f; } } else { _limitState = LimitState.Inactive; _impulse.Z = 0.0f; } } else { _limitState = LimitState.Inactive; _impulse.Z = 0.0f; } if (_enableMotor == false) { MotorImpulse = 0.0f; } if (data.step.warmStarting) { // Account for variable time step. _impulse *= data.step.dtRatio; MotorImpulse *= data.step.dtRatio; Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis; float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1; float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { _impulse = Vector3.Zero; MotorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }