public override bool SolvePositionConstraints(b2SolverData data) { b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(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 != b2LimitState.e_inactiveLimit && fixedRotation == false) { float angle = aB - aA - m_referenceAngle; float limitImpulse = 0.0f; if (m_limitState == b2LimitState.e_equalLimits) { // Prevent large angular corrections float C = b2Math.b2Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = b2Math.b2Abs(C); } else if (m_limitState == b2LimitState.e_atLowerLimit) { float C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = b2Math.b2Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == b2LimitState.e_atUpperLimit) { float C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = b2Math.b2Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; } aA -= m_invIA * limitImpulse; aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; b2Mat22 K = new b2Mat22(); K.exx = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; K.exy = -iA * rA.x * rA.y - iB * rB.x * rB.y; K.eyx = K.ex.y; K.eyy = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; b2Vec2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * b2Math.b2Cross(rA, impulse); cB += mB * impulse; aB += iB * b2Math.b2Cross(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 <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop; }
public override bool SolvePositionConstraints(b2SolverData data) { b2Vec2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); float mA = InvertedMassA, mB = InvertedMassB; float iA = InvertedIA, iB = InvertedIB; // Compute fresh Jacobians b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = cB + rB - cA - rA; b2Vec2 axis = b2Math.b2Mul(qA, m_localXAxisA); float a1 = b2Math.b2Cross(d + rA, axis); float a2 = b2Math.b2Cross(rB, axis); b2Vec2 perp = b2Math.b2Mul(qA, m_localYAxisA); float s1 = b2Math.b2Cross(d + rA, perp); float s2 = b2Math.b2Cross(rB, perp); b2Vec3 impulse; b2Vec2 C1 = new b2Vec2(); C1.x = b2Math.b2Dot(perp, d); C1.y = aB - aA - m_referenceAngle; float linearError = b2Math.b2Abs(C1.x); float angularError = b2Math.b2Abs(C1.y); bool active = false; float C2 = 0.0f; if (m_enableLimit) { float translation = b2Math.b2Dot(axis, d); if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop) { // Prevent large angular corrections C2 = b2Math.b2Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection); linearError = Math.Max(linearError, b2Math.b2Abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Math.b2Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_maxLinearCorrection, 0.0f); linearError = Math.Max(linearError, m_lowerTranslation - translation); active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Math.b2Clamp(translation - m_upperTranslation - b2Settings.b2_linearSlop, 0.0f, b2Settings.b2_maxLinearCorrection); linearError = Math.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; b2Mat33 K = new b2Mat33( new b2Vec3(k11, k12, k13), new b2Vec3(k12, k22, k23), new b2Vec3(k13, k23, k33)); b2Vec3 C = new b2Vec3(C1.x, C1.y, 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; } b2Mat22 K = new b2Mat22(); K.ex.Set(k11, k12); K.ey.Set(k12, k22); b2Vec2 impulse1 = K.Solve(-C1); impulse = new b2Vec3(); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } b2Vec2 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[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return linearError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop; }