bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // Get the pulley axes. b2Vec2 uA = cA + rA - m_groundAnchorA; b2Vec2 uB = cB + rB - m_groundAnchorB; float32 lengthA = uA.Length(); float32 lengthB = uB.Length(); if (lengthA > 10.0f * b2_linearSlop) { uA *= 1.0f / lengthA; } else { uA.SetZero(); } if (lengthB > 10.0f * b2_linearSlop) { uB *= 1.0f / lengthB; } else { uB.SetZero(); } // Compute effective mass. float32 ruA = b2Cross(rA, uA); float32 ruB = b2Cross(rB, uB); float32 mA = m_invMassA + m_invIA * ruA * ruA; float32 mB = m_invMassB + m_invIB * ruB * ruB; float32 mass = mA + m_ratio * m_ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float32 C = m_constant - lengthA - m_ratio * lengthB; float32 linearError = b2Abs(C); float32 impulse = -mass * C; b2Vec2 PA = -impulse * uA; b2Vec2 PB = -m_ratio * impulse * uB; cA += m_invMassA * PA; aA += m_invIA * b2Cross(rA, PA); cB += m_invMassB * PB; aB += m_invIB * b2Cross(rB, PB); 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 < b2_linearSlop; }
void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); // Compute the effective mass matrix. m_rA = b2Mul(qA, -m_localCenterA); m_rB = b2Mul(qB, -m_localCenterB); // 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] float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; b2Mat22 K; K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; K.ey.x = K.ex.y; K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; m_linearMass = K.GetInverse(); m_angularMass = iA + iB; if (m_angularMass > 0.0f) { m_angularMass = 1.0f / m_angularMass; } m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); m_angularError = aB - aA - m_angularOffset; if (data.step.warmStarting) { // Scale impulses to support a variable time step. m_linearImpulse *= data.step.dtRatio; m_angularImpulse *= data.step.dtRatio; b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); vA -= mA * P; wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); vB += mB * P; wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); } else { m_linearImpulse.SetZero(); m_angularImpulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_indexC = m_bodyC->m_islandIndex; m_indexD = m_bodyD->m_islandIndex; m_lcA = m_bodyA->m_sweep.localCenter; m_lcB = m_bodyB->m_sweep.localCenter; m_lcC = m_bodyC->m_sweep.localCenter; m_lcD = m_bodyD->m_sweep.localCenter; m_mA = m_bodyA->m_invMass; m_mB = m_bodyB->m_invMass; m_mC = m_bodyC->m_invMass; m_mD = m_bodyD->m_invMass; m_iA = m_bodyA->m_invI; m_iB = m_bodyB->m_invI; m_iC = m_bodyC->m_invI; m_iD = m_bodyD->m_invI; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; float32 aC = data.positions[m_indexC].a; b2Vec2 vC = data.velocities[m_indexC].v; float32 wC = data.velocities[m_indexC].w; float32 aD = data.positions[m_indexD].a; b2Vec2 vD = data.velocities[m_indexD].v; float32 wD = data.velocities[m_indexD].w; b2Rot qA(aA), qB(aB), qC(aC), qD(aD); m_mass = 0.0f; if (m_typeA == e_revoluteJoint) { m_JvAC.SetZero(); m_JwA = 1.0f; m_JwC = 1.0f; m_mass += m_iA + m_iC; } else { b2Vec2 u = b2Mul(qC, m_localAxisC); b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); m_JvAC = u; m_JwC = b2Cross(rC, u); m_JwA = b2Cross(rA, u); m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; } if (m_typeB == e_revoluteJoint) { m_JvBD.SetZero(); m_JwB = m_ratio; m_JwD = m_ratio; m_mass += m_ratio * m_ratio * (m_iB + m_iD); } else { b2Vec2 u = b2Mul(qD, m_localAxisD); b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); m_JvBD = m_ratio * u; m_JwD = m_ratio * b2Cross(rD, u); m_JwB = m_ratio * b2Cross(rB, u); m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; } // Compute effective mass. m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; if (data.step.warmStarting) { vA += (m_mA * m_impulse) * m_JvAC; wA += m_iA * m_impulse * m_JwA; vB += (m_mB * m_impulse) * m_JvBD; wB += m_iB * m_impulse * m_JwB; vC -= (m_mC * m_impulse) * m_JvAC; wC -= m_iC * m_impulse * m_JwC; vD -= (m_mD * m_impulse) * m_JvBD; wD -= m_iD * m_impulse * m_JwD; } else { m_impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; data.velocities[m_indexC].v = vC; data.velocities[m_indexC].w = wC; data.velocities[m_indexD].v = vD; data.velocities[m_indexD].w = wD; }
float32 Initialize(const b2SimplexCache* cache, const b2DistanceProxy* proxyA, const b2Sweep& sweepA, const b2DistanceProxy* proxyB, const b2Sweep& sweepB, float32 t1) { m_proxyA = proxyA; m_proxyB = proxyB; int32 count = cache->count; b2Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; b2Transform xfA, xfB; m_sweepA.GetTransform(&xfA, t1); m_sweepB.GetTransform(&xfB, t1); if (count == 1) { m_type = e_points; b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); b2Vec2 pointA = b2Mul(xfA, localPointA); b2Vec2 pointB = b2Mul(xfB, localPointB); m_axis = pointB - pointA; float32 s = m_axis.Normalize(); return s; } else if (cache->indexA[0] == cache->indexA[1]) { // Two points on B and one on A. m_type = e_faceB; b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); b2Vec2 normal = b2Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); b2Vec2 pointB = b2Mul(xfB, m_localPoint); b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); b2Vec2 pointA = b2Mul(xfA, localPointA); float32 s = b2Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } else { // Two points on A and one or two points on B. m_type = e_faceA; b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); b2Vec2 normal = b2Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); b2Vec2 pointA = b2Mul(xfA, m_localPoint); b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); b2Vec2 pointB = b2Mul(xfB, localPointB); float32 s = b2Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } }
bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); float32 angularError = 0.0f; float32 positionError = 0.0f; bool fixedRotation = (m_invIA + m_invIB == 0.0f); // Solve angular limit constraint. if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) { float32 angle = aB - aA - m_referenceAngle; float32 limitImpulse = 0.0f; if (m_limitState == e_equalLimits) { // Prevent large angular corrections float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = b2Abs(C); } else if (m_limitState == e_atLowerLimit) { float32 C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == e_atUpperLimit) { float32 C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = b2Clamp(C - b2_angularSlop, 0.0f, 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 = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 C = cB + rB - cA - rA; positionError = C.Length(); float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; b2Mat22 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; b2Vec2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * b2Cross(rA, impulse); cB += mB * impulse; aB += iB * 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 <= b2_linearSlop && angularError <= b2_angularSlop; }
bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte) { // TODO_ERIN block solve with limit. B2_NOT_USED(baumgarte); b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; float32 angularError = 0.0f; float32 positionError = 0.0f; // Solve angular limit constraint. if (m_enableLimit && m_limitState != e_inactiveLimit) { float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle; float32 limitImpulse = 0.0f; if (m_limitState == e_equalLimits) { // Prevent large angular corrections float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = b2Abs(C); } else if (m_limitState == e_atLowerLimit) { float32 C = angle - m_lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); limitImpulse = -m_motorMass * C; } else if (m_limitState == e_atUpperLimit) { float32 C = angle - m_upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); limitImpulse = -m_motorMass * C; } b1->m_sweep.a -= b1->m_invI * limitImpulse; b2->m_sweep.a += b2->m_invI * limitImpulse; b1->SynchronizeTransform(); b2->SynchronizeTransform(); } // Solve point-to-point constraint. { b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; positionError = C.Length(); float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass; float32 invI1 = b1->m_invI, invI2 = b2->m_invI; // Handle large detachment. const float32 k_allowedStretch = 10.0f * b2_linearSlop; if (C.LengthSquared() > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). b2Vec2 u = C; u.Normalize(); float32 m = invMass1 + invMass2; if (m > 0.0f) { m = 1.0f / m; } b2Vec2 impulse = m * (-C); const float32 k_beta = 0.5f; b1->m_sweep.c -= k_beta * invMass1 * impulse; b2->m_sweep.c += k_beta * invMass2 * impulse; C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; } 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 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y; K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x; b2Mat22 K3; K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y; K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x; b2Mat22 K = K1 + K2 + K3; b2Vec2 impulse = K.Solve(-C); b1->m_sweep.c -= b1->m_invMass * impulse; b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse); b2->m_sweep.c += b2->m_invMass * impulse; b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse); b1->SynchronizeTransform(); b2->SynchronizeTransform(); } return positionError <= b2_linearSlop && angularError <= b2_angularSlop; }
void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); // Compute the effective masses. b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = (cB - cA) + rB - rA; float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; // Compute motor Jacobian and effective mass. { m_axis = b2Mul(qA, m_localXAxisA); m_a1 = b2Cross(d + rA, m_axis); m_a2 = b2Cross(rB, m_axis); m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } } // Prismatic constraint. { m_perp = b2Mul(qA, m_localYAxisA); m_s1 = b2Cross(d + rA, m_perp); m_s2 = b2Cross(rB, m_perp); float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; float32 k12 = iA * m_s1 + iB * m_s2; float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; float32 k22 = iA + iB; if (k22 == 0.0f) { // For bodies with fixed rotation. k22 = 1.0f; } float32 k23 = iA * m_a1 + iB * m_a2; float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; m_K.ex.Set(k11, k12, k13); m_K.ey.Set(k12, k22, k23); m_K.ez.Set(k13, k23, k33); } // Compute motor and limit terms. if (m_enableLimit) { float32 jointTranslation = b2Dot(m_axis, d); if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { m_limitState = e_equalLimits; } else if (jointTranslation <= m_lowerTranslation) { if (m_limitState != e_atLowerLimit) { m_limitState = e_atLowerLimit; m_impulse.z = 0.0f; } } else if (jointTranslation >= m_upperTranslation) { if (m_limitState != e_atUpperLimit) { m_limitState = e_atUpperLimit; m_impulse.z = 0.0f; } } else { m_limitState = e_inactiveLimit; m_impulse.z = 0.0f; } } else { m_limitState = e_inactiveLimit; m_impulse.z = 0.0f; } if (m_enableMotor == false) { m_motorImpulse = 0.0f; } if (data.step.warmStarting) { // Account for variable time step. m_impulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { m_impulse.SetZero(); m_motorImpulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const { // Polygon mass, centroid, and inertia. // Let rho be the polygon density in mass per unit area. // Then: // mass = rho * int(dA) // centroid.x = (1/mass) * rho * int(x * dA) // centroid.y = (1/mass) * rho * int(y * dA) // I = rho * int((x*x + y*y) * dA) // // We can compute these integrals by summing all the integrals // for each triangle of the polygon. To evaluate the integral // for a single triangle, we make a change of variables to // the (u,v) coordinates of the triangle: // x = x0 + e1x * u + e2x * v // y = y0 + e1y * u + e2y * v // where 0 <= u && 0 <= v && u + v <= 1. // // We integrate u from [0,1-v] and then v from [0,1]. // We also need to use the Jacobian of the transformation: // D = cross(e1, e2) // // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) // // The rest of the derivation is handled by computer algebra. b2Assert(m_count >= 3); b2Vec2 center; center.Set(0.0f, 0.0f); float32 area = 0.0f; float32 I = 0.0f; // s is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). b2Vec2 s(0.0f, 0.0f); // This code would put the reference point inside the polygon. for (int32 i = 0; i < m_count; ++i) { s += m_vertices[i]; } s *= 1.0f / m_count; const float32 k_inv3 = 1.0f / 3.0f; for (int32 i = 0; i < m_count; ++i) { // Triangle vertices. b2Vec2 e1 = m_vertices[i] - s; b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; float32 D = b2Cross(e1, e2); float32 triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid center += triangleArea * k_inv3 * (e1 + e2); float32 ex1 = e1.x, ey1 = e1.y; float32 ex2 = e2.x, ey2 = e2.y; float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; I += (0.25f * k_inv3 * D) * (intx2 + inty2); } // Total mass massData->mass = density * area; // Center of mass b2Assert(area > b2_epsilon); center *= 1.0f / area; massData->center = center + s; // Inertia tensor relative to the local origin (point s). massData->I = density * I; // Shift to center of mass then to original body origin. massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); }
void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step) { b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; m_localCenterA = b1->GetLocalCenter(); m_localCenterB = b2->GetLocalCenter(); b2Transform xf1 = b1->GetTransform(); b2Transform xf2 = b2->GetTransform(); // Compute the effective masses. b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA); b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB); b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1; m_invMassA = b1->m_invMass; m_invIA = b1->m_invI; m_invMassB = b2->m_invMass; m_invIB = b2->m_invI; // Compute motor Jacobian and effective mass. { m_axis = b2Mul(xf1.R, m_localXAxis1); m_a1 = b2Cross(d + r1, m_axis); m_a2 = b2Cross(r2, m_axis); m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2; if (m_motorMass > b2_epsilon) { m_motorMass = 1.0f / m_motorMass; } } // Prismatic constraint. { m_perp = b2Mul(xf1.R, m_localYAxis1); m_s1 = b2Cross(d + r1, m_perp); m_s2 = b2Cross(r2, m_perp); float32 m1 = m_invMassA, m2 = m_invMassB; float32 i1 = m_invIA, i2 = m_invIB; float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; float32 k12 = i1 * m_s1 + i2 * m_s2; float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; float32 k22 = i1 + i2; float32 k23 = i1 * m_a1 + i2 * m_a2; float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; m_K.col1.Set(k11, k12, k13); m_K.col2.Set(k12, k22, k23); m_K.col3.Set(k13, k23, k33); } // Compute motor and limit terms. if (m_enableLimit) { float32 jointTranslation = b2Dot(m_axis, d); if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { m_limitState = e_equalLimits; } else if (jointTranslation <= m_lowerTranslation) { if (m_limitState != e_atLowerLimit) { m_limitState = e_atLowerLimit; m_impulse.z = 0.0f; } } else if (jointTranslation >= m_upperTranslation) { if (m_limitState != e_atUpperLimit) { m_limitState = e_atUpperLimit; m_impulse.z = 0.0f; } } else { m_limitState = e_inactiveLimit; m_impulse.z = 0.0f; } } else { m_limitState = e_inactiveLimit; m_impulse.z = 0.0f; } if (m_enableMotor == false) { m_motorImpulse = 0.0f; } if (step.warmStarting) { // Account for variable time step. m_impulse *= step.dtRatio; m_motorImpulse *= step.dtRatio; b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; float32 L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; float32 L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; b1->m_linearVelocity -= m_invMassA * P; b1->m_angularVelocity -= m_invIA * L1; b2->m_linearVelocity += m_invMassB * P; b2->m_angularVelocity += m_invIB * L2; } else { m_impulse.SetZero(); m_motorImpulse = 0.0f; } }
void b2Body::ResetMass() { // Compute mass data from shapes. Each shape has its own density. m_mass = 0.0f; m_invMass = 0.0f; m_I = 0.0f; m_invI = 0.0f; b2Vec2 center = b2Vec2_zero; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { const b2MassData& massData = f->GetMassData(); m_mass += massData.mass; center += massData.mass * massData.center; m_I += massData.I; } // Compute center of mass. if (m_mass > 0.0f) { m_invMass = 1.0f / m_mass; center *= m_invMass; } if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) { // Center the inertia about the center of mass. m_I -= m_mass * b2Dot(center, center); b2Assert(m_I > 0.0f); m_invI = 1.0f / m_I; } else { m_I = 0.0f; m_invI = 0.0f; } // Move center of mass. b2Vec2 oldCenter = m_sweep.c; m_sweep.localCenter = center; m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); // Update center of mass velocity. m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); // Determine the new body type. int16 oldType = m_type; if (m_invMass == 0.0f && m_invI == 0.0f) { m_type = e_staticType; } else { m_type = e_dynamicType; } // If the body type changed, we need to flag contacts for filtering. if (oldType != m_type) { for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next) { ce->contact->FlagForFiltering(); } } }
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { b2Assert(3 <= count && count <= b2_maxPolygonVertices); if (count < 3) { SetAsBox(1.0f, 1.0f); return; } int32 n = b2Min(count, b2_maxPolygonVertices); // Perform welding and copy vertices into local buffer. b2Vec2 ps[b2_maxPolygonVertices]; int32 tempCount = 0; for (int32 i = 0; i < n; ++i) { b2Vec2 v = vertices[i]; bool unique = true; for (int32 j = 0; j < tempCount; ++j) { if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) { unique = false; break; } } if (unique) { ps[tempCount++] = v; } } n = tempCount; if (n < 3) { // Polygon is degenerate. b2Assert(false); SetAsBox(1.0f, 1.0f); return; } // Create the convex hull using the Gift wrapping algorithm // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm // Find the right most point on the hull int32 i0 = 0; float32 x0 = ps[0].x; for (int32 i = 1; i < n; ++i) { float32 x = ps[i].x; if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) { i0 = i; x0 = x; } } int32 hull[b2_maxPolygonVertices]; int32 m = 0; int32 ih = i0; for (;;) { hull[m] = ih; int32 ie = 0; for (int32 j = 1; j < n; ++j) { if (ie == ih) { ie = j; continue; } b2Vec2 r = ps[ie] - ps[hull[m]]; b2Vec2 v = ps[j] - ps[hull[m]]; float32 c = b2Cross(r, v); if (c < 0.0f) { ie = j; } // Collinearity check if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { ie = j; } } ++m; ih = ie; if (ie == i0) { break; } } m_count = m; // Copy vertices. for (int32 i = 0; i < m; ++i) { m_vertices[i] = ps[hull[i]]; } // Compute normals. Ensure the edges have non-zero length. for (int32 i = 0; i < m; ++i) { int32 i1 = i; int32 i2 = i + 1 < m ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); m_normals[i] = b2Cross(edge, 1.0f); m_normals[i].Normalize(); } // Compute the polygon centroid. m_centroid = ComputeCentroid(m_vertices, m); }
float32 LH_b2BuoyancyController::ComputeSubmergedArea(b2Shape* shape, const b2Vec2& normal, float32 offset, const b2Transform& xf, b2Vec2* c, float32 density) const { if(shape->GetType() == b2Shape::e_edge) { //Note that v0 is independant of any details of the specific edge //We are relying on v0 being consistent between multiple edges of the same body b2Vec2 v0 = offset * normal; //b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal; b2Vec2 v1 = b2Mul(xf, ((b2EdgeShape*)shape)->m_vertex1); b2Vec2 v2 = b2Mul(xf, ((b2EdgeShape*)shape)->m_vertex2); float32 d1 = b2Dot(normal, v1) - offset; float32 d2 = b2Dot(normal, v2) - offset; if(d1>0) { if(d2>0) { return 0; } else { v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; } } else { if(d2>0) { v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2; } else { //Nothing } } // v0,v1,v2 represents a fully submerged triangle float32 k_inv3 = 1.0f / 3.0f; // Area weighted centroid *c = k_inv3 * (v0 + v1 + v2); b2Vec2 e1 = v1 - v0; b2Vec2 e2 = v2 - v0; return 0.5f * b2Cross(e1, e2); } else if(shape->GetType() == b2Shape::e_polygon) { //Transform plane into shape co-ordinates b2Vec2 normalL = b2MulT(xf.q,normal); float32 offsetL = offset - b2Dot(normal,xf.p); float32 depths[b2_maxPolygonVertices]; int32 diveCount = 0; int32 intoIndex = -1; int32 outoIndex = -1; bool lastSubmerged = false; int32 i; for(i=0;i<((b2PolygonShape*)shape)->m_vertexCount;i++){ depths[i] = b2Dot(normalL,((b2PolygonShape*)shape)->m_vertices[i]) - offsetL; bool isSubmerged = depths[i]<-FLT_EPSILON; if(i>0){ if(isSubmerged){ if(!lastSubmerged){ intoIndex = i-1; diveCount++; } }else{ if(lastSubmerged){ outoIndex = i-1; diveCount++; } } } lastSubmerged = isSubmerged; } switch(diveCount){ case 0: if(lastSubmerged){ //Completely submerged b2MassData md; ((b2PolygonShape*)shape)->ComputeMass(&md, density); *c = b2Mul(xf,md.center); return md.mass/density; }else{ //Completely dry return 0; } break; case 1: if(intoIndex==-1){ intoIndex = ((b2PolygonShape*)shape)->m_vertexCount-1; }else{ outoIndex = ((b2PolygonShape*)shape)->m_vertexCount-1; } break; } int32 intoIndex2 = (intoIndex+1)%((b2PolygonShape*)shape)->m_vertexCount; int32 outoIndex2 = (outoIndex+1)%((b2PolygonShape*)shape)->m_vertexCount; float32 intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); float32 outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); b2Vec2 intoVec( ((b2PolygonShape*)shape)->m_vertices[intoIndex].x*(1-intoLambda)+((b2PolygonShape*)shape)->m_vertices[intoIndex2].x*intoLambda, ((b2PolygonShape*)shape)->m_vertices[intoIndex].y*(1-intoLambda)+((b2PolygonShape*)shape)->m_vertices[intoIndex2].y*intoLambda); b2Vec2 outoVec( ((b2PolygonShape*)shape)->m_vertices[outoIndex].x*(1-outoLambda)+((b2PolygonShape*)shape)->m_vertices[outoIndex2].x*outoLambda, ((b2PolygonShape*)shape)->m_vertices[outoIndex].y*(1-outoLambda)+((b2PolygonShape*)shape)->m_vertices[outoIndex2].y*outoLambda); //Initialize accumulator float32 area = 0; b2Vec2 center(0,0); b2Vec2 p2 = ((b2PolygonShape*)shape)->m_vertices[intoIndex2]; b2Vec2 p3; float32 k_inv3 = 1.0f / 3.0f; //An awkward loop from intoIndex2+1 to outIndex2 i = intoIndex2; while(i!=outoIndex2){ i=(i+1)%((b2PolygonShape*)shape)->m_vertexCount; if(i==outoIndex2) p3 = outoVec; else p3 = ((b2PolygonShape*)shape)->m_vertices[i]; //Add the triangle formed by intoVec,p2,p3 { b2Vec2 e1 = p2 - intoVec; b2Vec2 e2 = p3 - intoVec; float32 D = b2Cross(e1, e2); float32 triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid center += triangleArea * k_inv3 * (intoVec + p2 + p3); } // p2=p3; } //Normalize and transform centroid center *= 1.0f/area; *c = b2Mul(xf,center); return area; } else if(shape->GetType() == b2Shape::e_circle) { b2Vec2 p = b2Mul(xf,((b2CircleShape*)shape)->m_p); float32 l = -(b2Dot(normal,p) - offset); if(l<-((b2CircleShape*)shape)->m_radius+FLT_EPSILON){ //Completely dry return 0; } if(l > ((b2CircleShape*)shape)->m_radius){ //Completely wet *c = p; return b2_pi*((b2CircleShape*)shape)->m_radius*((b2CircleShape*)shape)->m_radius; } //Magic float32 r2 = ((b2CircleShape*)shape)->m_radius*((b2CircleShape*)shape)->m_radius; float32 l2 = l*l; //TODO: write b2Sqrt to handle fixed point case. float32 area = r2 * (asin(l/((b2CircleShape*)shape)->m_radius) + b2_pi/2.0f)+ l * b2Sqrt(r2 - l2); float32 com = -2.0f/3.0f*pow(r2-l2,1.5f)/area; c->x = p.x + normal.x * com; c->y = p.y + normal.y * com; return area; } return 0; }
b2ContactSolver::b2ContactSolver(b2Contact * *contacts, int32 contactCount, b2StackAllocator *allocator, float32 impulseRatio) { m_allocator = allocator; m_constraintCount = contactCount; m_constraints = (b2ContactConstraint *) m_allocator->Allocate( m_constraintCount * sizeof(b2ContactConstraint)); for ( int32 i = 0; i<m_constraintCount; ++i) { b2Contact *contact = contacts[i]; b2Fixture *fixtureA = contact->m_fixtureA; b2Fixture *fixtureB = contact->m_fixtureB; b2Shape *shapeA = fixtureA->GetShape(); b2Shape *shapeB = fixtureB->GetShape(); float32 radiusA = shapeA->m_radius; float32 radiusB = shapeB->m_radius; b2Body *bodyA = fixtureA->GetBody(); b2Body *bodyB = fixtureB->GetBody(); b2Manifold *manifold = contact->GetManifold(); float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction()); float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution()); b2Vec2 vA = bodyA->m_linearVelocity; b2Vec2 vB = bodyB->m_linearVelocity; float32 wA = bodyA->m_angularVelocity; float32 wB = bodyB->m_angularVelocity; b2Assert(manifold ->pointCount > 0); b2WorldManifold worldManifold; worldManifold. Initialize(manifold, bodyA ->m_xf, radiusA, bodyB->m_xf, radiusB); b2ContactConstraint *cc = m_constraints + i; cc-> bodyA = bodyA; cc-> bodyB = bodyB; cc-> manifold = manifold; cc-> normal = worldManifold.normal; cc-> pointCount = manifold->pointCount; cc-> friction = friction; cc-> localNormal = manifold->localNormal; cc-> localPoint = manifold->localPoint; cc-> radius = radiusA + radiusB; cc-> type = manifold->type; for ( int32 j = 0; j<cc-> pointCount; ++j) { b2ManifoldPoint *cp = manifold->points + j; b2ContactConstraintPoint *ccp = cc->points + j; ccp-> normalImpulse = impulseRatio * cp->normalImpulse; ccp-> tangentImpulse = impulseRatio * cp->tangentImpulse; ccp-> localPoint = cp->localPoint; ccp-> rA = worldManifold.points[j] - bodyA->m_sweep.c; ccp-> rB = worldManifold.points[j] - bodyB->m_sweep.c; float32 rnA = b2Cross(ccp->rA, cc->normal); float32 rnB = b2Cross(ccp->rB, cc->normal); rnA *= rnA; rnB *= rnB; float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB; b2Assert(kNormal > b2_epsilon); ccp-> normalMass = 1.0f / kNormal; b2Vec2 tangent = b2Cross(cc->normal, 1.0f); float32 rtA = b2Cross(ccp->rA, tangent); float32 rtB = b2Cross(ccp->rB, tangent); rtA *= rtA; rtB *= rtB; float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB; b2Assert(kTangent > b2_epsilon); ccp-> tangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp-> velocityBias = 0.0f; float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA)); if (vRel < -b2_velocityThreshold) { ccp-> velocityBias = -restitution * vRel; } } // If we have two points, then prepare the block solver. if (cc->pointCount == 2) { b2ContactConstraintPoint *ccp1 = cc->points + 0; b2ContactConstraintPoint *ccp2 = cc->points + 1; float32 invMassA = bodyA->m_invMass; float32 invIA = bodyA->m_invI; float32 invMassB = bodyB->m_invMass; float32 invIB = bodyB->m_invI; float32 rn1A = b2Cross(ccp1->rA, cc->normal); float32 rn1B = b2Cross(ccp1->rB, cc->normal); float32 rn2A = b2Cross(ccp2->rA, cc->normal); float32 rn2B = b2Cross(ccp2->rB, cc->normal); float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float32 k_maxConditionNumber = 100.0f; if ( k11 *k11<k_maxConditionNumber *(k11 *k22 - k12 *k12 )) { // K is safe to invert. cc->K.col1. Set(k11, k12 ); cc->K.col2. Set(k12, k22 ); cc-> normalMass = cc->K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc-> pointCount = 1; } } } }
void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // Get the pulley axes. m_uA = cA + m_rA - m_groundAnchorA; m_uB = cB + m_rB - m_groundAnchorB; float32 lengthA = m_uA.Length(); float32 lengthB = m_uB.Length(); if (lengthA > 10.0f * b2_linearSlop) { m_uA *= 1.0f / lengthA; } else { m_uA.SetZero(); } if (lengthB > 10.0f * b2_linearSlop) { m_uB *= 1.0f / lengthB; } else { m_uB.SetZero(); } // Compute effective mass. float32 ruA = b2Cross(m_rA, m_uA); float32 ruB = b2Cross(m_rB, m_uB); float32 mA = m_invMassA + m_invIA * ruA * ruA; float32 mB = m_invMassB + m_invIB * ruB * ruB; m_mass = mA + m_ratio * m_ratio * mB; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } if (data.step.warmStarting) { // Scale impulses to support variable time steps. m_impulse *= data.step.dtRatio; // Warm starting. b2Vec2 PA = -(m_impulse) * m_uA; b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; vA += m_invMassA * PA; wA += m_invIA * b2Cross(m_rA, PA); vB += m_invMassB * PB; wB += m_invIB * b2Cross(m_rB, PB); } else { m_impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
// Possible regions: // - points[2] // - edge points[0]-points[2] // - edge points[1]-points[2] // - inside the triangle void b2Simplex::Solve3() { b2Vec2 w1 = m_v1.w; b2Vec2 w2 = m_v2.w; b2Vec2 w3 = m_v3.w; // Edge12 // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // a3 = 0 b2Vec2 e12 = w2 - w1; float32 w1e12 = b2Dot(w1, e12); float32 w2e12 = b2Dot(w2, e12); float32 d12_1 = w2e12; float32 d12_2 = -w1e12; // Edge13 // [1 1 ][a1] = [1] // [w1.e13 w3.e13][a3] = [0] // a2 = 0 b2Vec2 e13 = w3 - w1; float32 w1e13 = b2Dot(w1, e13); float32 w3e13 = b2Dot(w3, e13); float32 d13_1 = w3e13; float32 d13_2 = -w1e13; // Edge23 // [1 1 ][a2] = [1] // [w2.e23 w3.e23][a3] = [0] // a1 = 0 b2Vec2 e23 = w3 - w2; float32 w2e23 = b2Dot(w2, e23); float32 w3e23 = b2Dot(w3, e23); float32 d23_1 = w3e23; float32 d23_2 = -w2e23; // Triangle123 float32 n123 = b2Cross(e12, e13); float32 d123_1 = n123 * b2Cross(w2, w3); float32 d123_2 = n123 * b2Cross(w3, w1); float32 d123_3 = n123 * b2Cross(w1, w2); // w1 region if (d12_2 <= 0.0f && d13_2 <= 0.0f) { m_v1.a = 1.0f; m_count = 1; return; } // e12 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { float32 inv_d12 = 1.0f / (d12_1 + d12_2); m_v1.a = d12_1 * inv_d12; m_v2.a = d12_2 * inv_d12; m_count = 2; return; } // e13 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { float32 inv_d13 = 1.0f / (d13_1 + d13_2); m_v1.a = d13_1 * inv_d13; m_v3.a = d13_2 * inv_d13; m_count = 2; m_v2 = m_v3; return; } // w2 region if (d12_1 <= 0.0f && d23_2 <= 0.0f) { m_v2.a = 1.0f; m_count = 1; m_v1 = m_v2; return; } // w3 region if (d13_1 <= 0.0f && d23_1 <= 0.0f) { m_v3.a = 1.0f; m_count = 1; m_v1 = m_v3; return; } // e23 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { float32 inv_d23 = 1.0f / (d23_1 + d23_2); m_v2.a = d23_1 * inv_d23; m_v3.a = d23_2 * inv_d23; m_count = 2; m_v1 = m_v3; return; } // Must be in triangle123 float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); m_v1.a = d123_1 * inv_d123; m_v2.a = d123_2 * inv_d123; m_v3.a = d123_3 * inv_d123; m_count = 3; }
bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte) { B2_NOT_USED(baumgarte); b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; b2Vec2 c1 = b1->m_sweep.c; float32 a1 = b1->m_sweep.a; b2Vec2 c2 = b2->m_sweep.c; float32 a2 = b2->m_sweep.a; // Solve linear limit constraint. float32 linearError = 0.0f, angularError = 0.0f; bool active = false; float32 C2 = 0.0f; b2Mat22 R1(a1), R2(a2); b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA); b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB); b2Vec2 d = c2 + r2 - c1 - r1; if (m_enableLimit) { m_axis = b2Mul(R1, m_localXAxis1); m_a1 = b2Cross(d + r1, m_axis); m_a2 = b2Cross(r2, m_axis); float32 translation = b2Dot(m_axis, d); if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { // Prevent large angular corrections C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); linearError = b2Abs(translation); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); linearError = m_lowerTranslation - translation; active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); linearError = translation - m_upperTranslation; active = true; } } m_perp = b2Mul(R1, m_localYAxis1); m_s1 = b2Cross(d + r1, m_perp); m_s2 = b2Cross(r2, m_perp); b2Vec3 impulse; b2Vec2 C1; C1.x = b2Dot(m_perp, d); C1.y = a2 - a1 - m_refAngle; linearError = b2Max(linearError, b2Abs(C1.x)); angularError = b2Abs(C1.y); if (active) { float32 m1 = m_invMassA, m2 = m_invMassB; float32 i1 = m_invIA, i2 = m_invIB; float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; float32 k12 = i1 * m_s1 + i2 * m_s2; float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2; float32 k22 = i1 + i2; float32 k23 = i1 * m_a1 + i2 * m_a2; float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2; m_K.col1.Set(k11, k12, k13); m_K.col2.Set(k12, k22, k23); m_K.col3.Set(k13, k23, k33); b2Vec3 C; C.x = C1.x; C.y = C1.y; C.z = C2; impulse = m_K.Solve33(-C); } else { float32 m1 = m_invMassA, m2 = m_invMassB; float32 i1 = m_invIA, i2 = m_invIB; float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2; float32 k12 = i1 * m_s1 + i2 * m_s2; float32 k22 = i1 + i2; m_K.col1.Set(k11, k12, 0.0f); m_K.col2.Set(k12, k22, 0.0f); b2Vec2 impulse1 = m_K.Solve22(-C1); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } b2Vec2 P = impulse.x * m_perp + impulse.z * m_axis; float32 L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1; float32 L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2; c1 -= m_invMassA * P; a1 -= m_invIA * L1; c2 += m_invMassB * P; a2 += m_invIB * L2; // TODO_ERIN remove need for this. b1->m_sweep.c = c1; b1->m_sweep.a = a1; b2->m_sweep.c = c2; b2->m_sweep.a = a2; b1->SynchronizeTransform(); b2->SynchronizeTransform(); return linearError <= b2_linearSlop && angularError <= b2_angularSlop; }
void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step) { b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; b2Vec2 v1 = b1->m_linearVelocity; float32 w1 = b1->m_angularVelocity; b2Vec2 v2 = b2->m_linearVelocity; float32 w2 = b2->m_angularVelocity; float32 m1 = b1->m_invMass, m2 = b2->m_invMass; float32 i1 = b1->m_invI, i2 = b2->m_invI; // Solve motor constraint. if (m_enableMotor && m_limitState != e_equalLimits) { float32 Cdot = w2 - w1 - m_motorSpeed; float32 impulse = m_motorMass * (-Cdot); float32 oldImpulse = m_motorImpulse; float32 maxImpulse = step.dt * m_maxMotorTorque; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } // Solve limit constraint. if (m_enableLimit && m_limitState != e_inactiveLimit) { b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); // Solve point-to-point constraint b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1); float32 Cdot2 = w2 - w1; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 impulse = m_mass.Solve33(-Cdot); if (m_limitState == e_equalLimits) { m_impulse += impulse; } else if (m_limitState == e_atLowerLimit) { float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse < 0.0f) { b2Vec2 reduced = m_mass.Solve22(-Cdot1); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; m_impulse.x += reduced.x; m_impulse.y += reduced.y; m_impulse.z = 0.0f; } } else if (m_limitState == e_atUpperLimit) { float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse > 0.0f) { b2Vec2 reduced = m_mass.Solve22(-Cdot1); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; m_impulse.x += reduced.x; m_impulse.y += reduced.y; m_impulse.z = 0.0f; } } b2Vec2 P(impulse.x, impulse.y); v1 -= m1 * P; w1 -= i1 * (b2Cross(r1, P) + impulse.z); v2 += m2 * P; w2 += i2 * (b2Cross(r2, P) + impulse.z); } else { b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); // Solve point-to-point constraint b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1); b2Vec2 impulse = m_mass.Solve22(-Cdot); m_impulse.x += impulse.x; m_impulse.y += impulse.y; v1 -= m1 * impulse; w1 -= i1 * b2Cross(r1, impulse); v2 += m2 * impulse; w2 += i2 * b2Cross(r2, impulse); } b1->m_linearVelocity = v1; b1->m_angularVelocity = w1; b2->m_linearVelocity = v2; b2->m_angularVelocity = w2; }
bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); float32 positionError, angularError; b2Mat33 K; 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 (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 * b2Cross(rA, P); cB += mB * P; aB += iB * b2Cross(rB, P); } else { b2Vec2 C1 = cB + rB - cA - rA; float32 C2 = aB - aA - m_referenceAngle; positionError = C1.Length(); angularError = b2Abs(C2); b2Vec3 C(C1.x, C1.y, C2); b2Vec3 impulse = -K.Solve33(C); b2Vec2 P(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (b2Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (b2Cross(rB, P) + impulse.z); } 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 <= b2_linearSlop && angularError <= b2_angularSlop; }
void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step) { b2Body* b1 = m_bodyA; b2Body* b2 = m_bodyB; if (m_enableMotor || m_enableLimit) { // You cannot create a rotation limit between bodies that // both have fixed rotation. b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f); } // Compute the effective mass matrix. b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter()); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] float32 m1 = b1->m_invMass, m2 = b2->m_invMass; float32 i1 = b1->m_invI, i2 = b2->m_invI; m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2; m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2; m_mass.col3.x = -r1.y * i1 - r2.y * i2; m_mass.col1.y = m_mass.col2.x; m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2; m_mass.col3.y = r1.x * i1 + r2.x * i2; m_mass.col1.z = m_mass.col3.x; m_mass.col2.z = m_mass.col3.y; m_mass.col3.z = i1 + i2; m_motorMass = i1 + i2; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } if (m_enableMotor == false) { m_motorImpulse = 0.0f; } if (m_enableLimit) { float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle; if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) { m_limitState = e_equalLimits; } else if (jointAngle <= m_lowerAngle) { if (m_limitState != e_atLowerLimit) { m_impulse.z = 0.0f; } m_limitState = e_atLowerLimit; } else if (jointAngle >= m_upperAngle) { if (m_limitState != e_atUpperLimit) { m_impulse.z = 0.0f; } m_limitState = e_atUpperLimit; } else { m_limitState = e_inactiveLimit; m_impulse.z = 0.0f; } } else { m_limitState = e_inactiveLimit; } if (step.warmStarting) { // Scale impulses to support a variable time step. m_impulse *= step.dtRatio; m_motorImpulse *= step.dtRatio; b2Vec2 P(m_impulse.x, m_impulse.y); b1->m_linearVelocity -= m1 * P; b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z); b2->m_linearVelocity += m2 * P; b2->m_angularVelocity += i2 * (b2Cross(r2, P) + m_motorImpulse + m_impulse.z); } else { m_impulse.SetZero(); m_motorImpulse = 0.0f; } }
void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // 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] float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; b2Mat33 K; K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; K.ez.x = -m_rA.y * iA - m_rB.y * iB; K.ex.y = K.ey.x; K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; K.ez.y = m_rA.x * iA + m_rB.x * iB; K.ex.z = K.ez.x; K.ey.z = K.ez.y; K.ez.z = iA + iB; if (m_frequencyHz > 0.0f) { K.GetInverse22(&m_mass); float32 invM = iA + iB; float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; float32 C = aB - aA - m_referenceAngle; // Frequency float32 omega = 2.0f * b2_pi * m_frequencyHz; // Damping coefficient float32 d = 2.0f * m * m_dampingRatio * omega; // Spring stiffness float32 k = m * omega * omega; // magic formulas float32 h = data.step.dt; m_gamma = h * (d + h * k); m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; m_bias = C * h * k * m_gamma; invM += m_gamma; m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(&m_mass); m_gamma = 0.0f; m_bias = 0.0f; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. m_impulse *= data.step.dtRatio; b2Vec2 P(m_impulse.x, m_impulse.y); vA -= mA * P; wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); vB += mB * P; wB += iB * (b2Cross(m_rB, P) + m_impulse.z); } else { m_impulse.SetZero(); } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Rot qA(aA), qB(aB); float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; // Compute fresh Jacobians b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = cB + rB - cA - rA; b2Vec2 axis = b2Mul(qA, m_localXAxisA); float32 a1 = b2Cross(d + rA, axis); float32 a2 = b2Cross(rB, axis); b2Vec2 perp = b2Mul(qA, m_localYAxisA); float32 s1 = b2Cross(d + rA, perp); float32 s2 = b2Cross(rB, perp); b2Vec3 impulse; b2Vec2 C1; C1.x = b2Dot(perp, d); C1.y = aB - aA - m_referenceAngle; float32 linearError = b2Abs(C1.x); float32 angularError = b2Abs(C1.y); bool active = false; float32 C2 = 0.0f; if (m_enableLimit) { float32 translation = b2Dot(axis, d); if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { // Prevent large angular corrections C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); linearError = b2Max(linearError, b2Abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); linearError = b2Max(linearError, m_lowerTranslation - translation); active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); linearError = b2Max(linearError, translation - m_upperTranslation); active = true; } } if (active) { float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float32 k12 = iA * s1 + iB * s2; float32 k13 = iA * s1 * a1 + iB * s2 * a2; float32 k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float32 k23 = iA * a1 + iB * a2; float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; b2Mat33 K; K.ex.Set(k11, k12, k13); K.ey.Set(k12, k22, k23); K.ez.Set(k13, k23, k33); b2Vec3 C; C.x = C1.x; C.y = C1.y; C.z = C2; impulse = K.Solve33(-C); } else { float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float32 k12 = iA * s1 + iB * s2; float32 k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } b2Mat22 K; K.ex.Set(k11, k12); K.ey.Set(k12, k22); b2Vec2 impulse1 = K.Solve(-C1); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } b2Vec2 P = impulse.x * perp + impulse.z * axis; float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; float32 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 <= b2_linearSlop && angularError <= b2_angularSlop; }
// Initialize position dependent portions of the velocity constraints. void b2ContactSolver::InitializeVelocityConstraints() { for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* cc = m_constraints + i; float32 radiusA = cc->radiusA; float32 radiusB = cc->radiusB; b2Body* bodyA = cc->bodyA; b2Body* bodyB = cc->bodyB; b2Manifold* manifold = cc->manifold; b2Vec2 vA = bodyA->m_linearVelocity; b2Vec2 vB = bodyB->m_linearVelocity; float32 wA = bodyA->m_angularVelocity; float32 wB = bodyB->m_angularVelocity; b2Assert(manifold->pointCount > 0); b2WorldManifold worldManifold; worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB); cc->normal = worldManifold.normal; for (int32 j = 0; j < cc->pointCount; ++j) { b2ContactConstraintPoint* ccp = cc->points + j; ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c; ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c; float32 rnA = b2Cross(ccp->rA, cc->normal); float32 rnB = b2Cross(ccp->rB, cc->normal); rnA *= rnA; rnB *= rnB; float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB; b2Assert(kNormal > b2_epsilon); ccp->normalMass = 1.0f / kNormal; b2Vec2 tangent = b2Cross(cc->normal, 1.0f); float32 rtA = b2Cross(ccp->rA, tangent); float32 rtB = b2Cross(ccp->rB, tangent); rtA *= rtA; rtB *= rtB; float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB; b2Assert(kTangent > b2_epsilon); ccp->tangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp->velocityBias = 0.0f; float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA)); if (vRel < -b2_velocityThreshold) { ccp->velocityBias = -cc->restitution * vRel; } } // If we have two points, then prepare the block solver. if (cc->pointCount == 2) { b2ContactConstraintPoint* ccp1 = cc->points + 0; b2ContactConstraintPoint* ccp2 = cc->points + 1; float32 invMassA = bodyA->m_invMass; float32 invIA = bodyA->m_invI; float32 invMassB = bodyB->m_invMass; float32 invIB = bodyB->m_invI; float32 rn1A = b2Cross(ccp1->rA, cc->normal); float32 rn1B = b2Cross(ccp1->rB, cc->normal); float32 rn2A = b2Cross(ccp2->rA, cc->normal); float32 rn2B = b2Cross(ccp2->rB, cc->normal); float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float32 k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc->K.col1.Set(k11, k12); cc->K.col2.Set(k12, k22); cc->normalMass = cc->K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc->pointCount = 1; } } } }
void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) { b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; bool fixedRotation = (iA + iB == 0.0f); // Solve motor constraint. if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) { float32 Cdot = wB - wA - m_motorSpeed; float32 impulse = -m_motorMass * Cdot; float32 oldImpulse = m_motorImpulse; float32 maxImpulse = data.step.dt * m_maxMotorTorque; m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_motorImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve limit constraint. if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) { b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); float32 Cdot2 = wB - wA; b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); b2Vec3 impulse = -m_mass.Solve33(Cdot); if (m_limitState == e_equalLimits) { m_impulse += impulse; } else if (m_limitState == e_atLowerLimit) { float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse < 0.0f) { b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; m_impulse.x += reduced.x; m_impulse.y += reduced.y; m_impulse.z = 0.0f; } else { m_impulse += impulse; } } else if (m_limitState == e_atUpperLimit) { float32 newImpulse = m_impulse.z + impulse.z; if (newImpulse > 0.0f) { b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); b2Vec2 reduced = m_mass.Solve22(rhs); impulse.x = reduced.x; impulse.y = reduced.y; impulse.z = -m_impulse.z; m_impulse.x += reduced.x; m_impulse.y += reduced.y; m_impulse.z = 0.0f; } else { m_impulse += impulse; } } b2Vec2 P(impulse.x, impulse.y); vA -= mA * P; wA -= iA * (b2Cross(m_rA, P) + impulse.z); vB += mB * P; wB += iB * (b2Cross(m_rB, P) + impulse.z); } else { // Solve point-to-point constraint b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); b2Vec2 impulse = m_mass.Solve22(-Cdot); m_impulse.x += impulse.x; m_impulse.y += impulse.y; vA -= mA * impulse; wA -= iA * b2Cross(m_rA, impulse); vB += mB * impulse; wB += iB * b2Cross(m_rB, impulse); } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2ContactSolver::SolveVelocityConstraints() { for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* c = m_constraints + i; b2Body* bodyA = c->bodyA; b2Body* bodyB = c->bodyB; float32 wA = bodyA->m_angularVelocity; float32 wB = bodyB->m_angularVelocity; b2Vec2 vA = bodyA->m_linearVelocity; b2Vec2 vB = bodyB->m_linearVelocity; float32 invMassA = bodyA->m_invMass; float32 invIA = bodyA->m_invI; float32 invMassB = bodyB->m_invMass; float32 invIB = bodyB->m_invI; b2Vec2 normal = c->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); float32 friction = c->friction; b2Assert(c->pointCount == 1 || c->pointCount == 2); // Solve tangent constraints for (int32 j = 0; j < c->pointCount; ++j) { b2ContactConstraintPoint* ccp = c->points + j; // Relative velocity at contact b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA); // Compute tangent force float32 vt = b2Dot(dv, tangent); float32 lambda = ccp->tangentMass * (-vt); // b2Clamp the accumulated force float32 maxFriction = friction * ccp->normalImpulse; float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - ccp->tangentImpulse; // Apply contact impulse b2Vec2 P = lambda * tangent; vA -= invMassA * P; wA -= invIA * b2Cross(ccp->rA, P); vB += invMassB * P; wB += invIB * b2Cross(ccp->rB, P); ccp->tangentImpulse = newImpulse; } // Solve normal constraints if (c->pointCount == 1) { b2ContactConstraintPoint* ccp = c->points + 0; // Relative velocity at contact b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA); // Compute normal impulse float32 vn = b2Dot(dv, normal); float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias); // b2Clamp the accumulated impulse float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f); lambda = newImpulse - ccp->normalImpulse; // Apply contact impulse b2Vec2 P = lambda * normal; vA -= invMassA * P; wA -= invIA * b2Cross(ccp->rA, P); vB += invMassB * P; wB += invIB * b2Cross(ccp->rB, P); ccp->normalImpulse = newImpulse; } else { // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). // Build the mini LCP for this contact patch // // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 // // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) // b = vn_0 - velocityBias // // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid // solution that satisfies the problem is chosen. // // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). // // Substitute: // // x = x' - a // // Plug into above equation: // // vn = A * x + b // = A * (x' - a) + b // = A * x' + b - A * a // = A * x' + b' // b' = b - A * a; b2ContactConstraintPoint* cp1 = c->points + 0; b2ContactConstraintPoint* cp2 = c->points + 1; b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); b2Assert(a.x >= 0.0f && a.y >= 0.0f); // Relative velocity at contact b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); // Compute normal velocity float32 vn1 = b2Dot(dv1, normal); float32 vn2 = b2Dot(dv2, normal); b2Vec2 b; b.x = vn1 - cp1->velocityBias; b.y = vn2 - cp2->velocityBias; b -= b2Mul(c->K, a); const float32 k_errorTol = 1e-3f; B2_NOT_USED(k_errorTol); for (;;) { // // Case 1: vn = 0 // // 0 = A * x' + b' // // Solve for x': // // x' = - inv(A) * b' // b2Vec2 x = - b2Mul(c->normalMass, b); if (x.x >= 0.0f && x.y >= 0.0f) { // Resubstitute for the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; cp2->normalImpulse = x.y; #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); // Compute normal velocity vn1 = b2Dot(dv1, normal); vn2 = b2Dot(dv2, normal); b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); #endif break; } // // Case 2: vn1 = 0 and x2 = 0 // // 0 = a11 * x1' + a12 * 0 + b1' // vn2 = a21 * x1' + a22 * 0 + b2' // x.x = - cp1->normalMass * b.x; x.y = 0.0f; vn1 = 0.0f; vn2 = c->K.col1.y * x.x + b.y; if (x.x >= 0.0f && vn2 >= 0.0f) { // Resubstitute for the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; cp2->normalImpulse = x.y; #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); // Compute normal velocity vn1 = b2Dot(dv1, normal); b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); #endif break; } // // Case 3: vn2 = 0 and x1 = 0 // // vn1 = a11 * 0 + a12 * x2' + b1' // 0 = a21 * 0 + a22 * x2' + b2' // x.x = 0.0f; x.y = - cp2->normalMass * b.y; vn1 = c->K.col2.x * x.y + b.x; vn2 = 0.0f; if (x.y >= 0.0f && vn1 >= 0.0f) { // Resubstitute for the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; cp2->normalImpulse = x.y; #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); // Compute normal velocity vn2 = b2Dot(dv2, normal); b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); #endif break; } // // Case 4: x1 = 0 and x2 = 0 // // vn1 = b1 // vn2 = b2; x.x = 0.0f; x.y = 0.0f; vn1 = b.x; vn2 = b.y; if (vn1 >= 0.0f && vn2 >= 0.0f ) { // Resubstitute for the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += invMassB * (P1 + P2); wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); // Accumulate cp1->normalImpulse = x.x; cp2->normalImpulse = x.y; break; } // No solution, give up. This is hit sometimes, but it doesn't seem to matter. break; } } bodyA->m_linearVelocity = vA; bodyA->m_angularVelocity = wA; bodyB->m_linearVelocity = vB; bodyB->m_angularVelocity = wB; } }
void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // 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] float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; bool fixedRotation = (iA + iB == 0.0f); m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; m_mass.ex.y = m_mass.ey.x; m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; m_mass.ex.z = m_mass.ez.x; m_mass.ey.z = m_mass.ez.y; m_mass.ez.z = iA + iB; m_motorMass = iA + iB; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } if (m_enableMotor == false || fixedRotation) { m_motorImpulse = 0.0f; } if (m_enableLimit && fixedRotation == false) { float32 jointAngle = aB - aA - m_referenceAngle; if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) { m_limitState = e_equalLimits; } else if (jointAngle <= m_lowerAngle) { if (m_limitState != e_atLowerLimit) { m_impulse.z = 0.0f; } m_limitState = e_atLowerLimit; } else if (jointAngle >= m_upperAngle) { if (m_limitState != e_atUpperLimit) { m_impulse.z = 0.0f; } m_limitState = e_atUpperLimit; } else { m_limitState = e_inactiveLimit; m_impulse.z = 0.0f; } } else { m_limitState = e_inactiveLimit; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. m_impulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; b2Vec2 P(m_impulse.x, m_impulse.y); vA -= mA * P; wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); vB += mB * P; wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); } else { m_impulse.SetZero(); m_motorImpulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
// Sequential position solver for position constraints. bool b2ContactSolver::SolveTOIPositionConstraints(float32 baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB) { float32 minSeparation = 0.0f; for (int32 i = 0; i < m_count; ++i) { b2ContactConstraint* c = m_constraints + i; b2Body* bodyA = c->bodyA; b2Body* bodyB = c->bodyB; float32 massA = 0.0f; if (bodyA == toiBodyA || bodyA == toiBodyB) { massA = bodyA->m_mass; } float32 massB = 0.0f; if (bodyB == toiBodyA || bodyB == toiBodyB) { massB = bodyB->m_mass; } float32 invMassA = bodyA->m_mass * bodyA->m_invMass; float32 invIA = bodyA->m_mass * bodyA->m_invI; float32 invMassB = bodyB->m_mass * bodyB->m_invMass; float32 invIB = bodyB->m_mass * bodyB->m_invI; // Solve normal constraints for (int32 j = 0; j < c->pointCount; ++j) { b2PositionSolverManifold psm(c,j); //psm.Initialize(c, j); b2Vec2 normal = psm.normal; b2Vec2 point = psm.point; float32 separation = psm.separation; b2Vec2 rA = point - bodyA->m_sweep.c; b2Vec2 rB = point - bodyB->m_sweep.c; // Track max constraint error. minSeparation = b2Min(minSeparation, separation); // Prevent large corrections and allow slop. float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); // Compute the effective mass. float32 rnA = b2Cross(rA, normal); float32 rnB = b2Cross(rB, normal); float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB; // Compute normal impulse float32 impulse = K > 0.0f ? - C / K : 0.0f; b2Vec2 P = impulse * normal; bodyA->m_sweep.c -= invMassA * P; bodyA->m_sweep.a -= invIA * b2Cross(rA, P); bodyA->SynchronizeTransform(); bodyB->m_sweep.c += invMassB * P; bodyB->m_sweep.a += invIB * b2Cross(rB, P); bodyB->SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return minSeparation >= -1.5f * b2_linearSlop; }
void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); m_u = cB + m_rB - cA - m_rA; m_length = m_u.Length(); float32 C = m_length - m_maxLength; if (C > 0.0f) { m_state = e_atUpperLimit; } else { m_state = e_inactiveLimit; } if (m_length > b2_linearSlop) { m_u *= 1.0f / m_length; } else { m_u.SetZero(); m_mass = 0.0f; m_impulse = 0.0f; return; } // Compute effective mass. float32 crA = b2Cross(m_rA, m_u); float32 crB = b2Cross(m_rB, m_u); float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (data.step.warmStarting) { // Scale the impulse to support a variable time step. m_impulse *= data.step.dtRatio; b2Vec2 P = m_impulse * m_u; vA -= m_invMassA * P; wA -= m_invIA * b2Cross(m_rA, P); vB += m_invMassB * P; wB += m_invIB * b2Cross(m_rB, P); } else { m_impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexA = m_bodyA->m_islandIndex; m_indexB = m_bodyB->m_islandIndex; m_localCenterA = m_bodyA->m_sweep.localCenter; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassA = m_bodyA->m_invMass; m_invMassB = m_bodyB->m_invMass; m_invIA = m_bodyA->m_invI; m_invIB = m_bodyB->m_invI; float32 mA = m_invMassA, mB = m_invMassB; float32 iA = m_invIA, iB = m_invIB; b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 vA = data.velocities[m_indexA].v; float32 wA = data.velocities[m_indexA].w; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float32 wB = data.velocities[m_indexB].w; b2Rot qA(aA), qB(aB); // Compute the effective masses. b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); b2Vec2 d = cB + rB - cA - rA; // Point to line constraint { m_ay = b2Mul(qA, m_localYAxisA); m_sAy = b2Cross(d + rA, m_ay); m_sBy = b2Cross(rB, m_ay); m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } } // Spring constraint m_springMass = 0.0f; m_bias = 0.0f; m_gamma = 0.0f; if (m_frequencyHz > 0.0f) { m_ax = b2Mul(qA, m_localXAxisA); m_sAx = b2Cross(d + rA, m_ax); m_sBx = b2Cross(rB, m_ax); float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; if (invMass > 0.0f) { m_springMass = 1.0f / invMass; float32 C = b2Dot(d, m_ax); // Frequency float32 omega = 2.0f * b2_pi * m_frequencyHz; // Damping coefficient float32 d = 2.0f * m_springMass * m_dampingRatio * omega; // Spring stiffness float32 k = m_springMass * omega * omega; // magic formulas float32 h = data.step.dt; m_gamma = h * (d + h * k); if (m_gamma > 0.0f) { m_gamma = 1.0f / m_gamma; } m_bias = C * h * k * m_gamma; m_springMass = invMass + m_gamma; if (m_springMass > 0.0f) { m_springMass = 1.0f / m_springMass; } } } else { m_springImpulse = 0.0f; } // Rotational motor if (m_enableMotor) { m_motorMass = iA + iB; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } } else { m_motorMass = 0.0f; m_motorImpulse = 0.0f; } if (data.step.warmStarting) { // Account for variable time step. m_impulse *= data.step.dtRatio; m_springImpulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; vA -= m_invMassA * P; wA -= m_invIA * LA; vB += m_invMassB * P; wB += m_invIB * LB; } else { m_impulse = 0.0f; m_springImpulse = 0.0f; m_motorImpulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) { b2Vec2 cA = data.positions[m_indexA].c; float32 aA = data.positions[m_indexA].a; b2Vec2 cB = data.positions[m_indexB].c; float32 aB = data.positions[m_indexB].a; b2Vec2 cC = data.positions[m_indexC].c; float32 aC = data.positions[m_indexC].a; b2Vec2 cD = data.positions[m_indexD].c; float32 aD = data.positions[m_indexD].a; b2Rot qA(aA), qB(aB), qC(aC), qD(aD); float32 linearError = 0.0f; float32 coordinateA, coordinateB; b2Vec2 JvAC, JvBD; float32 JwA, JwB, JwC, JwD; float32 mass = 0.0f; if (m_typeA == e_revoluteJoint) { JvAC.SetZero(); JwA = 1.0f; JwC = 1.0f; mass += m_iA + m_iC; coordinateA = aA - aC - m_referenceAngleA; } else { b2Vec2 u = b2Mul(qC, m_localAxisC); b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); JvAC = u; JwC = b2Cross(rC, u); JwA = b2Cross(rA, u); mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; b2Vec2 pC = m_localAnchorC - m_lcC; b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); coordinateA = b2Dot(pA - pC, m_localAxisC); } if (m_typeB == e_revoluteJoint) { JvBD.SetZero(); JwB = m_ratio; JwD = m_ratio; mass += m_ratio * m_ratio * (m_iB + m_iD); coordinateB = aB - aD - m_referenceAngleB; } else { b2Vec2 u = b2Mul(qD, m_localAxisD); b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); JvBD = m_ratio * u; JwD = m_ratio * b2Cross(rD, u); JwB = m_ratio * b2Cross(rB, u); mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; b2Vec2 pD = m_localAnchorD - m_lcD; b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); coordinateB = b2Dot(pB - pD, m_localAxisD); } float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; float32 impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; } cA += m_mA * impulse * JvAC; aA += m_iA * impulse * JwA; cB += m_mB * impulse * JvBD; aB += m_iB * impulse * JwB; cC -= m_mC * impulse * JvAC; aC -= m_iC * impulse * JwC; cD -= m_mD * impulse * JvBD; aD -= m_iD * impulse * JwD; data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; data.positions[m_indexC].c = cC; data.positions[m_indexC].a = aC; data.positions[m_indexD].c = cD; data.positions[m_indexD].a = aD; // TODO_ERIN not implemented return linearError < b2_linearSlop; }
void b2PolygonShape::ComputeMass(b2MassData* massData, qreal density) const { // Polygon mass, centroid, and inertia. // Let rho be the polygon density in mass per unit area. // Then: // mass = rho * int(dA) // centroid.x = (1/mass) * rho * int(x * dA) // centroid.y = (1/mass) * rho * int(y * dA) // I = rho * int((x*x + y*y) * dA) // // We can compute these integrals by summing all the integrals // for each triangle of the polygon. To evaluate the integral // for a single triangle, we make a change of variables to // the (u,v) coordinates of the triangle: // x = x0 + e1x * u + e2x * v // y = y0 + e1y * u + e2y * v // where 0 <= u && 0 <= v && u + v <= 1. // // We integrate u from [0,1-v] and then v from [0,1]. // We also need to use the Jacobian of the transformation: // D = cross(e1, e2) // // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) // // The rest of the derivation is handled by computer algebra. b2Assert(m_vertexCount >= 3); b2Vec2 center; center.Set(0.0f, 0.0f); qreal area = 0.0f; qreal I = 0.0f; // pRef is the reference point for forming triangles. // It's location doesn't change the result (except for rounding error). b2Vec2 pRef(0.0f, 0.0f); #if 0 // This code would put the reference point inside the polygon. for (int32 i = 0; i < m_vertexCount; ++i) { pRef += m_vertices[i]; } pRef *= 1.0f / count; #endif const qreal k_inv3 = 1.0f / 3.0f; for (int32 i = 0; i < m_vertexCount; ++i) { // Triangle vertices. b2Vec2 p1 = pRef; b2Vec2 p2 = m_vertices[i]; b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0]; b2Vec2 e1 = p2 - p1; b2Vec2 e2 = p3 - p1; qreal D = b2Cross(e1, e2); qreal triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid center += triangleArea * k_inv3 * (p1 + p2 + p3); qreal px = p1.x, py = p1.y; qreal ex1 = e1.x, ey1 = e1.y; qreal ex2 = e2.x, ey2 = e2.y; qreal intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px; qreal inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py; I += D * (intx2 + inty2); } // Total mass massData->mass = density * area; // Center of mass b2Assert(area > b2_epsilon); center *= 1.0f / area; massData->center = center; // Inertia tensor relative to the local origin. massData->I = density * I; }