// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points. // Edge adjacency is handle to produce locally valid contact points and normals. This is intended // to allow the polygon to slide smoothly over an edge chain. // // Algorithm // 1. Classify front-side or back-side collision with edge. // 2. Compute separation // 3. Process adjacent edges // 4. Classify adjacent edge as convex, flat, null, or concave // 5. Skip null or concave edges. Concave edges get a separate manifold. // 6. If the edge is flat, compute contact points as normal. Discard boundary points. // 7. If the edge is convex, compute it's separation. // 8. Use the minimum separation of up to three edges. If the minimum separation // is not the primary edge, return. // 9. If the minimum separation is the primary edge, compute the contact points and return. void b2CollideEdgeAndPolygon( b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2PolygonShape* polygonB_in, const b2Transform& xfB) { manifold->pointCount = 0; b2Transform xf = b2MulT(xfA, xfB); // Create a polygon for edge shape A b2PolygonShape polygonA; polygonA.SetAsEdge(edgeA->m_vertex1, edgeA->m_vertex2); // Build polygonB in frame A b2PolygonShape polygonB; polygonB.m_radius = polygonB_in->m_radius; polygonB.m_vertexCount = polygonB_in->m_vertexCount; polygonB.m_centroid = b2Mul(xf, polygonB_in->m_centroid); for (int32 i = 0; i < polygonB.m_vertexCount; ++i) { polygonB.m_vertices[i] = b2Mul(xf, polygonB_in->m_vertices[i]); polygonB.m_normals[i] = b2Mul(xf.R, polygonB_in->m_normals[i]); } float32 totalRadius = polygonA.m_radius + polygonB.m_radius; // Edge geometry b2Vec2 v1 = edgeA->m_vertex1; b2Vec2 v2 = edgeA->m_vertex2; b2Vec2 e = v2 - v1; b2Vec2 edgeNormal(e.y, -e.x); edgeNormal.Normalize(); // Determine side bool isFrontSide = b2Dot(edgeNormal, polygonB.m_centroid - v1) >= 0.0f; if (isFrontSide == false) { edgeNormal = -edgeNormal; } // Compute primary separating axis b2EPAxis edgeAxis = b2EPEdgeSeparation(v1, v2, edgeNormal, &polygonB, totalRadius); if (edgeAxis.separation > totalRadius) { // Shapes are separated return; } // Classify adjacent edges b2EdgeType types[2] = {b2_isolated, b2_isolated}; if (edgeA->m_hasVertex0) { b2Vec2 v0 = edgeA->m_vertex0; float32 s = b2Dot(edgeNormal, v0 - v1); if (s > 0.1f * b2_linearSlop) { types[0] = b2_concave; } else if (s >= -0.1f * b2_linearSlop) { types[0] = b2_flat; } else { types[0] = b2_convex; } } if (edgeA->m_hasVertex3) { b2Vec2 v3 = edgeA->m_vertex3; float32 s = b2Dot(edgeNormal, v3 - v2); if (s > 0.1f * b2_linearSlop) { types[1] = b2_concave; } else if (s >= -0.1f * b2_linearSlop) { types[1] = b2_flat; } else { types[1] = b2_convex; } } if (types[0] == b2_convex) { // Check separation on previous edge. b2Vec2 v0 = edgeA->m_vertex0; b2Vec2 e0 = v1 - v0; b2Vec2 n0(e0.y, -e0.x); n0.Normalize(); if (isFrontSide == false) { n0 = -n0; } b2EPAxis axis1 = b2EPEdgeSeparation(v0, v1, n0, &polygonB, totalRadius); if (axis1.separation > edgeAxis.separation) { // The polygon should collide with previous edge return; } } if (types[1] == b2_convex) { // Check separation on next edge. b2Vec2 v3 = edgeA->m_vertex3; b2Vec2 e2 = v3 - v2; b2Vec2 n2(e2.y, -e2.x); n2.Normalize(); if (isFrontSide == false) { n2 = -n2; } b2EPAxis axis2 = b2EPEdgeSeparation(v2, v3, n2, &polygonB, totalRadius); if (axis2.separation > edgeAxis.separation) { // The polygon should collide with the next edge return; } } b2EPAxis polygonAxis = b2EPPolygonSeparation(v1, v2, edgeNormal, &polygonB, totalRadius); if (polygonAxis.separation > totalRadius) { return; } // Use hysteresis for jitter reduction. const float32 k_relativeTol = 0.98f; const float32 k_absoluteTol = 0.001f; b2EPAxis primaryAxis; if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } b2PolygonShape* poly1; b2PolygonShape* poly2; b2ClipVertex incidentEdge[2]; if (primaryAxis.type == b2EPAxis::e_edgeA) { poly1 = &polygonA; poly2 = &polygonB; if (isFrontSide == false) { primaryAxis.index = 1; } manifold->type = b2Manifold::e_faceA; } else { poly1 = &polygonB; poly2 = &polygonA; manifold->type = b2Manifold::e_faceB; } int32 edge1 = primaryAxis.index; b2FindIncidentEdge(incidentEdge, poly1, primaryAxis.index, poly2); int32 count1 = poly1->m_vertexCount; const b2Vec2* vertices1 = poly1->m_vertices; int32 iv1 = edge1; int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; b2Vec2 v11 = vertices1[iv1]; b2Vec2 v12 = vertices1[iv2]; b2Vec2 tangent = v12 - v11; tangent.Normalize(); b2Vec2 normal = b2Cross(tangent, 1.0f); b2Vec2 planePoint = 0.5f * (v11 + v12); // Face offset. float32 frontOffset = b2Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. b2ClipVertex clipPoints1[2]; b2ClipVertex clipPoints2[2]; int np; // Clip to box side 1 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); if (np < b2_maxManifoldPoints) { return; } // Clip to negative box side 1 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); if (np < b2_maxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.type == b2EPAxis::e_edgeA) { manifold->localNormal = normal; manifold->localPoint = planePoint; } else { manifold->localNormal = b2MulT(xf.R, normal); manifold->localPoint = b2MulT(xf, planePoint); } int32 pointCount = 0; for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { float32 separation; separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { b2ManifoldPoint* cp = manifold->points + pointCount; if (primaryAxis.type == b2EPAxis::e_edgeA) { cp->localPoint = b2MulT(xf, clipPoints2[i].v); cp->id = clipPoints2[i].id; } else { cp->localPoint = clipPoints2[i].v; cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; } if (cp->id.cf.typeA == b2ContactFeature::e_vertex && types[cp->id.cf.indexA] == b2_flat) { continue; } ++pointCount; } } manifold->pointCount = pointCount; }
void b2ContactSolver::SolveVelocityConstraints() { for (int32 i = 0; i < m_count; ++i) { b2ContactVelocityConstraint* vc = m_velocityConstraints + i; int32 indexA = vc->indexA; int32 indexB = vc->indexB; float32 mA = vc->invMassA; float32 iA = vc->invIA; float32 mB = vc->invMassB; float32 iB = vc->invIB; int32 pointCount = vc->pointCount; b2Vec2 vA = m_velocities[indexA].v; float32 wA = m_velocities[indexA].w; b2Vec2 vB = m_velocities[indexB].v; float32 wB = m_velocities[indexB].w; b2Vec2 normal = vc->normal; b2Vec2 tangent = b2Cross(normal, 1.0f); float32 friction = vc->friction; b2Assert(pointCount == 1 || pointCount == 2); // Solve tangent constraints first because non-penetration is more important // than friction. for (int32 j = 0; j < pointCount; ++j) { b2VelocityConstraintPoint* vcp = vc->points + j; // Relative velocity at contact b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); // Compute tangent force float32 vt = b2Dot(dv, tangent); float32 lambda = vcp->tangentMass * (-vt); // b2Clamp the accumulated force float32 maxFriction = friction * vcp->normalImpulse; float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); lambda = newImpulse - vcp->tangentImpulse; vcp->tangentImpulse = newImpulse; // Apply contact impulse b2Vec2 P = lambda * tangent; vA -= mA * P; wA -= iA * b2Cross(vcp->rA, P); vB += mB * P; wB += iB * b2Cross(vcp->rB, P); } // Solve normal constraints if (vc->pointCount == 1) { b2VelocityConstraintPoint* vcp = vc->points + 0; // Relative velocity at contact b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); // Compute normal impulse float32 vn = b2Dot(dv, normal); float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); // b2Clamp the accumulated impulse float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); lambda = newImpulse - vcp->normalImpulse; vcp->normalImpulse = newImpulse; // Apply contact impulse b2Vec2 P = lambda * normal; vA -= mA * P; wA -= iA * b2Cross(vcp->rA, P); vB += mB * P; wB += iB * b2Cross(vcp->rB, P); } 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 = vn0 - 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 = a + d // // a := old total impulse // x := new total impulse // d := incremental impulse // // For the current iteration we extend the formula for the incremental impulse // to compute the new total impulse: // // vn = A * d + b // = A * (x - a) + b // = A * x + b - A * a // = A * x + b' // b' = b - A * a; b2VelocityConstraintPoint* cp1 = vc->points + 0; b2VelocityConstraintPoint* cp2 = vc->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; // Compute b' b -= b2Mul(vc->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(vc->normalMass, b); if (x.x >= 0.0f && x.y >= 0.0f) { // Get the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; vA -= mA * (P1 + P2); wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += mB * (P1 + P2); wB += iB * (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 = vc->K.ex.y * x.x + b.y; if (x.x >= 0.0f && vn2 >= 0.0f) { // Get the incremental impulse b2Vec2 d = x - a; // Apply incremental impulse b2Vec2 P1 = d.x * normal; b2Vec2 P2 = d.y * normal; vA -= mA * (P1 + P2); wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += mB * (P1 + P2); wB += iB * (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 = vc->K.ey.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 -= mA * (P1 + P2); wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += mB * (P1 + P2); wB += iB * (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 -= mA * (P1 + P2); wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); vB += mB * (P1 + P2); wB += iB * (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; } } m_velocities[indexA].v = vA; m_velocities[indexA].w = wA; m_velocities[indexB].v = vB; m_velocities[indexB].w = wB; } }
void b2FrictionJoint::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); // Compute the effective mass matrix. 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; 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; } 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; }
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 b2Body::ResetMassData() { // 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; m_sweep.localCenter.SetZero(); // Static and kinematic bodies have zero mass. if (m_type == b2_staticBody || m_type == b2_kinematicBody) { m_sweep.c0 = m_xf.p; m_sweep.c = m_xf.p; m_sweep.a0 = m_sweep.a; return; } b2Assert(m_type == b2_dynamicBody); // Accumulate mass over all fixtures. b2Vec2 localCenter = b2Vec2_zero; for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { if (f->m_density == 0.0f) { continue; } b2MassData massData; f->GetMassData(&massData); m_mass += massData.mass; localCenter += massData.mass * massData.center; m_I += massData.I; } // Compute center of mass. if (m_mass > 0.0f) { m_invMass = 1.0f / m_mass; localCenter *= m_invMass; } else { // Force all dynamic bodies to have a positive mass. m_mass = 1.0f; m_invMass = 1.0f; } if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) { // Center the inertia about the center of mass. m_I -= m_mass * b2Dot(localCenter, localCenter); 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 = localCenter; 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); }
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf, int32 childIndex) const { B2_NOT_USED(childIndex); // Put the ray into the polygon's frame of reference. b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position); b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position); b2Vec2 d = p2 - p1; float32 lower = 0.0f, upper = input.maxFraction; int32 index = -1; for (int32 i = 0; i < m_vertexCount; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); float32 denominator = b2Dot(m_normals[i], d); if (denominator == 0.0f) { if (numerator < 0.0f) { return false; } } else { // Note: we want this predicate without division: // lower < numerator / denominator, where denominator < 0 // Since denominator < 0, we have to flip the inequality: // lower < numerator / denominator <==> denominator * lower > numerator. if (denominator < 0.0f && numerator < lower * denominator) { // Increase lower. // The segment enters this half-space. lower = numerator / denominator; index = i; } else if (denominator > 0.0f && numerator < upper * denominator) { // Decrease upper. // The segment exits this half-space. upper = numerator / denominator; } } // The use of epsilon here causes the assert on lower to trip // in some cases. Apparently the use of epsilon was to make edge // shapes work, but now those are handled separately. //if (upper < lower - b2_epsilon) if (upper < lower) { return false; } } b2Assert(0.0f <= lower && lower <= input.maxFraction); if (index >= 0) { output->fraction = lower; output->normal = b2Mul(xf.R, m_normals[index]); return true; } return false; }
void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step) { b2Body* bA = m_bodyA; b2Body* bB = m_bodyB; // Compute the effective mass matrix. b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); // 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 = bA->m_invMass, mB = bB->m_invMass; float32 iA = bA->m_invI, iB = bB->m_invI; b2Mat22 K1; K1.col1.x = mA + mB; K1.col2.x = 0.0f; K1.col1.y = 0.0f; K1.col2.y = mA + mB; b2Mat22 K2; K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y; K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x; b2Mat22 K3; K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y; K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x; b2Mat22 K = K1 + K2 + K3; m_linearMass = K.GetInverse(); m_angularMass = iA + iB; if (m_angularMass > 0.0f) { m_angularMass = 1.0f / m_angularMass; } if (step.warmStarting) { // Scale impulses to support a variable time step. m_linearImpulse *= step.dtRatio; m_angularImpulse *= step.dtRatio; b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); bA->m_linearVelocity -= mA * P; bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse); bB->m_linearVelocity += mB * P; bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse); } else { m_linearImpulse.SetZero(); m_angularImpulse = 0.0f; } }
b2Body::b2Body(const b2BodyDef* bd, b2World* world) { b2Assert(bd->position.IsValid()); b2Assert(bd->linearVelocity.IsValid()); b2Assert(b2IsValid(bd->angle)); b2Assert(b2IsValid(bd->angularVelocity)); b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f); b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); m_flags = 0; if (bd->bullet) { m_flags |= e_bulletFlag; } if (bd->fixedRotation) { m_flags |= e_fixedRotationFlag; } if (bd->allowSleep) { m_flags |= e_autoSleepFlag; } if (bd->awake) { m_flags |= e_awakeFlag; } if (bd->active) { m_flags |= e_activeFlag; } m_world = world; m_xf.position = bd->position; m_xf.R.Set(bd->angle); m_sweep.localCenter.SetZero(); m_sweep.a0 = m_sweep.a = bd->angle; m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); m_jointList = NULL; m_contactList = NULL; m_prev = NULL; m_next = NULL; m_linearVelocity = bd->linearVelocity; m_angularVelocity = bd->angularVelocity; m_linearDamping = bd->linearDamping; m_angularDamping = bd->angularDamping; m_force.SetZero(); m_torque = 0.0f; m_sleepTime = 0.0f; m_type = bd->type; if (m_type == b2_dynamicBody) { m_mass = 1.0f; m_invMass = 1.0f; } else { m_mass = 0.0f; m_invMass = 0.0f; } m_I = 0.0f; m_invI = 0.0f; m_userData = bd->userData; m_fixtureList = NULL; m_fixtureCount = 0; }
void b2CollidePolygonAndCircle( b2Manifold* manifold, const b2PolygonShape* polygonA, const b2Transform& xfA, const b2CircleShape* circleB, const b2Transform& xfB) { manifold->pointCount = 0; // Compute circle position in the frame of the polygon. b2Vec2 c = b2Mul(xfB, circleB->m_p); b2Vec2 cLocal = b2MulT(xfA, c); // Find the min separating edge. int normalIndex = 0; float separation = -b2_maxFloat; float radius = polygonA->m_radius + circleB->m_radius; int vertexCount = polygonA->m_count; const b2Vec2* vertices = polygonA->m_vertices; const b2Vec2* normals = polygonA->m_normals; for (int i = 0; i < vertexCount; ++i) { float s = b2Dot(normals[i], cLocal - vertices[i]); if (s > radius) { // Early out. return; } if (s > separation) { separation = s; normalIndex = i; } } // Vertices that subtend the incident face. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; b2Vec2 v1 = vertices[vertIndex1]; b2Vec2 v2 = vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < b2_epsilon) { manifold->pointCount = 1; manifold->type = b2Manifold::e_faceA; manifold->localNormal = normals[normalIndex]; manifold->localPoint = 0.5f * (v1 + v2); manifold->points[0].localPoint = circleB->m_p; manifold->points[0].id.key = 0; return; } // Compute barycentric coordinates float u1 = b2Dot(cLocal - v1, v2 - v1); float u2 = b2Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if (b2DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold->pointCount = 1; manifold->type = b2Manifold::e_faceA; manifold->localNormal = cLocal - v1; manifold->localNormal.Normalize(); manifold->localPoint = v1; manifold->points[0].localPoint = circleB->m_p; manifold->points[0].id.key = 0; } else if (u2 <= 0.0f) { if (b2DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold->pointCount = 1; manifold->type = b2Manifold::e_faceA; manifold->localNormal = cLocal - v2; manifold->localNormal.Normalize(); manifold->localPoint = v2; manifold->points[0].localPoint = circleB->m_p; manifold->points[0].id.key = 0; } else { b2Vec2 faceCenter = 0.5f * (v1 + v2); float separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]); if (separation > radius) { return; } manifold->pointCount = 1; manifold->type = b2Manifold::e_faceA; manifold->localNormal = normals[vertIndex1]; manifold->localPoint = faceCenter; manifold->points[0].localPoint = circleB->m_p; manifold->points[0].id.key = 0; } }
bool b2PulleyJoint::SolvePositionConstraints() { b2Body* b1 = m_body1; b2Body* b2 = m_body2; b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1; b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2; float32 linearError = 0.0f; if (m_state == e_atUpperLimit) { b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter()); b2Vec2 p1 = b1->m_sweep.c + r1; b2Vec2 p2 = b2->m_sweep.c + r2; // Get the pulley axes. m_u1 = p1 - s1; m_u2 = p2 - s2; float32 length1 = m_u1.Length(); float32 length2 = m_u2.Length(); if (length1 > b2_linearSlop) { m_u1 *= 1.0f / length1; } else { m_u1.SetZero(); } if (length2 > b2_linearSlop) { m_u2 *= 1.0f / length2; } else { m_u2.SetZero(); } float32 C = m_constant - length1 - m_ratio * length2; linearError = b2Max(linearError, -C); C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); float32 impulse = -m_pulleyMass * C; float32 oldImpulse = m_positionImpulse; m_positionImpulse = b2Max(0.0f, m_positionImpulse + impulse); impulse = m_positionImpulse - oldImpulse; b2Vec2 P1 = -impulse * m_u1; b2Vec2 P2 = -m_ratio * impulse * m_u2; b1->m_sweep.c += b1->m_invMass * P1; b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); b2->m_sweep.c += b2->m_invMass * P2; b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); b1->SynchronizeTransform(); b2->SynchronizeTransform(); } if (m_limitState1 == e_atUpperLimit) { b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 p1 = b1->m_sweep.c + r1; m_u1 = p1 - s1; float32 length1 = m_u1.Length(); if (length1 > b2_linearSlop) { m_u1 *= 1.0f / length1; } else { m_u1.SetZero(); } float32 C = m_maxLength1 - length1; linearError = b2Max(linearError, -C); C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); float32 impulse = -m_limitMass1 * C; float32 oldLimitPositionImpulse = m_limitPositionImpulse1; m_limitPositionImpulse1 = b2Max(0.0f, m_limitPositionImpulse1 + impulse); impulse = m_limitPositionImpulse1 - oldLimitPositionImpulse; b2Vec2 P1 = -impulse * m_u1; b1->m_sweep.c += b1->m_invMass * P1; b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1); b1->SynchronizeTransform(); } if (m_limitState2 == e_atUpperLimit) { b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter()); b2Vec2 p2 = b2->m_sweep.c + r2; m_u2 = p2 - s2; float32 length2 = m_u2.Length(); if (length2 > b2_linearSlop) { m_u2 *= 1.0f / length2; } else { m_u2.SetZero(); } float32 C = m_maxLength2 - length2; linearError = b2Max(linearError, -C); C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); float32 impulse = -m_limitMass2 * C; float32 oldLimitPositionImpulse = m_limitPositionImpulse2; m_limitPositionImpulse2 = b2Max(0.0f, m_limitPositionImpulse2 + impulse); impulse = m_limitPositionImpulse2 - oldLimitPositionImpulse; b2Vec2 P2 = -impulse * m_u2; b2->m_sweep.c += b2->m_invMass * P2; b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2); b2->SynchronizeTransform(); } return linearError < b2_linearSlop; }
void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step) { b2Body* b1 = m_body1; b2Body* b2 = m_body2; b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter()); b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter()); b2Vec2 p1 = b1->m_sweep.c + r1; b2Vec2 p2 = b2->m_sweep.c + r2; b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1; b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2; // Get the pulley axes. m_u1 = p1 - s1; m_u2 = p2 - s2; float32 length1 = m_u1.Length(); float32 length2 = m_u2.Length(); if (length1 > b2_linearSlop) { m_u1 *= 1.0f / length1; } else { m_u1.SetZero(); } if (length2 > b2_linearSlop) { m_u2 *= 1.0f / length2; } else { m_u2.SetZero(); } float32 C = m_constant - length1 - m_ratio * length2; if (C > 0.0f) { m_state = e_inactiveLimit; m_force = 0.0f; } else { m_state = e_atUpperLimit; m_positionImpulse = 0.0f; } if (length1 < m_maxLength1) { m_limitState1 = e_inactiveLimit; m_limitForce1 = 0.0f; } else { m_limitState1 = e_atUpperLimit; m_limitPositionImpulse1 = 0.0f; } if (length2 < m_maxLength2) { m_limitState2 = e_inactiveLimit; m_limitForce2 = 0.0f; } else { m_limitState2 = e_atUpperLimit; m_limitPositionImpulse2 = 0.0f; } // Compute effective mass. float32 cr1u1 = b2Cross(r1, m_u1); float32 cr2u2 = b2Cross(r2, m_u2); m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1; m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2; m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2; b2Assert(m_limitMass1 > B2_FLT_EPSILON); b2Assert(m_limitMass2 > B2_FLT_EPSILON); b2Assert(m_pulleyMass > B2_FLT_EPSILON); m_limitMass1 = 1.0f / m_limitMass1; m_limitMass2 = 1.0f / m_limitMass2; m_pulleyMass = 1.0f / m_pulleyMass; if (step.warmStarting) { // Warm starting. b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1; b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2; b1->m_linearVelocity += b1->m_invMass * P1; b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1); b2->m_linearVelocity += b2->m_invMass * P2; b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2); } else { m_force = 0.0f; m_limitForce1 = 0.0f; m_limitForce2 = 0.0f; } }
b2GearJoint::b2GearJoint(const b2GearJointDef* def) : b2Joint(def) { m_joint1 = def->joint1; m_joint2 = def->joint2; m_typeA = m_joint1->GetType(); m_typeB = m_joint2->GetType(); b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); float32 coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in b2Joint. m_bodyC = m_joint1->GetBodyA(); m_bodyA = m_joint1->GetBodyB(); // Get geometry of joint1 b2Transform xfA = m_bodyA->m_xf; float32 aA = m_bodyA->m_sweep.a; b2Transform xfC = m_bodyC->m_xf; float32 aC = m_bodyC->m_sweep.a; if (m_typeA == e_revoluteJoint) { b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; m_localAnchorC = revolute->m_localAnchorA; m_localAnchorA = revolute->m_localAnchorB; m_referenceAngleA = revolute->m_referenceAngle; m_localAxisC.SetZero(); coordinateA = aA - aC - m_referenceAngleA; } else { b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; m_localAnchorC = prismatic->m_localAnchorA; m_localAnchorA = prismatic->m_localAnchorB; m_referenceAngleA = prismatic->m_referenceAngle; m_localAxisC = prismatic->m_localXAxisA; b2Vec2 pC = m_localAnchorC; b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); coordinateA = b2Dot(pA - pC, m_localAxisC); } m_bodyD = m_joint2->GetBodyA(); m_bodyB = m_joint2->GetBodyB(); // Get geometry of joint2 b2Transform xfB = m_bodyB->m_xf; float32 aB = m_bodyB->m_sweep.a; b2Transform xfD = m_bodyD->m_xf; float32 aD = m_bodyD->m_sweep.a; if (m_typeB == e_revoluteJoint) { b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; m_localAnchorD = revolute->m_localAnchorA; m_localAnchorB = revolute->m_localAnchorB; m_referenceAngleB = revolute->m_referenceAngle; m_localAxisD.SetZero(); coordinateB = aB - aD - m_referenceAngleB; } else { b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; m_localAnchorD = prismatic->m_localAnchorA; m_localAnchorB = prismatic->m_localAnchorB; m_referenceAngleB = prismatic->m_referenceAngle; m_localAxisD = prismatic->m_localXAxisA; b2Vec2 pD = m_localAnchorD; b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); coordinateB = b2Dot(pB - pD, m_localAxisD); } m_ratio = def->ratio; m_constant = coordinateA + m_ratio * coordinateB; m_impulse = 0.0f; }
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 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; }
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; if (K.ez.z > 0.0f) { impulse = -K.Solve33(C); } else { b2Vec2 impulse2 = -K.Solve22(C1); impulse.Set(impulse2.x, impulse2.y, 0.0f); } 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; }
// Compute contact points for edge versus circle. // This accounts for edge connectivity. void b2CollideEdgeAndCircle(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2CircleShape* circleB, const b2Transform& xfB) { manifold->pointCount = 0; // Compute circle in frame of edge b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; b2Vec2 e = B - A; // Barycentric coordinates float32 u = b2Dot(e, B - Q); float32 v = b2Dot(e, Q - A); float32 radius = edgeA->m_radius + circleB->m_radius; b2ContactFeature cf; cf.indexB = 0; cf.typeB = b2ContactFeature::e_vertex; // Region A if (v <= 0.0f) { b2Vec2 P = A; b2Vec2 d = Q - P; float32 dd = b2Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA->m_hasVertex0) { b2Vec2 A1 = edgeA->m_vertex0; b2Vec2 B1 = A; b2Vec2 e1 = B1 - A1; float32 u1 = b2Dot(e1, B1 - Q); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.indexA = 0; cf.typeA = b2ContactFeature::e_vertex; manifold->pointCount = 1; manifold->type = b2Manifold::e_circles; manifold->localNormal.SetZero(); manifold->localPoint = P; manifold->points[0].id.key = 0; manifold->points[0].id.cf = cf; manifold->points[0].localPoint = circleB->m_p; return; } // Region B if (u <= 0.0f) { b2Vec2 P = B; b2Vec2 d = Q - P; float32 dd = b2Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA->m_hasVertex3) { b2Vec2 B2 = edgeA->m_vertex3; b2Vec2 A2 = B; b2Vec2 e2 = B2 - A2; float32 v2 = b2Dot(e2, Q - A2); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.indexA = 1; cf.typeA = b2ContactFeature::e_vertex; manifold->pointCount = 1; manifold->type = b2Manifold::e_circles; manifold->localNormal.SetZero(); manifold->localPoint = P; manifold->points[0].id.key = 0; manifold->points[0].id.cf = cf; manifold->points[0].localPoint = circleB->m_p; return; } // Region AB float32 den = b2Dot(e, e); b2Assert(den > 0.0f); b2Vec2 P = (1.0f / den) * (u * A + v * B); b2Vec2 d = Q - P; float32 dd = b2Dot(d, d); if (dd > radius * radius) { return; } b2Vec2 n(-e.y, e.x); if (b2Dot(n, Q - A) < 0.0f) { n.Set(-n.x, -n.y); } n.Normalize(); cf.indexA = 0; cf.typeA = b2ContactFeature::e_face; manifold->pointCount = 1; manifold->type = b2Manifold::e_faceA; manifold->localNormal = n; manifold->localPoint = A; manifold->points[0].id.key = 0; manifold->points[0].id.cf = cf; manifold->points[0].localPoint = circleB->m_p; }
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 if (K.ez.z == 0.0f) { K.GetInverse22(&m_mass); m_gamma = 0.0f; m_bias = 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; }
// The normal points from 1 to 2 void b2CollidePolygons(b2Manifold* manifold, const b2PolygonShape* polyA, const b2Transform& xfA, const b2PolygonShape* polyB, const b2Transform& xfB) { manifold->pointCount = 0; float32 totalRadius = polyA->m_radius + polyB->m_radius; int32 edgeA = 0; float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); if (separationA > totalRadius) return; int32 edgeB = 0; float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); if (separationB > totalRadius) return; const b2PolygonShape* poly1; // reference polygon const b2PolygonShape* poly2; // incident polygon b2Transform xf1, xf2; int32 edge1; // reference edge uint8 flip; const float32 k_relativeTol = 0.98f; const float32 k_absoluteTol = 0.001f; if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = edgeB; manifold->type = b2Manifold::e_faceB; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; manifold->type = b2Manifold::e_faceA; flip = 0; } b2ClipVertex incidentEdge[2]; b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); int32 count1 = poly1->m_vertexCount; const b2Vec2* vertices1 = poly1->m_vertices; int32 iv1 = edge1; int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; b2Vec2 v11 = vertices1[iv1]; b2Vec2 v12 = vertices1[iv2]; b2Vec2 localTangent = v12 - v11; localTangent.Normalize(); b2Vec2 localNormal = b2Cross(localTangent, 1.0f); b2Vec2 planePoint = 0.5f * (v11 + v12); b2Vec2 tangent = b2Mul(xf1.q, localTangent); b2Vec2 normal = b2Cross(tangent, 1.0f); v11 = b2Mul(xf1, v11); v12 = b2Mul(xf1, v12); // Face offset. float32 frontOffset = b2Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. b2ClipVertex clipPoints1[2]; b2ClipVertex clipPoints2[2]; int np; // Clip to box side 1 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) return; // Clip to negative box side 1 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold->localNormal = localNormal; manifold->localPoint = planePoint; int32 pointCount = 0; for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { b2ManifoldPoint* cp = manifold->points + pointCount; cp->localPoint = b2MulT(xf2, clipPoints2[i].v); cp->id = clipPoints2[i].id; if (flip) { // Swap features b2ContactFeature cf = cp->id.cf; cp->id.cf.indexA = cf.indexB; cp->id.cf.indexB = cf.indexA; cp->id.cf.typeA = cf.typeB; cp->id.cf.typeB = cf.typeA; } ++pointCount; } } manifold->pointCount = pointCount; }
void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step) { B2_NOT_USED(step); b2Body* bA = m_bodyA; b2Body* bB = m_bodyB; b2Vec2 vA = bA->m_linearVelocity; float32 wA = bA->m_angularVelocity; b2Vec2 vB = bB->m_linearVelocity; float32 wB = bB->m_angularVelocity; float32 mA = bA->m_invMass, mB = bB->m_invMass; float32 iA = bA->m_invI, iB = bB->m_invI; b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter()); b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter()); // Solve angular friction { float32 Cdot = wB - wA; float32 impulse = -m_angularMass * Cdot; float32 oldImpulse = m_angularImpulse; float32 maxImpulse = step.dt * m_maxTorque; m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_angularImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve linear friction { b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA); b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); b2Vec2 oldImpulse = m_linearImpulse; m_linearImpulse += impulse; float32 maxImpulse = step.dt * m_maxForce; if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.Normalize(); m_linearImpulse *= maxImpulse; } impulse = m_linearImpulse - oldImpulse; vA -= mA * impulse; wA -= iA * b2Cross(rA, impulse); vB += mB * impulse; wB += iB * b2Cross(rB, impulse); } bA->m_linearVelocity = vA; bA->m_angularVelocity = wA; bB->m_linearVelocity = vB; bB->m_angularVelocity = wB; }
// Find the max separation between poly1 and poly2 using edge normals from poly1. static float32 b2FindMaxSeparation(int32* edgeIndex, const b2PolygonShape* poly1, const b2Transform& xf1, const b2PolygonShape* poly2, const b2Transform& xf2) { int32 count1 = poly1->m_vertexCount; const b2Vec2* normals1 = poly1->m_normals; // Vector pointing from the centroid of poly1 to the centroid of poly2. b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid); b2Vec2 dLocal1 = b2MulT(xf1.q, d); // Find edge normal on poly1 that has the largest projection onto d. int32 edge = 0; float32 maxDot = -b2_maxFloat; for (int32 i = 0; i < count1; ++i) { float32 dot = b2Dot(normals1[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2); // Check the separation for the previous edge normal. int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); // Check the separation for the next edge normal. int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0; float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); // Find the best edge and the search direction. int32 bestEdge; float32 bestSeparation; int32 increment; if (sPrev > s && sPrev > sNext) { increment = -1; bestEdge = prevEdge; bestSeparation = sPrev; } else if (sNext > s) { increment = 1; bestEdge = nextEdge; bestSeparation = sNext; } else { *edgeIndex = edge; return s; } // Perform a local search for the best edge normal. for ( ; ; ) { if (increment == -1) edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; else edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2); if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } *edgeIndex = bestEdge; return bestSeparation; }
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; }
void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) { switch (fixture->GetType()) { case b2Shape::e_circle: { b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); b2Vec2 center = b2Mul(xf, circle->m_p); float32 radius = circle->m_radius; b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); m_debugDraw->DrawSolidCircle(center, radius, axis, color); } break; case b2Shape::e_edge: { b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); m_debugDraw->DrawSegment(v1, v2, color); } break; case b2Shape::e_chain: { b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); int32 count = chain->m_count; const b2Vec2* vertices = chain->m_vertices; b2Vec2 v1 = b2Mul(xf, vertices[0]); for (int32 i = 1; i < count; ++i) { b2Vec2 v2 = b2Mul(xf, vertices[i]); m_debugDraw->DrawSegment(v1, v2, color); m_debugDraw->DrawCircle(v1, 0.05f, color); v1 = v2; } } break; case b2Shape::e_polygon: { b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); int32 vertexCount = poly->m_vertexCount; b2Assert(vertexCount <= b2_maxPolygonVertices); b2Vec2 vertices[b2_maxPolygonVertices]; for (int32 i = 0; i < vertexCount; ++i) { vertices[i] = b2Mul(xf, poly->m_vertices[i]); } m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); } break; default: break; } }
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 b2ContactSolver::SolveVelocityConstraints() { for (int32 i = 0; i < m_constraintCount; ++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; } }
// Initialize position dependent portions of the velocity constraints. void b2ContactSolver::InitializeVelocityConstraints() { for (int32 i = 0; i < m_count; ++i) { b2ContactVelocityConstraint* vc = m_velocityConstraints + i; b2ContactPositionConstraint* pc = m_positionConstraints + i; float32 radiusA = pc->radiusA; float32 radiusB = pc->radiusB; b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); int32 indexA = vc->indexA; int32 indexB = vc->indexB; float32 mA = vc->invMassA; float32 mB = vc->invMassB; float32 iA = vc->invIA; float32 iB = vc->invIB; b2Vec2 localCenterA = pc->localCenterA; b2Vec2 localCenterB = pc->localCenterB; b2Vec2 cA = m_positions[indexA].c; float32 aA = m_positions[indexA].a; b2Vec2 vA = m_velocities[indexA].v; float32 wA = m_velocities[indexA].w; b2Vec2 cB = m_positions[indexB].c; float32 aB = m_positions[indexB].a; b2Vec2 vB = m_velocities[indexB].v; float32 wB = m_velocities[indexB].w; b2Assert(manifold->pointCount > 0); b2Transform xfA, xfB; xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - b2Mul(xfA.q, localCenterA); xfB.p = cB - b2Mul(xfB.q, localCenterB); b2WorldManifold worldManifold; worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); vc->normal = worldManifold.normal; int32 pointCount = vc->pointCount; for (int32 j = 0; j < pointCount; ++j) { b2VelocityConstraintPoint* vcp = vc->points + j; vcp->rA = worldManifold.points[j] - cA; vcp->rB = worldManifold.points[j] - cB; float32 rnA = b2Cross(vcp->rA, vc->normal); float32 rnB = b2Cross(vcp->rB, vc->normal); float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; b2Vec2 tangent = b2Cross(vc->normal, 1.0f); float32 rtA = b2Cross(vcp->rA, tangent); float32 rtB = b2Cross(vcp->rB, tangent); float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp->velocityBias = 0.0f; float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); if (vRel < -b2_velocityThreshold) { vcp->velocityBias = -vc->restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc->pointCount == 2) { b2VelocityConstraintPoint* vcp1 = vc->points + 0; b2VelocityConstraintPoint* vcp2 = vc->points + 1; float32 rn1A = b2Cross(vcp1->rA, vc->normal); float32 rn1B = b2Cross(vcp1->rB, vc->normal); float32 rn2A = b2Cross(vcp2->rA, vc->normal); float32 rn2B = b2Cross(vcp2->rB, vc->normal); float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float32 k12 = mA + mB + iA * rn1A * rn2A + iB * 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. vc->K.ex.Set(k11, k12); vc->K.ey.Set(k12, k22); vc->normalMass = vc->K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc->pointCount = 1; } } } }
void b2Distance(b2DistanceOutput* output, b2SimplexCache* cache, const b2DistanceInput* input) { ++b2_gjkCalls; const b2DistanceProxy* proxyA = &input->proxyA; const b2DistanceProxy* proxyB = &input->proxyB; b2Transform transformA = input->transformA; b2Transform transformB = input->transformB; // Initialize the simplex. b2Simplex simplex; simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. b2SimplexVertex* vertices = &simplex.m_v1; const int32 k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. int32 saveA[3], saveB[3]; int32 saveCount = 0; b2Vec2 closestPoint = simplex.GetClosestPoint(); float32 distanceSqr1 = closestPoint.LengthSquared(); float32 distanceSqr2 = distanceSqr1; // Main iteration loop. int32 iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int32 i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: b2Assert(false); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.m_count == 3) { break; } // Compute closest point. b2Vec2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. b2Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < b2_epsilon * b2_epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. b2SimplexVertex* vertex = vertices + simplex.m_count; vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); b2Vec2 wBLocal; vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; // Iteration count is equated to the number of support point calls. ++iter; ++b2_gjkIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int32 i = 0; i < saveCount; ++i) { if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.m_count; } b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(&output->pointA, &output->pointB); output->distance = b2Distance(output->pointA, output->pointB); output->iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input->useRadii) { float32 rA = proxyA->m_radius; float32 rB = proxyB->m_radius; if (output->distance > rA + rB && output->distance > b2_epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output->distance -= rA + rB; b2Vec2 normal = output->pointB - output->pointA; normal.Normalize(); output->pointA += rA * normal; output->pointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. b2Vec2 p = 0.5f * (output->pointA + output->pointB); output->pointA = p; output->pointB = p; output->distance = 0.0f; } } }
// Sequential position solver for position constraints. bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) { float32 minSeparation = 0.0f; for (int32 i = 0; i < m_count; ++i) { b2ContactPositionConstraint* pc = m_positionConstraints + i; int32 indexA = pc->indexA; int32 indexB = pc->indexB; b2Vec2 localCenterA = pc->localCenterA; b2Vec2 localCenterB = pc->localCenterB; int32 pointCount = pc->pointCount; float32 mA = 0.0f; float32 iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc->invMassA; iA = pc->invIA; } float32 mB = pc->invMassB; float32 iB = pc->invIB; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc->invMassB; iB = pc->invIB; } b2Vec2 cA = m_positions[indexA].c; float32 aA = m_positions[indexA].a; b2Vec2 cB = m_positions[indexB].c; float32 aB = m_positions[indexB].a; // Solve normal constraints for (int32 j = 0; j < pointCount; ++j) { b2Transform xfA, xfB; xfA.q.Set(aA); xfB.q.Set(aB); xfA.p = cA - b2Mul(xfA.q, localCenterA); xfB.p = cB - b2Mul(xfB.q, localCenterB); b2PositionSolverManifold psm; psm.Initialize(pc, xfA, xfB, j); b2Vec2 normal = psm.normal; b2Vec2 point = psm.point; float32 separation = psm.separation; b2Vec2 rA = point - cA; b2Vec2 rB = point - cB; // Track max constraint error. minSeparation = b2Min(minSeparation, separation); // Prevent large corrections and allow slop. float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); // Compute the effective mass. float32 rnA = b2Cross(rA, normal); float32 rnB = b2Cross(rB, normal); float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float32 impulse = K > 0.0f ? - C / K : 0.0f; b2Vec2 P = impulse * normal; cA -= mA * P; aA -= iA * b2Cross(rA, P); cB += mB * P; aB += iB * b2Cross(rB, P); } m_positions[indexA].c = cA; m_positions[indexA].a = aA; m_positions[indexB].c = cB; m_positions[indexB].a = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return minSeparation >= -1.5f * b2_linearSlop; }
void b2WorldManifold::Initialize(const b2Manifold* manifold, const b2Transform& xfA, float32 radiusA, const b2Transform& xfB, float32 radiusB) { if (manifold->pointCount == 0) { return; } switch (manifold->type) { case b2Manifold::e_circles: { normal.Set(1.0f, 0.0f); b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) { normal = pointB - pointA; normal.Normalize(); } b2Vec2 cA = pointA + radiusA * normal; b2Vec2 cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); separations[0] = b2Dot(cB - cA, normal); } break; case b2Manifold::e_faceA: { normal = b2Mul(xfA.q, manifold->localNormal); b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); for (int32 i = 0; i < manifold->pointCount; ++i) { b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; b2Vec2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); separations[i] = b2Dot(cB - cA, normal); } } break; case b2Manifold::e_faceB: { normal = b2Mul(xfB.q, manifold->localNormal); b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); for (int32 i = 0; i < manifold->pointCount; ++i) { b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; b2Vec2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); separations[i] = b2Dot(cA - cB, normal); } // Ensure normal points from A to B. normal = -normal; } break; } }
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; }
b2Body::b2Body(const b2BodyDef* bd, b2World* world) { m_flags = 0; if (bd->isBullet) { m_flags |= e_bulletFlag; } if (bd->fixedRotation) { m_flags |= e_fixedRotationFlag; } if (bd->allowSleep) { m_flags |= e_allowSleepFlag; } if (bd->isSleeping) { m_flags |= e_sleepFlag; } m_world = world; m_xf.position = bd->position; m_xf.R.Set(bd->angle); m_sweep.localCenter = bd->massData.center; m_sweep.t0 = 1.0f; m_sweep.a0 = m_sweep.a = bd->angle; m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); m_jointList = NULL; m_contactList = NULL; m_controllerList = NULL; m_prev = NULL; m_next = NULL; m_linearVelocity = bd->linearVelocity; m_angularVelocity = bd->angularVelocity; m_linearDamping = bd->linearDamping; m_angularDamping = bd->angularDamping; m_force.Set(0.0f, 0.0f); m_torque = 0.0f; m_linearVelocity.SetZero(); m_angularVelocity = 0.0f; m_sleepTime = 0.0f; m_invMass = 0.0f; m_I = 0.0f; m_invI = 0.0f; m_mass = bd->massData.mass; if (m_mass > 0.0f) { m_invMass = 1.0f / m_mass; } m_I = bd->massData.I; if (m_I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) { m_invI = 1.0f / m_I; } if (m_invMass == 0.0f && m_invI == 0.0f) { m_type = e_staticType; } else { m_type = e_dynamicType; } m_userData = bd->userData; m_fixtureList = NULL; m_fixtureCount = 0; }