public override bool SolvePositionConstraints(b2SolverData data) { b2Vec2 cA = m_bodyA.InternalPosition.c; float aA = m_bodyA.InternalPosition.a; b2Vec2 cB = m_bodyB.InternalPosition.c; float aB = m_bodyB.InternalPosition.a; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); float positionError, angularError; b2Vec3 ex = new b2Vec3(); b2Vec3 ey = new b2Vec3(); b2Vec3 ez = new b2Vec3(); ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; ez.x = -rA.y * iA - rB.y * iB; ex.y = ey.x; ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; ez.y = rA.x * iA + rB.x * iB; ex.z = ez.x; ey.z = ez.y; ez.z = iA + iB; b2Mat33 K = new b2Mat33(ex, ey, ez); if (m_frequencyHz > 0.0f) { b2Vec2 C1 = cB + rB - cA - rA; positionError = C1.Length; angularError = 0.0f; b2Vec2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * b2Math.b2Cross(rA, P); cB += mB * P; aB += iB * b2Math.b2Cross(rB, P); } else { b2Vec2 C1 = cB + rB - cA - rA; float C2 = aB - aA - m_referenceAngle; positionError = C1.Length; angularError = b2Math.b2Abs(C2); b2Vec3 C = new b2Vec3(C1.x, C1.y, C2); b2Vec3 impulse = -K.Solve33(C); b2Vec2 P = new b2Vec2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (b2Math.b2Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (b2Math.b2Cross(rB, P) + impulse.z); } m_bodyA.InternalPosition.c = cA; m_bodyA.InternalPosition.a = aA; m_bodyB.InternalPosition.c = cB; m_bodyB.InternalPosition.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; }