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 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); } 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); }
public override bool SolvePositionConstraints(float baumgarte) { // TODO_ERIN block solve with limit float oldLimitImpulse; float C; b2Mat22 tMat; b2Body bA = m_bodyA; b2Body bB = m_bodyB; float angularError = 0.0f; float positionError = 0.0f; float tX; float impulseX; float impulseY; // Solve angular limit constraint. if (m_enableLimit && m_limitState != e_inactiveLimit) { float angle = bB.m_sweep.a - bA.m_sweep.a - m_referenceAngle; float limitImpulse = 0.0f; if (m_limitState == e_equalLimits) { // Prevent large angular corrections C = b2Math.Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = b2Math.Abs(C); } else if (m_limitState == e_atLowerLimit) { C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = b2Math.Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == e_atUpperLimit) { C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = b2Math.Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; } bA.m_sweep.a -= bA.m_invI * limitImpulse; bB.m_sweep.a += bB.m_invI * limitImpulse; bA.SynchronizeTransform(); bB.SynchronizeTransform(); } // Solve point-to-point constraint { //b2Vec2 r1 = b2Mul(bA->m_xf.R, m_localAnchor1 - bA->GetLocalCenter()); tMat = bA.m_xf.R; float r1X = m_localAnchor1.x - bA.m_sweep.localCenter.x; float r1Y = m_localAnchor1.y - bA.m_sweep.localCenter.y; tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y); r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y); r1X = tX; //b2Vec2 r2 = b2Mul(bB->m_xf.R, m_localAnchor2 - bB->GetLocalCenter()); tMat = bB.m_xf.R; float r2X = m_localAnchor2.x - bB.m_sweep.localCenter.x; float r2Y = m_localAnchor2.y - bB.m_sweep.localCenter.y; tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y); r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y); r2X = tX; //b2Vec2 C = bB->m_sweep.c + r2 - bA->m_sweep.c - r1; float CX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X; float CY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y; float CLengthSquared = CX * CX + CY * CY; float CLength = Mathf.Sqrt(CLengthSquared); positionError = CLength; float invMass1 = bA.m_invMass; float invMass2 = bB.m_invMass; float invI1 = bA.m_invI; float invI2 = bB.m_invI; //Handle large detachment. const float k_allowedStretch = 10.0f * b2Settings.b2_linearSlop; if (CLengthSquared > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation) //b2Vec2 u = C; u.Normalize(); float uX = CX / CLength; float uY = CY / CLength; float k = invMass1 + invMass2; //b2Settings.b2Assert(k>Number.MIN_VALUE) float m = 1.0f / k; impulseX = m * (-CX); impulseY = m * (-CY); const float k_beta = 0.5f; bA.m_sweep.c.x -= k_beta * invMass1 * impulseX; bA.m_sweep.c.y -= k_beta * invMass1 * impulseY; bB.m_sweep.c.x += k_beta * invMass2 * impulseX; bB.m_sweep.c.y += k_beta * invMass2 * impulseY; //C = bB->m_sweep.c + r2 - bA->m_sweep.c - r1; CX = bB.m_sweep.c.x + r2X - bA.m_sweep.c.x - r1X; CY = bB.m_sweep.c.y + r2Y - bA.m_sweep.c.y - r1Y; } //b2Mat22 K1; K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f; K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2; //b2Mat22 K2; K2.col1.x = invI1 * r1Y * r1Y; K2.col2.x = -invI1 * r1X * r1Y; K2.col1.y = -invI1 * r1X * r1Y; K2.col2.y = invI1 * r1X * r1X; //b2Mat22 K3; K3.col1.x = invI2 * r2Y * r2Y; K3.col2.x = -invI2 * r2X * r2Y; K3.col1.y = -invI2 * r2X * r2Y; K3.col2.y = invI2 * r2X * r2X; //b2Mat22 K = K1 + K2 + K3; K.SetM(K1); K.AddM(K2); K.AddM(K3); //b2Vec2 impulse = K.Solve(-C); K.Solve(tImpulse, -CX, -CY); impulseX = tImpulse.x; impulseY = tImpulse.y; //bA.m_sweep.c -= bA.m_invMass * impulse; bA.m_sweep.c.x -= bA.m_invMass * impulseX; bA.m_sweep.c.y -= bA.m_invMass * impulseY; //bA.m_sweep.a -= bA.m_invI * b2Cross(r1, impulse); bA.m_sweep.a -= bA.m_invI * (r1X * impulseY - r1Y * impulseX); //bB.m_sweep.c += bB.m_invMass * impulse; bB.m_sweep.c.x += bB.m_invMass * impulseX; bB.m_sweep.c.y += bB.m_invMass * impulseY; //bB.m_sweep.a += bB.m_invI * b2Cross(r2, impulse); bB.m_sweep.a += bB.m_invI * (r2X * impulseY - r2Y * impulseX); bA.SynchronizeTransform(); bB.SynchronizeTransform(); } return(positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop); }