void Island::SolveTOI(const PTimeStep& subStep, s32 toiIndexA, s32 toiIndexB) { assert(toiIndexA < m_bodyCount); assert(toiIndexB < m_bodyCount); // Initialize the body state. for (s32 i = 0; i < m_bodyCount; ++i) { Body* 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; } ContactSolverDef contactSolverDef; contactSolverDef.contacts = m_contacts; contactSolverDef.count = m_contactCount; contactSolverDef.allocator = m_allocator; contactSolverDef.step = subStep; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; ContactSolver contactSolver(&contactSolverDef); // Solve position constraints. for (s32 i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } #if 0 // Is the new position really safe? for (s32 i = 0; i < m_contactCount; ++i) { Contact* c = m_contacts[i]; Fixture* fA = c->GetFixtureA(); Fixture* fB = c->GetFixtureB(); Body* bA = fA->GetBody(); Body* bB = fB->GetBody(); s32 indexA = c->GetChildIndexA(); s32 indexB = c->GetChildIndexB(); DistanceInput input; input.proxyA.Set(fA->GetShape(), indexA); input.proxyB.Set(fB->GetShape(), indexB); input.Transform2DA = bA->GetTransform2D(); input.Transform2DB = bB->GetTransform2D(); input.useRadii = false; DistanceOutput output; SimplexCache cache; cache.count = 0; Distance(&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 (s32 i = 0; i < subStep.velocityIterations; ++i) { contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. real32 h = subStep.delta; // Integrate positions for (s32 i = 0; i < m_bodyCount; ++i) { glm::vec2 c = m_positions[i].c; real32 a = m_positions[i].a; glm::vec2 v = m_velocities[i].v; real32 w = m_velocities[i].w; // Check for large velocities glm::vec2 translation = h * v; if (glm::dot(translation, translation) > maxTranslationSquared) { real32 ratio = maxTranslation / translation.length(); v *= ratio; } real32 rotation = h * w; if (rotation * rotation > maxRotationSquared) { real32 ratio = maxRotation / glm::abs(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 Body* body = m_bodies[i]; body->m_sweep.c = c; body->m_sweep.a = a; body->m_linearVelocity = v; body->m_angularVelocity = w; body->SynchronizeTransform2D(); } Report(contactSolver.m_velocityConstraints); }
// This is the top level collision call for the time step. Here // all the narrow phase collision is processed for the world // contact list. void ContactManager::Collide() { // Update awake contacts. Contact* c = m_contactList; while (c) { Fixture* fixtureA = c->GetFixtureA(); Fixture* fixtureB = c->GetFixtureB(); s32 indexA = c->GetChildIndexA(); s32 indexB = c->GetChildIndexB(); Body* bodyA = fixtureA->GetBody(); Body* bodyB = fixtureB->GetBody(); // Is this contact flagged for filtering? if (c->m_flags & Contact::filterFlag) { // Should these bodies collide? if (bodyB->ShouldCollide(bodyA) == false) { Contact* cNuke = c; c = cNuke->GetNext(); Destroy(cNuke); continue; } // Check user filtering. if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) { Contact* cNuke = c; c = cNuke->GetNext(); Destroy(cNuke); continue; } // Clear the filtering flag. c->m_flags &= ~Contact::filterFlag; } bool activeA = bodyA->IsAwake() && bodyA->m_type != staticBody; bool activeB = bodyB->IsAwake() && bodyB->m_type != staticBody; // At least one body must be awake and it must be dynamic or kinematic. if (activeA == false && activeB == false) { c = c->GetNext(); continue; } s32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; s32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); // Here we destroy contacts that cease to overlap in the broad-phase. if (overlap == false) { Contact* cNuke = c; c = cNuke->GetNext(); Destroy(cNuke); continue; } // The contact persists. c->Update(m_contactListener); c = c->GetNext(); } }
void Body::DestroyFixture(Fixture* fixture) { assert(m_world->IsLocked() == false); if (m_world->IsLocked() == true) { return; } assert(fixture->m_body == this); // Remove the fixture from this body's singly linked list. assert(m_fixtureCount > 0); Fixture** 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. assert(found); // Destroy any contacts associated with the fixture. ContactEdge* edge = m_contactList; while (edge) { Contact* c = edge->contact; edge = edge->next; Fixture* fixtureA = c->GetFixtureA(); Fixture* 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); } } BlockAllocator* allocator = &m_world->m_blockAllocator; if (m_flags & activeFlag) { BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; fixture->DestroyProxies(broadPhase); } fixture->Destroy(allocator); fixture->m_body = NULL; fixture->m_next = NULL; fixture->~Fixture(); allocator->Free(fixture, sizeof(Fixture)); --m_fixtureCount; // Reset the mass data. ResetMassData(); }
void ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) { FixtureProxy* proxyA = (FixtureProxy*)proxyUserDataA; FixtureProxy* proxyB = (FixtureProxy*)proxyUserDataB; Fixture* fixtureA = proxyA->fixture; Fixture* fixtureB = proxyB->fixture; s32 indexA = proxyA->childIndex; s32 indexB = proxyB->childIndex; Body* bodyA = fixtureA->GetBody(); Body* bodyB = fixtureB->GetBody(); // Are the fixtures on the same body? if (bodyA == bodyB) { return; } // TODO_ERIN use a hash table to remove a potential bottleneck when both // bodies have a lot of contacts. // Does a contact already exist? ContactEdge* edge = bodyB->GetContactList(); while (edge) { if (edge->other == bodyA) { Fixture* fA = edge->contact->GetFixtureA(); Fixture* fB = edge->contact->GetFixtureB(); s32 iA = edge->contact->GetChildIndexA(); s32 iB = edge->contact->GetChildIndexB(); if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { // A contact already exists. return; } if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { // A contact already exists. return; } } edge = edge->next; } // Does a joint override collision? Is at least one body dynamic? if (bodyB->ShouldCollide(bodyA) == false) { return; } // Check user filtering. if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) { return; } // Call the factory. Contact* c = Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); if (c == NULL) { return; } // Contact creation may swap fixtures. fixtureA = c->GetFixtureA(); fixtureB = c->GetFixtureB(); indexA = c->GetChildIndexA(); indexB = c->GetChildIndexB(); bodyA = fixtureA->GetBody(); bodyB = fixtureB->GetBody(); // Insert into the world. c->m_prev = NULL; c->m_next = m_contactList; if (m_contactList != NULL) { m_contactList->m_prev = c; } m_contactList = c; // Connect to island graph. // Connect to body A c->m_nodeA.contact = c; c->m_nodeA.other = bodyB; c->m_nodeA.prev = NULL; c->m_nodeA.next = bodyA->m_contactList; if (bodyA->m_contactList != NULL) { bodyA->m_contactList->prev = &c->m_nodeA; } bodyA->m_contactList = &c->m_nodeA; // Connect to body B c->m_nodeB.contact = c; c->m_nodeB.other = bodyA; c->m_nodeB.prev = NULL; c->m_nodeB.next = bodyB->m_contactList; if (bodyB->m_contactList != NULL) { bodyB->m_contactList->prev = &c->m_nodeB; } bodyB->m_contactList = &c->m_nodeB; // Wake up the bodies if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) { bodyA->SetAwake(true); bodyB->SetAwake(true); } ++m_contactCount; }