void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) { m_indexB = m_bodyB->m_islandIndex; m_localCenterB = m_bodyB->m_sweep.localCenter; m_invMassB = m_bodyB->m_invMass; m_invIB = m_bodyB->m_invI; 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 qB(aB); float32 mass = m_bodyB->GetMass(); // Frequency float32 omega = 2.0f * b2_pi * m_frequencyHz; // Damping coefficient float32 d = 2.0f * mass * m_dampingRatio * omega; // Spring stiffness float32 k = mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. float32 h = data.step.dt; b2Assert(d + h * k > b2_epsilon); m_gamma = h * (d + h * k); if (m_gamma != 0.0f) { m_gamma = 1.0f / m_gamma; } m_beta = h * k * m_gamma; // Compute the effective mass matrix. m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] b2Mat22 K; K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; K.ex.y = -m_invIB * m_rB.x * m_rB.y; K.ey.x = K.ex.y; K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; m_mass = K.GetInverse(); m_C = cB + m_rB - m_targetA; m_C *= m_beta; // Cheat with some damping wB *= 0.98f; if (data.step.warmStarting) { m_impulse *= data.step.dtRatio; vB += m_invMassB * m_impulse; wB += m_invIB * b2Cross(m_rB, m_impulse); } else { m_impulse.SetZero(); } data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
void b2World::DestroyBody(b2Body* b) { b2Assert(m_bodyCount > 0); b2Assert(IsLocked() == false); if (IsLocked()) { return; } // Delete the attached joints. b2JointEdge* je = b->m_jointList; while (je) { b2JointEdge* je0 = je; je = je->next; if (m_destructionListener) { m_destructionListener->SayGoodbye(je0->joint); } DestroyJoint(je0->joint); b->m_jointList = je; } b->m_jointList = NULL; // Delete the attached contacts. b2ContactEdge* ce = b->m_contactList; while (ce) { b2ContactEdge* ce0 = ce; ce = ce->next; m_contactManager.Destroy(ce0->contact); } b->m_contactList = NULL; // Delete the attached fixtures. This destroys broad-phase proxies. b2Fixture* f = b->m_fixtureList; while (f) { b2Fixture* f0 = f; f = f->m_next; if (m_destructionListener) { m_destructionListener->SayGoodbye(f0); } f0->DestroyProxies(&m_contactManager.m_broadPhase); f0->Destroy(&m_blockAllocator); f0->~b2Fixture(); m_blockAllocator.Free(f0, sizeof(b2Fixture)); b->m_fixtureList = f; b->m_fixtureCount -= 1; } b->m_fixtureList = NULL; b->m_fixtureCount = 0; // Remove world body list. if (b->m_prev) { b->m_prev->m_next = b->m_next; } if (b->m_next) { b->m_next->m_prev = b->m_prev; } if (b == m_bodyList) { m_bodyList = b->m_next; } --m_bodyCount; b->~b2Body(); m_blockAllocator.Free(b, sizeof(b2Body)); }
// Find islands, integrate and solve constraints, solve position constraints void b2World::Solve(const b2TimeStep& step) { // Size the island for the worst case. b2Island island(m_bodyCount, m_contactManager.m_contactCount, m_jointCount, &m_stackAllocator, m_contactManager.m_contactListener); // Clear all the island flags. for (b2Body* b = m_bodyList; b; b = b->m_next) { b->m_flags &= ~b2Body::e_islandFlag; } for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) { c->m_flags &= ~b2Contact::e_islandFlag; } for (b2Joint* j = m_jointList; j; j = j->m_next) { j->m_islandFlag = false; } // Build and simulate all awake islands. b2_int32 stackSize = m_bodyCount; b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) { if (seed->m_flags & b2Body::e_islandFlag) { continue; } if (seed->IsAwake() == false || seed->IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed->GetType() == b2_staticBody) { continue; } // Reset island and stack. island.Clear(); b2_int32 stackCount = 0; stack[stackCount++] = seed; seed->m_flags |= b2Body::e_islandFlag; // Perform a depth first search (DFS) on the constraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. b2Body* b = stack[--stackCount]; b2Assert(b->IsActive() == true); island.Add(b); // Make sure the body is awake. b->SetAwake(true); // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b->GetType() == b2_staticBody) { continue; } // Search all contacts connected to this body. for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) { b2Contact* contact = ce->contact; // Has this contact already been added to an island? if (contact->m_flags & b2Contact::e_islandFlag) { continue; } // Is this contact solid and touching? if (contact->IsEnabled() == false || contact->IsTouching() == false) { continue; } // Skip sensors. bool sensorA = contact->m_fixtureA->m_isSensor; bool sensorB = contact->m_fixtureB->m_isSensor; if (sensorA || sensorB) { continue; } island.Add(contact); contact->m_flags |= b2Contact::e_islandFlag; b2Body* other = ce->other; // Was the other body already added to this island? if (other->m_flags & b2Body::e_islandFlag) { continue; } b2Assert(stackCount < stackSize); stack[stackCount++] = other; other->m_flags |= b2Body::e_islandFlag; } // Search all joints connect to this body. for (b2JointEdge* je = b->m_jointList; je; je = je->next) { if (je->joint->m_islandFlag == true) { continue; } b2Body* other = je->other; // Don't simulate joints connected to inactive bodies. if (other->IsActive() == false) { continue; } island.Add(je->joint); je->joint->m_islandFlag = true; if (other->m_flags & b2Body::e_islandFlag) { continue; } b2Assert(stackCount < stackSize); stack[stackCount++] = other; other->m_flags |= b2Body::e_islandFlag; } } island.Solve(step, m_gravity, m_allowSleep); // Post solve cleanup. for (b2_int32 i = 0; i < island.m_bodyCount; ++i) { // Allow static bodies to participate in other islands. b2Body* b = island.m_bodies[i]; if (b->GetType() == b2_staticBody) { b->m_flags &= ~b2Body::e_islandFlag; } } } m_stackAllocator.Free(stack); // Synchronize fixtures, check for out of range bodies. for (b2Body* b = m_bodyList; b; b = b->GetNext()) { // If a body was not in an island then it did not move. if ((b->m_flags & b2Body::e_islandFlag) == 0) { continue; } if (b->GetType() == b2_staticBody) { continue; } // Update fixtures (for broad-phase). b->SynchronizeFixtures(); } // Look for new contacts. m_contactManager.FindNewContacts(); }
b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) { m_step = def->step; m_allocator = def->allocator; m_count = def->count; m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint)); m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint)); m_positions = def->positions; m_velocities = def->velocities; m_contacts = def->contacts; // Initialize position independent portions of the constraints. for (int32 i = 0; i < m_count; ++i) { b2Contact* contact = m_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(); int32 pointCount = manifold->pointCount; b2Assert(pointCount > 0); b2ContactVelocityConstraint* vc = m_velocityConstraints + i; vc->friction = contact->m_friction; vc->restitution = contact->m_restitution; vc->tangentSpeed = contact->m_tangentSpeed; vc->indexA = bodyA->m_islandIndex; vc->indexB = bodyB->m_islandIndex; vc->invMassA = bodyA->m_invMass; vc->invMassB = bodyB->m_invMass; vc->invIA = bodyA->m_invI; vc->invIB = bodyB->m_invI; vc->contactIndex = i; vc->pointCount = pointCount; vc->K.SetZero(); vc->normalMass.SetZero(); b2ContactPositionConstraint* pc = m_positionConstraints + i; pc->indexA = bodyA->m_islandIndex; pc->indexB = bodyB->m_islandIndex; pc->invMassA = bodyA->m_invMass; pc->invMassB = bodyB->m_invMass; pc->localCenterA = bodyA->m_sweep.localCenter; pc->localCenterB = bodyB->m_sweep.localCenter; pc->invIA = bodyA->m_invI; pc->invIB = bodyB->m_invI; pc->localNormal = manifold->localNormal; pc->localPoint = manifold->localPoint; pc->pointCount = pointCount; pc->radiusA = radiusA; pc->radiusB = radiusB; pc->type = manifold->type; for (int32 j = 0; j < pointCount; ++j) { b2ManifoldPoint* cp = manifold->points + j; b2VelocityConstraintPoint* vcp = vc->points + j; if (m_step.warmStarting) { vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse; vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse; } else { vcp->normalImpulse = 0.0f; vcp->tangentImpulse = 0.0f; } vcp->rA.SetZero(); vcp->rB.SetZero(); vcp->normalMass = 0.0f; vcp->tangentMass = 0.0f; vcp->velocityBias = 0.0f; pc->localPoints[j] = cp->localPoint; } } }
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.q, input.p1 - xf.p); b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); 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.q, m_normals[index]); return true; } return false; }
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_sweep.c = m_xf.position; return; } b2Assert(m_type == b2_dynamicBody); // Accumulate mass over all fixtures. b2Vec2 center = 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; 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; } 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(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); }
// 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 && g_blockSolve) { 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; } } } }
// 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 b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { if (count < 3) { SetAsBox(0.01f, 0.01f); return; } count = b2Min(count, b2_maxPolygonVertices); // Copy vertices into local buffer b2Vec2 ps[b2_maxPolygonVertices]; for (int32 i = 0; i < count; ++i) { ps[i] = vertices[i]; } // 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 < count; ++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 < count; ++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; } } if (m < 3) { SetAsBox(0.01f, 0.01f); return; } 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); }
// TODO_ERIN adjust linear velocity and torque to account for movement of center. void b2Body::SetMassFromShapes() { // 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) { b2MassData massData; f->ComputeMass(&massData); m_mass += massData.mass; center += massData.mass * massData.center; m_I += massData.I; } // Compute center of mass, and shift the origin to the COM. 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. m_sweep.localCenter = center; m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); 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 refilter the broad-phase proxies. if (oldType != m_type) { for (b2Fixture* f = m_fixtureList; f; f = f->m_next) { f->RefilterProxy(m_world->m_broadPhase, m_xf); } } }
b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) { b2Joint* joint = NULL; switch (def->type) { case e_distanceJoint: { void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def); } break; case e_mouseJoint: { void* mem = allocator->Allocate(sizeof(b2MouseJoint)); joint = new (mem) b2MouseJoint((b2MouseJointDef*)def); } break; case e_prismaticJoint: { void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def); } break; case e_revoluteJoint: { void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def); } break; case e_pulleyJoint: { void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def); } break; case e_gearJoint: { void* mem = allocator->Allocate(sizeof(b2GearJoint)); joint = new (mem) b2GearJoint((b2GearJointDef*)def); } break; case e_wheelJoint: { void* mem = allocator->Allocate(sizeof(b2WheelJoint)); joint = new (mem) b2WheelJoint((b2WheelJointDef*)def); } break; case e_weldJoint: { void* mem = allocator->Allocate(sizeof(b2WeldJoint)); joint = new (mem) b2WeldJoint((b2WeldJointDef*)def); } break; case e_frictionJoint: { void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); joint = new (mem) b2FrictionJoint((b2FrictionJointDef*)def); } break; case e_ropeJoint: { void* mem = allocator->Allocate(sizeof(b2RopeJoint)); joint = new (mem) b2RopeJoint((b2RopeJointDef*)def); } break; default: b2Assert(false); break; } return joint; }
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.R, 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.R, 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; } }
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) { ++b2_toiCalls; output->state = b2TOIOutput::e_unknown; output->t = input->tMax; const b2DistanceProxy* proxyA = &input->proxyA; const b2DistanceProxy* proxyB = &input->proxyB; b2Sweep sweepA = input->sweepA; b2Sweep sweepB = input->sweepB; // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float32 tMax = input->tMax; float32 totalRadius = proxyA->m_radius + proxyB->m_radius; float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); float32 tolerance = 0.25f * b2_linearSlop; b2Assert(target > tolerance); float32 t1 = 0.0f; const int32 k_maxIterations = 20; // TODO_ERIN b2Settings int32 iter = 0; // Prepare input for distance query. b2SimplexCache cache; cache.count = 0; b2DistanceInput distanceInput; distanceInput.proxyA = input->proxyA; distanceInput.proxyB = input->proxyB; distanceInput.useRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for(;;) { b2Transform xfA, xfB; sweepA.GetTransform(&xfA, t1); sweepB.GetTransform(&xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. distanceInput.transformA = xfA; distanceInput.transformB = xfB; b2DistanceOutput distanceOutput; b2Distance(&distanceOutput, &cache, &distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output->state = b2TOIOutput::e_overlapped; output->t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output->state = b2TOIOutput::e_touching; output->t = t1; break; } // Initialize the separating axis. b2SeparationFunction fcn; fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); #if 0 // Dump the curve seen by the root finder { const int32 N = 100; float32 dx = 1.0f / N; float32 xs[N+1]; float32 fs[N+1]; float32 x = 0.0f; for (int32 i = 0; i <= N; ++i) { sweepA.GetTransform(&xfA, x); sweepB.GetTransform(&xfB, x); float32 f = fcn.Evaluate(xfA, xfB) - target; printf("%g %g\n", x, f); xs[i] = x; fs[i] = f; x += dx; } } #endif // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of vertices. bool done = false; float32 t2 = tMax; int32 pushBackIter = 0; for (;;) { // Find the deepest point at t2. Store the witness point indices. int32 indexA, indexB; float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output->state = b2TOIOutput::e_separated; output->t = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float32 s1 = fcn.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output->state = b2TOIOutput::e_failed; output->t = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // Victory! t1 should hold the TOI (could be 0.0). output->state = b2TOIOutput::e_touching; output->t = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int32 rootIterCount = 0; float32 a1 = t1, a2 = t2; for (;;) { // Use a mix of the secant rule and bisection. float32 t; if (rootIterCount & 1) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } float32 s = fcn.Evaluate(indexA, indexB, t); if (b2Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } ++rootIterCount; ++b2_toiRootIters; if (rootIterCount == 50) { break; } } b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == b2_maxPolygonVertices) { break; } } ++iter; ++b2_toiIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output->state = b2TOIOutput::e_failed; output->t = t1; break; } } b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); }
float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const { b2Transform xfA, xfB; m_sweepA.GetTransform(&xfA, t); m_sweepB.GetTransform(&xfB, t); switch (m_type) { case e_points: { b2Vec2 axisA = b2MulT(xfA.R, m_axis); b2Vec2 axisB = b2MulT(xfB.R, -m_axis); *indexA = m_proxyA->GetSupport(axisA); *indexB = m_proxyB->GetSupport(axisB); b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); b2Vec2 pointA = b2Mul(xfA, localPointA); b2Vec2 pointB = b2Mul(xfB, localPointB); float32 separation = b2Dot(pointB - pointA, m_axis); return separation; } case e_faceA: { b2Vec2 normal = b2Mul(xfA.R, m_axis); b2Vec2 pointA = b2Mul(xfA, m_localPoint); b2Vec2 axisB = b2MulT(xfB.R, -normal); *indexA = -1; *indexB = m_proxyB->GetSupport(axisB); b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); b2Vec2 pointB = b2Mul(xfB, localPointB); float32 separation = b2Dot(pointB - pointA, normal); return separation; } case e_faceB: { b2Vec2 normal = b2Mul(xfB.R, m_axis); b2Vec2 pointB = b2Mul(xfB, m_localPoint); b2Vec2 axisA = b2MulT(xfA.R, -normal); *indexB = -1; *indexA = m_proxyA->GetSupport(axisA); b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); b2Vec2 pointA = b2Mul(xfA, localPointA); float32 separation = b2Dot(pointA - pointB, normal); return separation; } default: b2Assert(false); *indexA = -1; *indexB = -1; return 0.0f; } }
void b2Body::DestroyFixture(b2Fixture* fixture) { b2Assert(m_world->IsLocked() == false); if (m_world->IsLocked() == true) { return; } b2Assert(fixture->m_body == this); // Remove the fixture from this body's singly linked list. b2Assert(m_fixtureCount > 0); b2Fixture** node = &m_fixtureList; bool found = false; while (*node != NULL) { if (*node == fixture) { *node = fixture->m_next; found = true; break; } node = &(*node)->m_next; } // You tried to remove a shape that is not attached to this body. b2Assert(found); // Destroy any contacts associated with the fixture. b2ContactEdge* edge = m_contactList; while (edge) { b2Contact* c = edge->contact; edge = edge->next; b2Fixture* fixtureA = c->GetFixtureA(); b2Fixture* fixtureB = c->GetFixtureB(); if (fixture == fixtureA || fixture == fixtureB) { // This destroys the contact and removes it from // this body's contact list. m_world->m_contactManager.Destroy(c); } } b2BlockAllocator* allocator = &m_world->m_blockAllocator; if (m_flags & e_activeFlag) { b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy); b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; fixture->DestroyProxy(broadPhase); } else { b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy); } fixture->Destroy(allocator); fixture->m_body = NULL; fixture->m_next = NULL; fixture->~b2Fixture(); allocator->Free(fixture, sizeof(b2Fixture)); --m_fixtureCount; // Reset the mass data. ResetMassData(); }
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: wB = 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; } }
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; }
b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator) { m_step = step; 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->m_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.m_normal; cc->pointCount = manifold->m_pointCount; cc->friction = friction; cc->restitution = restitution; cc->localPlaneNormal = manifold->m_localPlaneNormal; cc->localPoint = manifold->m_localPoint; cc->radius = radiusA + radiusB; cc->type = manifold->m_type; for (int32 j = 0; j < cc->pointCount; ++j) { b2ManifoldPoint* cp = manifold->m_points + j; b2ContactConstraintPoint* ccp = cc->points + j; ccp->normalImpulse = cp->m_normalImpulse; ccp->tangentImpulse = cp->m_tangentImpulse; ccp->localPoint = cp->m_localPoint; ccp->rA = worldManifold.m_points[j] - bodyA->m_sweep.c; ccp->rB = worldManifold.m_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_FLT_EPSILON); ccp->normalMass = 1.0f / kNormal; float32 kEqualized = bodyA->m_mass * bodyA->m_invMass + bodyB->m_mass * bodyB->m_invMass; kEqualized += bodyA->m_mass * bodyA->m_invI * rnA + bodyB->m_mass * bodyB->m_invI * rnB; b2Assert(kEqualized > B2_FLT_EPSILON); ccp->equalizedMass = 1.0f / kEqualized; 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_FLT_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 = 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; } } } }
b2Fixture::~b2Fixture() { b2Assert(m_shape == NULL); b2Assert(m_proxyId == b2BroadPhase::e_nullProxy); }
void b2GearJoint::SetRatio(float32 ratio) { b2Assert(b2IsValid(ratio)); m_ratio = ratio; }
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) - vc->tangentSpeed; 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 (pointCount == 1 || g_blockSolve == false) { 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 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, 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; } }
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; }
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_count; 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 b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) { b2Assert(toiIndexA < m_bodyCount); b2Assert(toiIndexB < m_bodyCount); // Initialize the body state. for (int32 i = 0; i < m_bodyCount; ++i) { b2Body* b = m_bodies[i]; m_positions[i].c = b->m_sweep.c; m_positions[i].a = b->m_sweep.a; m_velocities[i].v = b->m_linearVelocity; m_velocities[i].w = b->m_angularVelocity; } b2ContactSolverDef contactSolverDef; contactSolverDef.contacts = m_contacts; contactSolverDef.count = m_contactCount; contactSolverDef.allocator = m_allocator; contactSolverDef.step = subStep; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; b2ContactSolver contactSolver(&contactSolverDef); // Solve position constraints. for (int32 i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } #if 0 // Is the new position really safe? for (int32 i = 0; i < m_contactCount; ++i) { b2Contact* c = m_contacts[i]; b2Fixture* fA = c->GetFixtureA(); b2Fixture* fB = c->GetFixtureB(); b2Body* bA = fA->GetBody(); b2Body* bB = fB->GetBody(); int32 indexA = c->GetChildIndexA(); int32 indexB = c->GetChildIndexB(); b2DistanceInput input; input.proxyA.Set(fA->GetShape(), indexA); input.proxyB.Set(fB->GetShape(), indexB); input.transformA = bA->GetTransform(); input.transformB = bB->GetTransform(); input.useRadii = false; b2DistanceOutput output; b2SimplexCache cache; cache.count = 0; b2Distance(&output, &cache, &input); if (output.distance == 0 || cache.count == 3) { cache.count += 0; } } #endif // Leap of faith to new safe state. m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int32 i = 0; i < subStep.velocityIterations; ++i) { contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float32 h = subStep.dt; // Integrate positions for (int32 i = 0; i < m_bodyCount; ++i) { b2Vec2 c = m_positions[i].c; float32 a = m_positions[i].a; b2Vec2 v = m_velocities[i].v; float32 w = m_velocities[i].w; // Check for large velocities b2Vec2 translation = h * v; if (b2Dot(translation, translation) > b2_maxTranslationSquared) { float32 ratio = b2_maxTranslation / translation.Length(); v *= ratio; } float32 rotation = h * w; if (rotation * rotation > b2_maxRotationSquared) { float32 ratio = b2_maxRotation / b2Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; m_positions[i].c = c; m_positions[i].a = a; m_velocities[i].v = v; m_velocities[i].w = w; // Sync bodies b2Body* body = m_bodies[i]; body->m_sweep.c = c; body->m_sweep.a = a; body->m_linearVelocity = v; body->m_angularVelocity = w; body->SynchronizeTransform(); } Report(contactSolver.m_velocityConstraints); }
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_vertexCount >= 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_vertexCount; ++i) { s += m_vertices[i]; } s *= 1.0f / m_vertexCount; const float32 k_inv3 = 1.0f / 3.0f; for (int32 i = 0; i < m_vertexCount; ++i) { // Triangle vertices. b2Vec2 e1 = m_vertices[i] - s; b2Vec2 e2 = i + 1 < m_vertexCount ? 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)); }
b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) : b2Contact(fixtureA, fixtureB) { b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); }
void b2World::DestroyJoint(b2Joint* j) { b2Assert(IsLocked() == false); if (IsLocked()) { return; } bool collideConnected = j->m_collideConnected; // Remove from the doubly linked list. if (j->m_prev) { j->m_prev->m_next = j->m_next; } if (j->m_next) { j->m_next->m_prev = j->m_prev; } if (j == m_jointList) { m_jointList = j->m_next; } // Disconnect from island graph. b2Body* bodyA = j->m_bodyA; b2Body* bodyB = j->m_bodyB; // Wake up connected bodies. bodyA->SetAwake(true); bodyB->SetAwake(true); // Remove from body 1. if (j->m_edgeA.prev) { j->m_edgeA.prev->next = j->m_edgeA.next; } if (j->m_edgeA.next) { j->m_edgeA.next->prev = j->m_edgeA.prev; } if (&j->m_edgeA == bodyA->m_jointList) { bodyA->m_jointList = j->m_edgeA.next; } j->m_edgeA.prev = NULL; j->m_edgeA.next = NULL; // Remove from body 2 if (j->m_edgeB.prev) { j->m_edgeB.prev->next = j->m_edgeB.next; } if (j->m_edgeB.next) { j->m_edgeB.next->prev = j->m_edgeB.prev; } if (&j->m_edgeB == bodyB->m_jointList) { bodyB->m_jointList = j->m_edgeB.next; } j->m_edgeB.prev = NULL; j->m_edgeB.next = NULL; b2Joint::Destroy(j, &m_blockAllocator); b2Assert(m_jointCount > 0); --m_jointCount; // If the joint prevents collisions, then flag any contacts for filtering. if (collideConnected == false) { b2ContactEdge* edge = bodyB->GetContactList(); while (edge) { if (edge->other == bodyA) { // Flag the contact for filtering at the next time step (where either // body is awake). edge->contact->FlagForFiltering(); } edge = edge->next; } } }
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; } }
// Find TOI contacts and solve them. void b2World::SolveTOI(const b2TimeStep& step) { b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); if (m_stepComplete) { for (b2Body* b = m_bodyList; b; b = b->m_next) { b->m_flags &= ~b2Body::e_islandFlag; b->m_sweep.alpha0 = 0.0f; } for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) { // Invalidate TOI c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); c->m_toiCount = 0; c->m_toi = 1.0f; } } // Find TOI events and solve them. for (;;) { // Find the first TOI. b2Contact* minContact = NULL; b2_float32 minAlpha = 1.0f; for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) { // Is this contact disabled? if (c->IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c->m_toiCount > b2_maxSubSteps) { continue; } b2_float32 alpha = 1.0f; if (c->m_flags & b2Contact::e_toiFlag) { // This contact has a valid cached TOI. alpha = c->m_toi; } else { b2Fixture* fA = c->GetFixtureA(); b2Fixture* fB = c->GetFixtureB(); // Is there a sensor? if (fA->IsSensor() || fB->IsSensor()) { continue; } b2Body* bA = fA->GetBody(); b2Body* bB = fB->GetBody(); b2BodyType typeA = bA->m_type; b2BodyType typeB = bB->m_type; b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); bool activeA = bA->IsAwake() && typeA != b2_staticBody; bool activeB = bB->IsAwake() && typeB != b2_staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } // Compute the TOI for this contact. // Put the sweeps onto the same time interval. b2_float32 alpha0 = bA->m_sweep.alpha0; if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) { alpha0 = bB->m_sweep.alpha0; bA->m_sweep.Advance(alpha0); } else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) { alpha0 = bA->m_sweep.alpha0; bB->m_sweep.Advance(alpha0); } b2Assert(alpha0 < 1.0f); b2_int32 indexA = c->GetChildIndexA(); b2_int32 indexB = c->GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] b2TOIInput input; input.proxyA.Set(fA->GetShape(), indexA); input.proxyB.Set(fB->GetShape(), indexB); input.sweepA = bA->m_sweep; input.sweepB = bB->m_sweep; input.tMax = 1.0f; b2TOIOutput output; b2TimeOfImpact(&output, &input); // Beta is the fraction of the remaining portion of the . b2_float32 beta = output.t; if (output.state == b2TOIOutput::e_touching) { alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c->m_toi = alpha; c->m_flags |= b2Contact::e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } // Advance the bodies to the TOI. b2Fixture* fA = minContact->GetFixtureA(); b2Fixture* fB = minContact->GetFixtureB(); b2Body* bA = fA->GetBody(); b2Body* bB = fB->GetBody(); b2Sweep backup1 = bA->m_sweep; b2Sweep backup2 = bB->m_sweep; bA->Advance(minAlpha); bB->Advance(minAlpha); // The TOI contact likely has some new contact points. minContact->Update(m_contactManager.m_contactListener); minContact->m_flags &= ~b2Contact::e_toiFlag; ++minContact->m_toiCount; // Is the contact solid? if (minContact->IsEnabled() == false || minContact->IsTouching() == false) { // Restore the sweeps. minContact->SetEnabled(false); bA->m_sweep = backup1; bB->m_sweep = backup2; bA->SynchronizeTransform(); bB->SynchronizeTransform(); continue; } bA->SetAwake(true); bB->SetAwake(true); // Build the island island.Clear(); island.Add(bA); island.Add(bB); island.Add(minContact); bA->m_flags |= b2Body::e_islandFlag; bB->m_flags |= b2Body::e_islandFlag; minContact->m_flags |= b2Contact::e_islandFlag; // Get contacts on bodyA and bodyB. b2Body* bodies[2] = {bA, bB}; for (b2_int32 i = 0; i < 2; ++i) { b2Body* body = bodies[i]; if (body->m_type == b2_dynamicBody) { for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) { if (island.m_bodyCount == island.m_bodyCapacity) { break; } if (island.m_contactCount == island.m_contactCapacity) { break; } b2Contact* contact = ce->contact; // Has this contact already been added to the island? if (contact->m_flags & b2Contact::e_islandFlag) { continue; } // Only add static, kinematic, or bullet bodies. b2Body* other = ce->other; if (other->m_type == b2_dynamicBody && body->IsBullet() == false && other->IsBullet() == false) { continue; } // Skip sensors. bool sensorA = contact->m_fixtureA->m_isSensor; bool sensorB = contact->m_fixtureB->m_isSensor; if (sensorA || sensorB) { continue; } // Tentatively advance the body to the TOI. b2Sweep backup = other->m_sweep; if ((other->m_flags & b2Body::e_islandFlag) == 0) { other->Advance(minAlpha); } // Update the contact points contact->Update(m_contactManager.m_contactListener); // Was the contact disabled by the user? if (contact->IsEnabled() == false) { other->m_sweep = backup; other->SynchronizeTransform(); continue; } // Are there contact points? if (contact->IsTouching() == false) { other->m_sweep = backup; other->SynchronizeTransform(); continue; } // Add the contact to the island contact->m_flags |= b2Contact::e_islandFlag; island.Add(contact); // Has the other body already been added to the island? if (other->m_flags & b2Body::e_islandFlag) { continue; } // Add the other body to the island. other->m_flags |= b2Body::e_islandFlag; if (other->m_type != b2_staticBody) { other->SetAwake(true); } island.Add(other); } } } b2TimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; subStep.positionIterations = 20; subStep.velocityIterations = step.velocityIterations; subStep.warmStarting = false; island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (b2_int32 i = 0; i < island.m_bodyCount; ++i) { b2Body* body = island.m_bodies[i]; body->m_flags &= ~b2Body::e_islandFlag; if (body->m_type != b2_dynamicBody) { continue; } body->SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) { ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_contactManager.FindNewContacts(); if (m_subStepping) { m_stepComplete = false; break; } } }
/// Free a b2TrackedBlock. void b2TrackedBlock::Free(b2TrackedBlock *block) { b2Assert(block); block->~b2TrackedBlock(); b2Free(block); }