void PhysicsWorld::FindAllCollisions() { contactManager.numOldContacts = contactManager.numContacts; assert(contactManager.numOldContacts < MaxNumContacts ); memcpy(contactManager.oldContacts, contactManager.contacts, contactManager.numOldContacts * sizeof(Contact)); contactManager.numContacts = 0; /* O(n^2) collsion detection ( or worse LOL ) Check all fixtures vs all fixtures for collisions, We dont collide fixtures that are from the same body */ for ( size_t i = 0; i < numBodies; i++ ) { for ( size_t j = i + 1; j < numBodies; j++ ) { if ( (bodies[i]->type != eDynamic && bodies[j]->type != eDynamic) ) { continue; } if ( contactManager.contactFilter && !contactManager.contactFilter->ShouldCollide(bodies[i], bodies[j]) ) { continue; } Fixture *fi = bodies[i]->GetFixtureList(); while ( fi ) { Fixture *fj = bodies[j]->GetFixtureList(); while ( fj ) { if ( fi->IsSensor( ) || fj->IsSensor( ) ) { if ( CheckOverlap(fi->GetShape(), bodies[i]->GetPosition(), fj->GetShape(), bodies[j]->GetPosition()) ) { contactManager.contactListener->OnSensor(fi, fj); } } else { // check collision and add to the contact list if they intersect contactManager.AddPair(fi, fj); } fj = fj->GetNext(); } fi = fi->GetNext(); } } } contactManager.MergeContacts(); }
void Contact::Destroy(Contact* contact, BlockAllocator* allocator) { assert(s_initialized == true); Fixture* fixtureA = contact->m_fixtureA; Fixture* fixtureB = contact->m_fixtureB; if (contact->m_manifold.pointCount > 0 && fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) { fixtureA->GetBody()->SetAwake(true); fixtureB->GetBody()->SetAwake(true); } Shape::Type typeA = fixtureA->GetType(); Shape::Type typeB = fixtureB->GetType(); assert(0 <= typeA && typeB < Shape::e_typeCount); assert(0 <= typeA && typeB < Shape::e_typeCount); ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; destroyFcn(contact, allocator); }
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; }