internal override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); float angularError = 0.0f; float positionError = 0.0f; bool fixedRotation = (m_invIA + m_invIB == 0.0f); // Solve angular limit constraint. if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false) { float angle = aB - aA - m_referenceAngle; float limitImpulse = 0.0f; if (m_limitState == LimitState.e_equalLimits) { // Prevent large angular corrections float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = Math.Abs(C); } else if (m_limitState == LimitState.e_atLowerLimit) { float C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == LimitState.e_atUpperLimit) { float C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection); limitImpulse = -m_motorMass * C; } aA -= m_invIA * limitImpulse; aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB); Vec2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Mat22 K; 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; Vec2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * Utilities.Cross(rA, impulse); cB += mB * impulse; aB += iB * Utilities.Cross(rB, impulse); } 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 bool SolvePositionConstraints(SolverData data){ Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA); Rot qB = new Rot(aB); float angularError = 0.0f; float positionError = 0.0f; bool fixedRotation = (m_invIA + m_invIB == 0.0f); // Solve angular limit constraint. if (m_enableLimit && m_limitState != LimitState.e_inactiveLimit && fixedRotation == false) { float angle = aB - aA - m_referenceAngle; float limitImpulse = 0.0f; if (m_limitState ==LimitState.e_equalLimits) { // Prevent large angular corrections float C = Utilities.Clamp(angle - m_lowerAngle, -Settings._maxAngularCorrection, Settings._maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = Math.Abs(C); } else if (m_limitState == LimitState.e_atLowerLimit) { float C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = Utilities.Clamp(C + Settings._angularSlop, -Settings._maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == LimitState.e_atUpperLimit) { float C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = Utilities.Clamp(C - Settings._angularSlop, 0.0f, Settings._maxAngularCorrection); limitImpulse = -m_motorMass * C; } aA -= m_invIA * limitImpulse; aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); Vec2 rA = Utilities.Mul(qA, m_localAnchorA - m_localCenterA); Vec2 rB = Utilities.Mul(qB, m_localAnchorB - m_localCenterB); Vec2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Mat22 K; 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; Vec2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * Utilities.Cross(rA, impulse); cB += mB * impulse; aB += iB * Utilities.Cross(rB, impulse); } 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; }
/// Set this based on the position and angle. public void Set(Vec2 position, float angle) { p = position; q.Set(angle); }