From bcf305b118c32f644636d2ba2b9f802eee83ae66 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 9 Sep 2018 21:59:02 +0200 Subject: [PATCH] Refactor contact points creation --- CMakeLists.txt | 2 - src/collision/CollisionDetection.cpp | 371 ++++-- src/collision/CollisionDetection.h | 12 +- ....sync-conflict-20180707-081346-ARAT43F.cpp | 1084 +++++++++++++++++ src/collision/ContactManifold.cpp | 287 ++++- src/collision/ContactManifold.h | 16 +- src/collision/ContactManifoldInfo.cpp | 305 ----- src/collision/ContactManifoldInfo.h | 117 -- src/collision/ContactManifoldSet.cpp | 147 +-- src/collision/ContactManifoldSet.h | 15 +- src/collision/NarrowPhaseInfo.cpp | 6 - src/collision/NarrowPhaseInfo.h | 3 - src/containers/Set.h | 50 +- src/engine/DynamicsWorld.cpp | 3 +- src/engine/OverlappingPair.cpp | 96 +- src/engine/OverlappingPair.h | 36 +- test/tests/containers/TestSet.h | 17 +- 17 files changed, 1698 insertions(+), 869 deletions(-) create mode 100644 src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp delete mode 100644 src/collision/ContactManifoldInfo.cpp delete mode 100644 src/collision/ContactManifoldInfo.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 147a6a73..efa0bc04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,7 +81,6 @@ SET (REACTPHYSICS3D_HEADERS "src/body/CollisionBody.h" "src/body/RigidBody.h" "src/collision/ContactPointInfo.h" - "src/collision/ContactManifoldInfo.h" "src/collision/broadphase/BroadPhaseAlgorithm.h" "src/collision/broadphase/DynamicAABBTree.h" "src/collision/narrowphase/CollisionDispatch.h" @@ -167,7 +166,6 @@ SET (REACTPHYSICS3D_SOURCES "src/body/Body.cpp" "src/body/CollisionBody.cpp" "src/body/RigidBody.cpp" - "src/collision/ContactManifoldInfo.cpp" "src/collision/broadphase/BroadPhaseAlgorithm.cpp" "src/collision/broadphase/DynamicAABBTree.cpp" "src/collision/narrowphase/DefaultCollisionDispatch.cpp" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 277d4647..c835c4e7 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -37,7 +37,6 @@ #include "collision/OverlapCallback.h" #include "collision/NarrowPhaseInfo.h" #include "collision/ContactManifold.h" -#include "collision/ContactManifoldInfo.h" #include "utils/Profiler.h" #include "engine/EventListener.h" #include "collision/RaycastInfo.h" @@ -107,8 +106,7 @@ void CollisionDetection::computeMiddlePhase() { RP3D_PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); // For each possible collision pair of bodies - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { OverlappingPair* pair = it->second; @@ -130,9 +128,9 @@ void CollisionDetection::computeMiddlePhase() { if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Destroy the overlapping pair - it->second->~OverlappingPair(); + pair->~OverlappingPair(); - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); it = mOverlappingPairs.remove(it); continue; } @@ -256,6 +254,8 @@ void CollisionDetection::computeNarrowPhase() { RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); + List narrowPhaseInfos(mMemoryManager.getSingleFrameAllocator()); + NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; while (currentNarrowPhaseInfo != nullptr) { @@ -269,14 +269,13 @@ void CollisionDetection::computeNarrowPhase() { LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo(); + narrowPhaseInfos.add(currentNarrowPhaseInfo); + // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) { - // Add the contact points as a potential contact manifold into the pair - currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - lastCollisionFrameInfo->wasColliding = true; } else { @@ -287,24 +286,29 @@ void CollisionDetection::computeNarrowPhase() { lastCollisionFrameInfo->isValid = true; } - NarrowPhaseInfo* narrowPhaseInfoToDelete = currentNarrowPhaseInfo; currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; - - // Call the destructor - narrowPhaseInfoToDelete->~NarrowPhaseInfo(); - - // Release the allocated memory for the narrow phase info - mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); } // Convert the potential contact into actual contacts - processAllPotentialContacts(); + processAllPotentialContacts(narrowPhaseInfos, mOverlappingPairs); // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); // Report contacts to the user reportAllContacts(); + + // Destroy the narrow phase infos + for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } } // Allow the broadphase to notify the collision detection about an overlapping pair. @@ -329,8 +333,10 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig); + assert(newPair != nullptr); + // Add the new overlapping pair mOverlappingPairs.add(Pair, OverlappingPair*>(pairID, newPair)); // Wake up the two bodies @@ -344,16 +350,18 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { assert(proxyShape->getBroadPhaseId() != -1); // Remove all the overlapping pairs involving this proxy shape - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { - if (it->second->getShape1()->getBroadPhaseId() == proxyShape->getBroadPhaseId()|| - it->second->getShape2()->getBroadPhaseId() == proxyShape->getBroadPhaseId()) { + for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + + OverlappingPair* pair = it->second; + + if (pair->getShape1()->getBroadPhaseId() == proxyShape->getBroadPhaseId()|| + pair->getShape2()->getBroadPhaseId() == proxyShape->getBroadPhaseId()) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair - it->second->~OverlappingPair(); - mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); + pair->~OverlappingPair(); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); it = mOverlappingPairs.remove(it); } else { @@ -370,8 +378,7 @@ void CollisionDetection::addAllContactManifoldsToBodies() { RP3D_PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler); // For each overlapping pairs in contact during the narrow-phase - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { // Add all the contact manifolds of the pair into the list of contact manifolds // of the two bodies involved in the contact @@ -430,42 +437,36 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { } /// Convert the potential contact into actual contacts -void CollisionDetection::processAllPotentialContacts() { +void CollisionDetection::processAllPotentialContacts(const List& narrowPhaseInfos, + const OverlappingPairMap& overlappingPairs) { RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); - // For each overlapping pairs in contact during the narrow-phase - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + // For each narrow phase info object + for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { - // Process the potential contacts of the overlapping pair - processPotentialContacts(it->second); + NarrowPhaseInfo* narrowPhaseInfo = *it; + + assert(narrowPhaseInfo != nullptr); + + if (narrowPhaseInfo->contactPoints != nullptr) { + + // Transfer the contact points from the narrow phase info to the overlapping pair + narrowPhaseInfo->overlappingPair->addPotentialContactPoints(narrowPhaseInfo); + } } -} -// Process the potential contact manifold of a pair to create actual contact manifold -void CollisionDetection::processPotentialContacts(OverlappingPair* pair) { + // For each overlapping pairs in contact during the narrow-phase + for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { - // Reduce the number of contact points of the manifold - pair->reducePotentialContactManifolds(); + OverlappingPair* pair = it->second; - // Add all the potential contact manifolds as actual contact manifolds to the pair - ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds(); - while (potentialManifold != nullptr) { + // Clear the obsolete contact manifolds and contact points + pair->clearObsoleteManifoldsAndContactPoints(); - pair->addContactManifold(potentialManifold); - - potentialManifold = potentialManifold->mNext; - } - - // Clear the obsolete contact manifolds and contact points - pair->clearObsoleteManifoldsAndContactPoints(); - - // Reduce the contact manifolds and contact points if there are too many of them - pair->reduceContactManifolds(); - - // Reset the potential contacts of the pair - pair->clearPotentialContactManifolds(); + // Reduce the contact manifolds and contact points if there are too many of them + pair->reduceContactManifolds(); + } } // Report contacts for all the colliding overlapping pairs @@ -474,13 +475,14 @@ void CollisionDetection::reportAllContacts() { RP3D_PROFILE("CollisionDetection::reportAllContacts()", mProfiler); // For each overlapping pairs in contact during the narrow-phase - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + + OverlappingPair* pair = it->second; // If there is a user callback - if (mWorld->mEventListener != nullptr && it->second->hasContacts()) { + if (mWorld->mEventListener != nullptr && pair->hasContacts()) { - CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mMemoryManager); + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); // Trigger a callback event to report the new contact to the user mWorld->mEventListener->newContact(collisionInfo); @@ -751,6 +753,10 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body assert(collisionCallback != nullptr); + List collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + List allNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); + // For each proxy shape proxy shape of the first body ProxyShape* body1ProxyShape = body1->getProxyShapesList(); while (body1ProxyShape != nullptr) { @@ -766,16 +772,36 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Test if the AABBs of the two proxy shapes overlap if (aabb1.testCollision(aabb2)) { - // Create a temporary overlapping pair - OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); + OverlappingPair* pair; + const Pair pairID = OverlappingPair::computeID(body1ProxyShape, body2ProxyShape); + + // Try to retrieve a corresponding copy of the overlapping pair (if it exists) + auto itPair = overlappingPairs.find(pairID); + + // If a copy of the overlapping pair does not exist yet + if (itPair == overlappingPairs.end()) { + + // Create a temporary copy of the overlapping pair + pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + overlappingPairs.add(Pair, OverlappingPair*>(pairID, pair)); + } + else { // If a temporary copy of this overlapping pair already exists + + // Retrieve the existing copy of the overlapping pair + pair = itPair->second; + } // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + allNarrowPhaseInfos.add(narrowPhaseInfo); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); @@ -790,30 +816,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // notifyContact() callback method will be called. if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { - // Add the contact points as a potential contact manifold into the pair - narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + collidingNarrowPhaseInfos.add(narrowPhaseInfo); } } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; - - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory - mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - - // Process the potential contacts - processPotentialContacts(&pair); - - if (pair.hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); - collisionCallback->notifyContact(collisionInfo); - } } // Go to the next proxy shape @@ -823,6 +831,38 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Go to the next proxy shape body1ProxyShape = body1ProxyShape->getNext(); } + + // Process the potential contacts + processAllPotentialContacts(collidingNarrowPhaseInfos, overlappingPairs); + + // For each overlapping pair + for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { + + OverlappingPair* pair = it->second; + + if (pair->hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); + collisionCallback->notifyContact(collisionInfo); + } + + // Destroy the temporary overlapping pair + pair->~OverlappingPair(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); + } + + // Destroy the narrow phase infos + for (auto it = allNarrowPhaseInfos.begin(); it != allNarrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } } // Test and report collisions between a body and all the others bodies of the world @@ -830,6 +870,10 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c assert(callback != nullptr); + List collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + List allNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); + // For each proxy shape proxy shape of the body ProxyShape* bodyProxyShape = body->getProxyShapesList(); while (bodyProxyShape != nullptr) { @@ -859,16 +903,36 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // Check if the collision filtering allows collision between the two shapes if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { - // Create a temporary overlapping pair - OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); + OverlappingPair* pair; + const Pair pairID = OverlappingPair::computeID(bodyProxyShape, proxyShape); + + // Try to retrieve a corresponding copy of the overlapping pair (if it exists) + auto itPair = overlappingPairs.find(pairID); + + // If a copy of the overlapping pair does not exist yet + if (itPair == overlappingPairs.end()) { + + // Create a temporary overlapping pair + pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + overlappingPairs.add(Pair, OverlappingPair*>(pairID, pair)); + } + else { // If a temporary copy of this overlapping pair already exists + + // Retrieve the existing copy of the overlapping pair + pair = itPair->second; + } // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + allNarrowPhaseInfos.add(narrowPhaseInfo); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); @@ -883,29 +947,11 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c // notifyContact() callback method will be called. if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { - // Add the contact points as a potential contact manifold into the pair - narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + collidingNarrowPhaseInfos.add(narrowPhaseInfo); } } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; - - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory - mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); - } - - // Process the potential contacts - processPotentialContacts(&pair); - - if (pair.hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); - callback->notifyContact(collisionInfo); } } } @@ -918,6 +964,38 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c bodyProxyShape = bodyProxyShape->getNext(); } } + + // Process the potential contacts + processAllPotentialContacts(collidingNarrowPhaseInfos, overlappingPairs); + + // For each overlapping pair + for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { + + OverlappingPair* pair = it->second; + + if (pair->hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); + callback->notifyContact(collisionInfo); + } + + // Destroy the temporary overlapping pair + pair->~OverlappingPair(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); + } + + // Destroy the narrow phase infos + for (auto it = allNarrowPhaseInfos.begin(); it != allNarrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } } // Test and report collisions between all shapes of the world @@ -928,18 +1006,39 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Compute the broad-phase collision detection computeBroadPhase(); + List collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + List allNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator()); + // For each possible collision pair of bodies - Map, OverlappingPair*>::Iterator it; - for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { OverlappingPair* originalPair = it->second; - // Create a new overlapping pair so that we do not work on the original one - OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(), - mMemoryManager.getPoolAllocator(), mWorld->mConfig); + OverlappingPair* pair; + const Pair pairID = OverlappingPair::computeID(originalPair->getShape1(), originalPair->getShape2()); - ProxyShape* shape1 = pair.getShape1(); - ProxyShape* shape2 = pair.getShape2(); + // Try to retrieve a corresponding copy of the overlapping pair (if it exists) + auto itPair = overlappingPairs.find(pairID); + + // If a copy of the overlapping pair does not exist yet + if (itPair == overlappingPairs.end()) { + + // Create a temporary overlapping pair + pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + overlappingPairs.add(Pair, OverlappingPair*>(pairID, pair)); + } + else { // If a temporary copy of this overlapping pair already exists + + // Retrieve the existing copy of the overlapping pair + pair = itPair->second; + } + + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. @@ -948,11 +1047,13 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // Compute the middle-phase collision detection between the two shapes - NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); // For each narrow-phase info object while (narrowPhaseInfo != nullptr) { + allNarrowPhaseInfos.add(narrowPhaseInfo); + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); @@ -967,32 +1068,46 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // notifyContact() callback method will be called. if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { - // Add the contact points as a potential contact manifold into the pair - narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); + collidingNarrowPhaseInfos.add(narrowPhaseInfo); } } - NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; narrowPhaseInfo = narrowPhaseInfo->next; - - // Call the destructor - currentNarrowPhaseInfo->~NarrowPhaseInfo(); - - // Release the allocated memory - mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); } - - // Process the potential contacts - processPotentialContacts(&pair); - - if (pair.hasContacts()) { - - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mMemoryManager); - callback->notifyContact(collisionInfo); - } } } + + // Process the potential contacts + processAllPotentialContacts(collidingNarrowPhaseInfos, overlappingPairs); + + // For each overlapping pair + for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { + + OverlappingPair* pair = it->second; + + if (pair->hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); + callback->notifyContact(collisionInfo); + } + + // Destroy the temporary overlapping pair + pair->~OverlappingPair(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); + } + + // Destroy the narrow phase infos + for (auto it = allNarrowPhaseInfos.begin(); it != allNarrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } } // Fill-in the collision detection matrix diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index a6c36ab8..6a72bf0b 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -59,6 +59,8 @@ class CollisionDetection { private : + using OverlappingPairMap = Map, OverlappingPair*>; + // -------------------- Attributes -------------------- // /// Memory manager @@ -80,7 +82,7 @@ class CollisionDetection { NarrowPhaseInfo* mNarrowPhaseInfoList; /// Broad-phase overlapping pairs - Map, OverlappingPair*> mOverlappingPairs; + OverlappingPairMap mOverlappingPairs; /// Broad-phase algorithm BroadPhaseAlgorithm mBroadPhaseAlgorithm; @@ -131,17 +133,15 @@ class CollisionDetection { NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair); /// Convert the potential contact into actual contacts - void processAllPotentialContacts(); - - /// Process the potential contact manifold of a pair to create actual contact manifold - void processPotentialContacts(OverlappingPair* pair); + void processAllPotentialContacts(const List& narrowPhaseInfos, + const OverlappingPairMap& overlappingPairs); /// Report contacts for all the colliding overlapping pairs void reportAllContacts(); /// Process the potential contacts where one collion is a concave shape void processSmoothMeshContacts(OverlappingPair* pair); - + public : // -------------------- Methods -------------------- // diff --git a/src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp b/src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp new file mode 100644 index 00000000..7dfb2f72 --- /dev/null +++ b/src/collision/CollisionDetection.sync-conflict-20180707-081346-ARAT43F.cpp @@ -0,0 +1,1084 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2018 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "CollisionDetection.h" +#include "engine/CollisionWorld.h" +#include "collision/OverlapCallback.h" +#include "body/Body.h" +#include "collision/shapes/BoxShape.h" +#include "collision/shapes/ConcaveShape.h" +#include "body/RigidBody.h" +#include "configuration.h" +#include "collision/CollisionCallback.h" +#include "collision/MiddlePhaseTriangleCallback.h" +#include "collision/OverlapCallback.h" +#include "collision/NarrowPhaseInfo.h" +#include "collision/ContactManifold.h" +#include "collision/ContactManifoldInfo.h" +#include "utils/Profiler.h" +#include "engine/EventListener.h" +#include "collision/RaycastInfo.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; +using namespace std; + + +// Constructor +CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager) + : mMemoryManager(memoryManager), mWorld(world), mNarrowPhaseInfoList(nullptr), + mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseAlgorithm(*this), + mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false) { + + // Set the default collision dispatch configuration + setCollisionDispatch(&mDefaultCollisionDispatch); + + // Fill-in the collision detection matrix with algorithms + fillInCollisionMatrix(); + +#ifdef IS_PROFILING_ACTIVE + + mProfiler = nullptr; + +#endif + +} + +// Compute the collision detection +void CollisionDetection::computeCollisionDetection() { + + RP3D_PROFILE("CollisionDetection::computeCollisionDetection()", mProfiler); + + // Compute the broad-phase collision detection + computeBroadPhase(); + + // Compute the middle-phase collision detection + computeMiddlePhase(); + + // Compute the narrow-phase collision detection + computeNarrowPhase(); + + // Reset the linked list of narrow-phase info + mNarrowPhaseInfoList = nullptr; +} + +// Compute the broad-phase collision detection +void CollisionDetection::computeBroadPhase() { + + RP3D_PROFILE("CollisionDetection::computeBroadPhase()", mProfiler); + + // If new collision shapes have been added to bodies + if (mIsCollisionShapesAdded) { + + // Ask the broad-phase to recompute the overlapping pairs of collision + // shapes. This call can only add new overlapping pairs in the collision + // detection. + mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryManager); + } +} + +// Compute the middle-phase collision detection +void CollisionDetection::computeMiddlePhase() { + + RP3D_PROFILE("CollisionDetection::computeMiddlePhase()", mProfiler); + + // For each possible collision pair of bodies + Map, OverlappingPair*>::Iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + + OverlappingPair* pair = it->second; + + // Make all the contact manifolds and contact points of the pair obsolete + pair->makeContactsObsolete(); + + // Make all the last frame collision info obsolete + pair->makeLastFrameCollisionInfosObsolete(); + + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); + + assert(shape1->getBroadPhaseId() != -1); + assert(shape2->getBroadPhaseId() != -1); + assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId()); + + // Check if the two shapes are still overlapping. Otherwise, we destroy the + // overlapping pair + if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + + // Destroy the overlapping pair + it->second->~OverlappingPair(); + + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); + it = mOverlappingPairs.remove(it); + continue; + } + else { + ++it; + } + + // Check if the collision filtering allows collision between the two shapes + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) { + + CollisionBody* const body1 = shape1->getBody(); + CollisionBody* const body2 = shape2->getBody(); + + // Check that at least one body is awake and not static + bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC; + bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC; + if (!isBody1Active && !isBody2Active) continue; + + // Check if the bodies are in the set of bodies that cannot collide between each other + bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); + if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; + + bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + bool isShape2Convex = shape2->getCollisionShape()->isConvex(); + + // If both shapes are convex + if (isShape1Convex && isShape2Convex) { + + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + NarrowPhaseInfo* firstNarrowPhaseInfo = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = new (mMemoryManager.allocate(MemoryManager::AllocationType::Frame, sizeof(NarrowPhaseInfo))) + NarrowPhaseInfo(pair, shape1->getCollisionShape(), + shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), mMemoryManager.getSingleFrameAllocator()); + mNarrowPhaseInfoList->next = firstNarrowPhaseInfo; + + } + // Concave vs Convex algorithm + else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { + + NarrowPhaseInfo* narrowPhaseInfo = nullptr; + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), &narrowPhaseInfo); + + // Add all the narrow-phase info object reported by the callback into the + // list of all the narrow-phase info object + while (narrowPhaseInfo != nullptr) { + NarrowPhaseInfo* next = narrowPhaseInfo->next; + narrowPhaseInfo->next = mNarrowPhaseInfoList; + mNarrowPhaseInfoList = narrowPhaseInfo; + + narrowPhaseInfo = next; + } + } + // Concave vs Concave shape + else { + // Not handled + continue; + } + + // Remove the obsolete last frame collision infos + pair->clearObsoleteLastFrameCollisionInfos(); + } + } +} + +// Compute the concave vs convex middle-phase algorithm for a given pair of bodies +void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator, + NarrowPhaseInfo** firstNarrowPhaseInfo) { + + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); + + ProxyShape* convexProxyShape; + ProxyShape* concaveProxyShape; + const ConvexShape* convexShape; + const ConcaveShape* concaveShape; + + // Collision shape 1 is convex, collision shape 2 is concave + if (shape1->getCollisionShape()->isConvex()) { + convexProxyShape = shape1; + convexShape = static_cast(shape1->getCollisionShape()); + concaveProxyShape = shape2; + concaveShape = static_cast(shape2->getCollisionShape()); + } + else { // Collision shape 2 is convex, collision shape 1 is concave + convexProxyShape = shape2; + convexShape = static_cast(shape2->getCollisionShape()); + concaveProxyShape = shape1; + concaveShape = static_cast(shape1->getCollisionShape()); + } + + // Set the parameters of the callback object + MiddlePhaseTriangleCallback middlePhaseCallback(pair, concaveProxyShape, convexProxyShape, + concaveShape, allocator); + +#ifdef IS_PROFILING_ACTIVE + + // Set the profiler + middlePhaseCallback.setProfiler(mProfiler); + +#endif + + // Compute the convex shape AABB in the local-space of the convex shape + const Transform convexToConcaveTransform = concaveProxyShape->getLocalToWorldTransform().getInverse() * + convexProxyShape->getLocalToWorldTransform(); + AABB aabb; + convexShape->computeAABB(aabb, convexToConcaveTransform); + + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape->testAllTriangles(middlePhaseCallback, aabb); + + // Add all the narrow-phase info object reported by the callback into the + // list of all the narrow-phase info object + *firstNarrowPhaseInfo = middlePhaseCallback.narrowPhaseInfoList; +} + +// Compute the narrow-phase collision detection +void CollisionDetection::computeNarrowPhase() { + + RP3D_PROFILE("CollisionDetection::computeNarrowPhase()", mProfiler); + + List narrowPhaseInfos(mMemoryManager.getSingleFrameAllocator()); + + NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList; + while (currentNarrowPhaseInfo != nullptr) { + + // Select the narrow phase algorithm to use according to the two collision shapes + const CollisionShapeType shape1Type = currentNarrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = currentNarrowPhaseInfo->collisionShape2->getType(); + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + + // If there is no collision algorithm between those two kinds of shapes, skip it + if (narrowPhaseAlgorithm != nullptr) { + + LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo(); + + narrowPhaseInfos.add(currentNarrowPhaseInfo); + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true, mMemoryManager.getSingleFrameAllocator())) { + + lastCollisionFrameInfo->wasColliding = true; + } + else { + lastCollisionFrameInfo->wasColliding = false; + } + + // The previous frame collision info is now valid + lastCollisionFrameInfo->isValid = true; + } + + currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; + } + + // Convert the potential contact into actual contacts + processAllPotentialContacts(narrowPhaseInfos, mMemoryManager.getSingleFrameAllocator()); + + // Add all the contact manifolds (between colliding bodies) to the bodies + addAllContactManifoldsToBodies(); + + // Report contacts to the user + reportAllContacts(); + + // Destroy the narrow phase infos + for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Frame, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } +} + +// Allow the broadphase to notify the collision detection about an overlapping pair. +/// This method is called by the broad-phase collision detection algorithm +void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { + + assert(shape1->getBroadPhaseId() != -1); + assert(shape2->getBroadPhaseId() != -1); + assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId()); + + // Check if the collision filtering allows collision between the two shapes + if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; + + // Compute the overlapping pair ID + Pair pairID = OverlappingPair::computeID(shape1, shape2); + + // Check if the overlapping pair already exists + if (mOverlappingPairs.containsKey(pairID)) return; + + // Create the overlapping pair and add it into the set of overlapping pairs + OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(), + mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig); + assert(newPair != nullptr); + + mOverlappingPairs.add(Pair, OverlappingPair*>(pairID, newPair)); + + // Wake up the two bodies + shape1->getBody()->setIsSleeping(false); + shape2->getBody()->setIsSleeping(false); +} + +// Remove a body from the collision detection +void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { + + assert(proxyShape->getBroadPhaseId() != -1); + + // Remove all the overlapping pairs involving this proxy shape + Map, OverlappingPair*>::Iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) { + if (it->second->getShape1()->getBroadPhaseId() == proxyShape->getBroadPhaseId()|| + it->second->getShape2()->getBroadPhaseId() == proxyShape->getBroadPhaseId()) { + + // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved + + // Destroy the overlapping pair + it->second->~OverlappingPair(); + mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, it->second, sizeof(OverlappingPair)); + it = mOverlappingPairs.remove(it); + } + else { + ++it; + } + } + + // Remove the body from the broad-phase + mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); +} + +void CollisionDetection::addAllContactManifoldsToBodies() { + + RP3D_PROFILE("CollisionDetection::addAllContactManifoldsToBodies()", mProfiler); + + // For each overlapping pairs in contact during the narrow-phase + Map, OverlappingPair*>::Iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + + // Add all the contact manifolds of the pair into the list of contact manifolds + // of the two bodies involved in the contact + addContactManifoldToBody(it->second); + } +} + +// Ray casting method +void CollisionDetection::raycast(RaycastCallback* raycastCallback, + const Ray& ray, + unsigned short raycastWithCategoryMaskBits) const { + + RP3D_PROFILE("CollisionDetection::raycast()", mProfiler); + + RaycastTest rayCastTest(raycastCallback); + + // Ask the broad-phase algorithm to call the testRaycastAgainstShape() + // callback method for each proxy shape hit by the ray in the broad-phase + mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); +} + +// Add a contact manifold to the linked list of contact manifolds of the two bodies involved +// in the corresponding contact +void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { + + assert(pair != nullptr); + + CollisionBody* body1 = pair->getShape1()->getBody(); + CollisionBody* body2 = pair->getShape2()->getBody(); + const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); + + // For each contact manifold in the set of manifolds in the pair + ContactManifold* contactManifold = manifoldSet.getContactManifolds(); + while (contactManifold != nullptr) { + + assert(contactManifold->getNbContactPoints() > 0); + + // Add the contact manifold at the beginning of the linked + // list of contact manifolds of the first body + ContactManifoldListElement* listElement1 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) + ContactManifoldListElement(contactManifold, + body1->mContactManifoldsList); + body1->mContactManifoldsList = listElement1; + + // Add the contact manifold at the beginning of the linked + // list of the contact manifolds of the second body + ContactManifoldListElement* listElement2 = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(ContactManifoldListElement))) + ContactManifoldListElement(contactManifold, + body2->mContactManifoldsList); + body2->mContactManifoldsList = listElement2; + + contactManifold = contactManifold->getNext(); + } +} + +/// Convert the potential contact into actual contacts +void CollisionDetection::processAllPotentialContacts(const List& narrowPhaseInfos, + MemoryAllocator& tempMemoryAllocator) { + + RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler); + + Set collidingPairs(tempMemoryAllocator); + + // For each narrow phase info object + for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + assert(narrowPhaseInfo != nullptr); + + if (narrowPhaseInfo->contactPoints != nullptr) { + + // Transfer the contact points from the narrow phase info to the overlapping pair + narrowPhaseInfo->overlappingPair->addPotentialContactPoints(narrowPhaseInfo); + + // Store the colliding pair + collidingPairs.add(narrowPhaseInfo->overlappingPair); + } + } + + // For each overlapping pairs in contact during the narrow-phase + for (auto it = collidingPairs.begin(); it != collidingPairs.end(); ++it) { + + OverlappingPair* pair = *it; + + // Clear the obsolete contact manifolds and contact points + pair->clearObsoleteManifoldsAndContactPoints(); + + // Reduce the contact manifolds and contact points if there are too many of them + pair->reduceContactManifolds(); + + // Reset the potential contacts of the pair + pair->clearPotentialContactManifolds(); + } +} + +// Report contacts for all the colliding overlapping pairs +void CollisionDetection::reportAllContacts() { + + RP3D_PROFILE("CollisionDetection::reportAllContacts()", mProfiler); + + // For each overlapping pairs in contact during the narrow-phase + Map, OverlappingPair*>::Iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + + // If there is a user callback + if (mWorld->mEventListener != nullptr && it->second->hasContacts()) { + + CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mMemoryManager); + + // Trigger a callback event to report the new contact to the user + mWorld->mEventListener->newContact(collisionInfo); + } + } +} + +// Compute the middle-phase collision detection between two proxy shapes +NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) { + + ProxyShape* shape1 = pair->getShape1(); + ProxyShape* shape2 = pair->getShape2(); + + // ------------------------------------------------------- + + const bool isShape1Convex = shape1->getCollisionShape()->isConvex(); + const bool isShape2Convex = shape2->getCollisionShape()->isConvex(); + + NarrowPhaseInfo* narrowPhaseInfo = nullptr; + + pair->makeLastFrameCollisionInfosObsolete(); + + // If both shapes are convex + if ((isShape1Convex && isShape2Convex)) { + + // No middle-phase is necessary, simply create a narrow phase info + // for the narrow-phase collision detection + narrowPhaseInfo = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, + sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(), + shape2->getCollisionShape(), shape1->getLocalToWorldTransform(), + shape2->getLocalToWorldTransform(), mMemoryManager.getPoolAllocator()); + + } + // Concave vs Convex algorithm + else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) { + + // Run the middle-phase collision detection algorithm to find the triangles of the concave + // shape we need to use during the narrow-phase collision detection + computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getPoolAllocator(), &narrowPhaseInfo); + } + + pair->clearObsoleteLastFrameCollisionInfos(); + + return narrowPhaseInfo; +} + +// Report all the bodies that overlap with the aabb in parameter +void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, + unsigned short categoryMaskBits) { + assert(overlapCallback != nullptr); + + Set reportedBodies(mMemoryManager.getPoolAllocator()); + + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes); + + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { + + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + + CollisionBody* overlapBody = proxyShape->getBody(); + + // If the proxy shape is from a body that we have not already reported collision + if (reportedBodies.find(overlapBody->getId()) == reportedBodies.end()) { + + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + + // Add the body into the set of reported bodies + reportedBodies.add(overlapBody->getId()); + + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } + } + + // Go to the next overlapping proxy shape + element = element->next; + } +} + +// Return true if two bodies overlap +bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2) { + + // For each proxy shape proxy shape of the first body + ProxyShape* body1ProxyShape = body1->getProxyShapesList(); + while (body1ProxyShape != nullptr) { + + AABB aabb1 = body1ProxyShape->getWorldAABB(); + + // For each proxy shape of the second body + ProxyShape* body2ProxyShape = body2->getProxyShapesList(); + while (body2ProxyShape != nullptr) { + + AABB aabb2 = body2ProxyShape->getWorldAABB(); + + // Test if the AABBs of the two proxy shapes overlap + if (aabb1.testCollision(aabb2)) { + + // Create a temporary overlapping pair + OverlappingPair pair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + + bool isColliding = false; + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // If we have not found a collision yet + if (!isColliding) { + + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + + // Return if we have found a narrow-phase collision + if (isColliding) return true; + } + + // Go to the next proxy shape + body2ProxyShape = body2ProxyShape->getNext(); + } + + // Go to the next proxy shape + body1ProxyShape = body1ProxyShape->getNext(); + } + + // No overlap has been found + return false; +} + +// Report all the bodies that overlap with the body in parameter +void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, + unsigned short categoryMaskBits) { + + assert(overlapCallback != nullptr); + + Set reportedBodies(mMemoryManager.getPoolAllocator()); + + // For each proxy shape proxy shape of the body + ProxyShape* bodyProxyShape = body->getProxyShapesList(); + while (bodyProxyShape != nullptr) { + + if (bodyProxyShape->getBroadPhaseId() != -1) { + + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->getBroadPhaseId()); + + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + + const bodyindex bodyId = body->getId(); + + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { + + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + + // If the proxy shape is from a body that we have not already reported collision and the + // two proxy collision shapes are not from the same body + if (reportedBodies.find(proxyShape->getBody()->getId()) == reportedBodies.end() && + proxyShape->getBody()->getId() != bodyId) { + + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + + // Create a temporary overlapping pair + OverlappingPair pair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + + bool isColliding = false; + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + // If we have not found a collision yet + if (!isColliding) { + + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false, mMemoryManager.getPoolAllocator()); + } + } + + NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + currentNarrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory + mMemoryManager.release(MemoryManager::AllocationType::Pool, currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } + + // Return if we have found a narrow-phase collision + if (isColliding) { + + CollisionBody* overlapBody = proxyShape->getBody(); + + // Add the body into the set of reported bodies + reportedBodies.add(overlapBody->getId()); + + // Notify the overlap to the user + overlapCallback->notifyOverlap(overlapBody); + } + } + } + + // Go to the next overlapping proxy shape + element = element->next; + } + } + + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); + } +} + +// Test and report collisions between two bodies +void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* collisionCallback) { + + assert(collisionCallback != nullptr); + + List collidingNarrowPhaseInfos(mMemoryManager.getPoolAllocator()); + Set collidingPairs(mMemoryManager.getPoolAllocator()); + + // For each proxy shape proxy shape of the first body + ProxyShape* body1ProxyShape = body1->getProxyShapesList(); + while (body1ProxyShape != nullptr) { + + AABB aabb1 = body1ProxyShape->getWorldAABB(); + + // For each proxy shape of the second body + ProxyShape* body2ProxyShape = body2->getProxyShapesList(); + while (body2ProxyShape != nullptr) { + + AABB aabb2 = body2ProxyShape->getWorldAABB(); + + // Test if the AABBs of the two proxy shapes overlap + if (aabb1.testCollision(aabb2)) { + + // Create a temporary overlapping pair + OverlappingPair* pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(body1ProxyShape, body2ProxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* allNarrowPhaseInfos = computeMiddlePhaseForProxyShapes(pair); + + // For each narrow-phase info object + NarrowPhaseInfo* narrowPhaseInfo = allNarrowPhaseInfos; + while (narrowPhaseInfo != nullptr) { + + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator())) { + + collidingNarrowPhaseInfos.add(narrowPhaseInfo); + collidingPairs.add(pair); + } + } + + narrowPhaseInfo = narrowPhaseInfo->next; + } + } + + // Go to the next proxy shape + body2ProxyShape = body2ProxyShape->getNext(); + } + + // Go to the next proxy shape + body1ProxyShape = body1ProxyShape->getNext(); + } + + // Process the potential contacts + processAllPotentialContacts(collidingNarrowPhaseInfos, mMemoryManager.getPoolAllocator()); + + // For each colliding pair + for (auto it = collidingPairs.begin(); it != collidingPairs.end(); ++it) { + + OverlappingPair* pair = *it; + + assert(pair->hasContacts()); + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); + collisionCallback->notifyContact(collisionInfo); + + // Destroy the temporary overlapping pair + pair->~OverlappingPair(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); + } + + // Destroy the narrow phase infos + NarrowPhaseInfo* narrowPhaseInfo = mNarrowPhaseInfoList; + while (narrowPhaseInfo != nullptr) { + + NarrowPhaseInfo* narrowPhaseInfoToDelete = narrowPhaseInfo; + narrowPhaseInfo = narrowPhaseInfo->next; + + // Call the destructor + narrowPhaseInfoToDelete->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfoToDelete, sizeof(NarrowPhaseInfo)); + } +} + +// Test and report collisions between a body and all the others bodies of the world +void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits) { + + assert(callback != nullptr); + + List narrowPhaseInfos(mMemoryManager.getPoolAllocator()); + Set overlappingPairs(mMemoryManager.getPoolAllocator()); + + // For each proxy shape proxy shape of the body + ProxyShape* bodyProxyShape = body->getProxyShapesList(); + while (bodyProxyShape != nullptr) { + + if (bodyProxyShape->getBroadPhaseId() != -1) { + + // Get the AABB of the shape + const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->getBroadPhaseId()); + + // Ask the broad-phase to get all the overlapping shapes + LinkedList overlappingNodes(mMemoryManager.getPoolAllocator()); + mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes); + + const bodyindex bodyId = body->getId(); + + // For each overlaping proxy shape + LinkedList::ListElement* element = overlappingNodes.getListHead(); + while (element != nullptr) { + + // Get the overlapping proxy shape + int broadPhaseId = element->data; + ProxyShape* proxyShape = mBroadPhaseAlgorithm.getProxyShapeForBroadPhaseId(broadPhaseId); + + // If the two proxy collision shapes are not from the same body + if (proxyShape->getBody()->getId() != bodyId) { + + // Check if the collision filtering allows collision between the two shapes + if ((proxyShape->getCollisionCategoryBits() & categoryMaskBits) != 0) { + + // Create a temporary overlapping pair + OverlappingPair* pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(bodyProxyShape, proxyShape, mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + + overlappingPairs.add(pair); + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair); + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + narrowPhaseInfos.add(narrowPhaseInfo); + + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator()); + } + + narrowPhaseInfo = narrowPhaseInfo->next; + } + } + } + + // Go to the next overlapping proxy shape + element = element->next; + } + + // Go to the next proxy shape + bodyProxyShape = bodyProxyShape->getNext(); + } + } + + // Process the potential contacts + processAllPotentialContacts(narrowPhaseInfos, mMemoryManager.getPoolAllocator()); + + // For each overlapping pair + for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { + + OverlappingPair* pair = *it; + + if (pair->hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); + callback->notifyContact(collisionInfo); + } + + // Destroy the temporary overlapping pair + pair->~OverlappingPair(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); + } + + // Destroy the narrow phase infos + for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } +} + +// Test and report collisions between all shapes of the world +void CollisionDetection::testCollision(CollisionCallback* callback) { + + assert(callback != nullptr); + + // Compute the broad-phase collision detection + computeBroadPhase(); + + List narrowPhaseInfos(mMemoryManager.getPoolAllocator()); + Set overlappingPairs(mMemoryManager.getPoolAllocator()); + + // For each possible collision pair of bodies + Map, OverlappingPair*>::Iterator it; + for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) { + + OverlappingPair* originalPair = it->second; + + // Create a temporary overlapping pair + OverlappingPair* pair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair))) + OverlappingPair(originalPair->getShape1(), originalPair->getShape2(), mMemoryManager.getPoolAllocator(), + mMemoryManager.getPoolAllocator(), mWorld->mConfig); + overlappingPairs.add(pair); + + ProxyShape* shape1 = pair.getShape1(); + ProxyShape* shape2 = pair.getShape2(); + + // Check if the collision filtering allows collision between the two shapes and + // that the two shapes are still overlapping. + if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 && + (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0) && + mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { + + // Compute the middle-phase collision detection between the two shapes + NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair); + + // For each narrow-phase info object + while (narrowPhaseInfo != nullptr) { + + const CollisionShapeType shape1Type = narrowPhaseInfo->collisionShape1->getType(); + const CollisionShapeType shape2Type = narrowPhaseInfo->collisionShape2->getType(); + + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type); + + // If there is a collision algorithm for those two kinds of shapes + if (narrowPhaseAlgorithm != nullptr) { + + narrowPhaseInfos.add(narrowPhaseInfo); + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true, mMemoryManager.getPoolAllocator()); + } + + narrowPhaseInfo = narrowPhaseInfo->next; + } + } + } + + // Process the potential contacts + processAllPotentialContacts(narrowPhaseInfos, mMemoryManager.getPoolAllocator()); + + // For each overlapping pair + for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) { + + OverlappingPair* pair = *it; + + if (pair->hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(pair, mMemoryManager); + callback->notifyContact(collisionInfo); + } + + // Destroy the temporary overlapping pair + pair->~OverlappingPair(); + mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair)); + } + + // Destroy the narrow phase infos + for (auto it = narrowPhaseInfos.begin(); it != narrowPhaseInfos.end(); ++it) { + + NarrowPhaseInfo* narrowPhaseInfo = *it; + + // Call the destructor + narrowPhaseInfo->~NarrowPhaseInfo(); + + // Release the allocated memory for the narrow phase info + mMemoryManager.release(MemoryManager::AllocationType::Pool, narrowPhaseInfo, sizeof(NarrowPhaseInfo)); + } +} + +// Fill-in the collision detection matrix +void CollisionDetection::fillInCollisionMatrix() { + + // For each possible type of collision shape + for (int i=0; iselectAlgorithm(i, j); + } + } +} + +// Return the world event listener +EventListener* CollisionDetection::getWorldEventListener() { + return mWorld->mEventListener; +} + +// Return the world-space AABB of a given proxy shape +const AABB CollisionDetection::getWorldAABB(const ProxyShape* proxyShape) const { + assert(proxyShape->getBroadPhaseId() > -1); + return mBroadPhaseAlgorithm.getFatAABB(proxyShape->getBroadPhaseId()); +} diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index 8530697c..4e80a625 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -26,12 +26,11 @@ // Libraries #include "ContactManifold.h" #include "constraint/ContactPoint.h" -#include "collision/ContactManifoldInfo.h" using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, +ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings) : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), @@ -39,18 +38,6 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mWorldSettings(worldSettings) { - // For each contact point info in the manifold - const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo(); - while(pointInfo != nullptr) { - - // Add the new contact point - addContactPoint(pointInfo); - - pointInfo = pointInfo->next; - } - - assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD); - assert(mNbContactPoints > 0); } // Destructor @@ -121,20 +108,45 @@ decimal ContactManifold::getLargestContactDepth() const { // Add a contact point void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) { - assert(contactPointInfo != nullptr); + // For each contact point in the manifold + bool isSimilarPointFound = false; + ContactPoint* oldContactPoint = mContactPoints; + while (oldContactPoint != nullptr) { - // Create the new contact point - ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mWorldSettings); + assert(oldContactPoint != nullptr); - // Add the new contact point into the manifold - contactPoint->setNext(mContactPoints); - contactPoint->setPrevious(nullptr); - if (mContactPoints != nullptr) { - mContactPoints->setPrevious(contactPoint); - } - mContactPoints = contactPoint; + // If the new contact point is similar (very close) to the old contact point + if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) { - mNbContactPoints++; + // Replace (update) the old contact point with the new one + oldContactPoint->update(contactPointInfo); + isSimilarPointFound = true; + break; + } + + oldContactPoint = oldContactPoint->getNext(); + } + + // If we have not found a similar contact point + if (!isSimilarPointFound) { + + // Create the new contact point + ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo, mWorldSettings); + + // Add the new contact point into the manifold + contactPoint->setNext(mContactPoints); + contactPoint->setPrevious(nullptr); + if (mContactPoints != nullptr) { + mContactPoints->setPrevious(contactPoint); + } + + mContactPoints = contactPoint; + + mNbContactPoints++; + } + + // The old manifold is no longer obsolete + mIsObsolete = false; } // Set to true to make the manifold obsolete @@ -176,49 +188,202 @@ void ContactManifold::clearObsoleteContactPoints() { assert(mContactPoints != nullptr); } -// Make sure we do not have too much contact points by keeping only the best -// contact points of the manifold (with largest penetration depth) -void ContactManifold::reduce() { +// Reduce the number of contact points of the currently computed manifold +// This is based on the technique described by Dirk Gregorius in his +// "Contacts Creation" GDC presentation. This method will reduce the number of +// contact points to a maximum of 4 points (but it can be less). +void ContactManifold::reduce(const Transform& shape1ToWorldTransform) { assert(mContactPoints != nullptr); - // Remove contact points while there is too much contact points - while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { - removeNonOptimalContactPoint(); - } + // The following algorithm only works to reduce to a maximum of 4 contact points + assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); - assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0); - assert(mContactPoints != nullptr); -} + // If there are too many contact points in the manifold + if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { -// Remove a contact point that is not optimal (with a small penetration depth) -void ContactManifold::removeNonOptimalContactPoint() { + uint nbReducedPoints = 0; - assert(mContactPoints != nullptr); - assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD); - - // Get the contact point with the minimum penetration depth among all points - ContactPoint* contactPoint = mContactPoints; - ContactPoint* minContactPoint = nullptr; - decimal minPenetrationDepth = DECIMAL_LARGEST; - while (contactPoint != nullptr) { - - ContactPoint* nextContactPoint = contactPoint->getNext(); - - if (contactPoint->getPenetrationDepth() < minPenetrationDepth) { - - minContactPoint = contactPoint; - minPenetrationDepth = contactPoint->getPenetrationDepth(); + ContactPoint* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD]; + for (int i=0; igetNormal(); + + // Compute a search direction + const Vector3 searchDirection(1, 1, 1); + ContactPoint* element = mContactPoints; + pointsToKeep[0] = element; + decimal maxDotProduct = searchDirection.dot(element->getLocalPointOnShape1()); + element = element->getNext(); + nbReducedPoints = 1; + while(element != nullptr) { + + decimal dotProduct = searchDirection.dot(element->getLocalPointOnShape1()); + if (dotProduct > maxDotProduct) { + maxDotProduct = dotProduct; + pointsToKeep[0] = element; + } + element = element->getNext(); + } + assert(pointsToKeep[0] != nullptr); + assert(nbReducedPoints == 1); + + // Compute the second contact point we need to keep. + // The second point we keep is the one farthest away from the first point. + + decimal maxDistance = decimal(0.0); + element = mContactPoints; + while(element != nullptr) { + + if (element == pointsToKeep[0]) { + element = element->getNext(); + continue; + } + + decimal distance = (pointsToKeep[0]->getLocalPointOnShape1() - element->getLocalPointOnShape1()).lengthSquare(); + if (distance >= maxDistance) { + maxDistance = distance; + pointsToKeep[1] = element; + nbReducedPoints = 2; + } + element = element->getNext(); + } + assert(pointsToKeep[1] != nullptr); + assert(nbReducedPoints == 2); + + // Compute the third contact point we need to keep. + // The second point is the one producing the triangle with the larger area + // with first and second point. + + // We compute the most positive or most negative triangle area (depending on winding) + ContactPoint* thirdPointMaxArea = nullptr; + ContactPoint* thirdPointMinArea = nullptr; + decimal minArea = decimal(0.0); + decimal maxArea = decimal(0.0); + bool isPreviousAreaPositive = true; + element = mContactPoints; + while(element != nullptr) { + + if (element == pointsToKeep[0] || element == pointsToKeep[1]) { + element = element->getNext(); + continue; + } + + const Vector3 newToFirst = pointsToKeep[0]->getLocalPointOnShape1() - element->getLocalPointOnShape1(); + const Vector3 newToSecond = pointsToKeep[1]->getLocalPointOnShape1() - element->getLocalPointOnShape1(); + + // Compute the triangle area + decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); + + if (area >= maxArea) { + maxArea = area; + thirdPointMaxArea = element; + } + if (area <= minArea) { + minArea = area; + thirdPointMinArea = element; + } + element = element->getNext(); + } + assert(minArea <= decimal(0.0)); + assert(maxArea >= decimal(0.0)); + if (maxArea > (-minArea)) { + isPreviousAreaPositive = true; + pointsToKeep[2] = thirdPointMaxArea; + nbReducedPoints = 3; + } + else { + isPreviousAreaPositive = false; + pointsToKeep[2] = thirdPointMinArea; + nbReducedPoints = 3; + } + + // Compute the 4th point by choosing the triangle that add the most + // triangle area to the previous triangle and has opposite sign area (opposite winding) + + decimal largestArea = decimal(0.0); // Largest area (positive or negative) + element = mContactPoints; + + if (nbReducedPoints == 3) { + + // For each remaining point + while(element != nullptr) { + + if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) { + element = element->getNext(); + continue; + } + + // For each edge of the triangle made by the first three points + for (uint i=0; i<3; i++) { + + uint edgeVertex1Index = i; + uint edgeVertex2Index = i < 2 ? i + 1 : 0; + + const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->getLocalPointOnShape1() - element->getLocalPointOnShape1(); + const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->getLocalPointOnShape1() - element->getLocalPointOnShape1(); + + // Compute the triangle area + decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); + + // We are looking at the triangle with maximal area (positive or negative). + // If the previous area is positive, we are looking at negative area now. + // If the previous area is negative, we are looking at the positive area now. + if (isPreviousAreaPositive && area <= largestArea) { + largestArea = area; + pointsToKeep[3] = element; + nbReducedPoints = 4; + } + else if (!isPreviousAreaPositive && area >= largestArea) { + largestArea = area; + pointsToKeep[3] = element; + nbReducedPoints = 4; + } + } + + element = element->getNext(); + } + } + + // Delete the contact points we do not want to keep from the linked list + element = mContactPoints; + ContactPoint* previousElement = nullptr; + while(element != nullptr) { + + bool deletePoint = true; + + // Skip the points we want to keep + for (uint i=0; igetNext(); + deletePoint = false; + } + } + + if (deletePoint) { + + ContactPoint* contactPointToDelete = element; + element = element->getNext(); + + removeContactPoint(contactPointToDelete); + } + } + + assert(nbReducedPoints > 0 && nbReducedPoints <= 4); + mNbContactPoints = nbReducedPoints; } - - assert(minContactPoint != nullptr); - - // Remove the non optimal contact point - removeContactPoint(minContactPoint); - - assert(mNbContactPoints > 0); - assert(mContactPoints != nullptr); } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 3c2207e3..4b368660 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -93,6 +93,11 @@ class ContactManifold { private: + // -------------------- Constants -------------------- // + + /// Maximum number of contact points in a reduced contact manifold + const int MAX_CONTACT_POINTS_IN_MANIFOLD = 4; + // -------------------- Attributes -------------------- // /// Pointer to the first proxy shape of the contact @@ -181,11 +186,8 @@ class ContactManifold { /// Add a contact point void addContactPoint(const ContactPointInfo* contactPointInfo); - /// Make sure we do not have too much contact points by keeping only the best ones - void reduce(); - - /// Remove a contact point that is not optimal (with a small penetration depth) - void removeNonOptimalContactPoint(); + /// Reduce the number of contact points of the currently computed manifold + void reduce(const Transform& shape1ToWorldTransform); /// Remove a contact point void removeContactPoint(ContactPoint* contactPoint); @@ -219,8 +221,8 @@ class ContactManifold { // -------------------- Methods -------------------- // /// Constructor - ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings); + ContactManifold(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator, + const WorldSettings& worldSettings); /// Destructor ~ContactManifold(); diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp deleted file mode 100644 index 852af781..00000000 --- a/src/collision/ContactManifoldInfo.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2018 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "ContactManifoldInfo.h" -#include "collision/ContactPointInfo.h" - -using namespace reactphysics3d; - -// Constructor -ContactManifoldInfo::ContactManifoldInfo(MemoryAllocator& allocator) - : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) { - -} - -// Destructor -ContactManifoldInfo::~ContactManifoldInfo() { - - // Remove all the contact points - reset(); -} - -// Add a new contact point into the manifold -void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo) { - - assert(contactPointInfo->penetrationDepth > decimal(0.0)); - - // Add it into the linked list of contact points - contactPointInfo->next = mContactPointsList; - mContactPointsList = contactPointInfo; - - mNbContactPoints++; -} - -// Remove all the contact points -void ContactManifoldInfo::reset() { - - // Delete all the contact points in the linked list - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - ContactPointInfo* elementToDelete = element; - element = element->next; - - // Call the constructor - elementToDelete->~ContactPointInfo(); - - // Delete the current element - mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); - } - - mContactPointsList = nullptr; - mNbContactPoints = 0; -} - -// Return the largest penetration depth among its contact points -decimal ContactManifoldInfo::getLargestPenetrationDepth() const { - - ContactPointInfo* contactPoint = mContactPointsList; - assert(contactPoint != nullptr); - decimal maxDepth = decimal(0.0); - while (contactPoint != nullptr) { - - if (contactPoint->penetrationDepth > maxDepth) { - maxDepth = contactPoint->penetrationDepth; - } - - contactPoint = contactPoint->next; - } - - return maxDepth; -} - -// Reduce the number of contact points of the currently computed manifold -// This is based on the technique described by Dirk Gregorius in his -// "Contacts Creation" GDC presentation. This method will reduce the number of -// contact points to a maximum of 4 points (but it can be less). -void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) { - - assert(mContactPointsList != nullptr); - - // The following algorithm only works to reduce to a maximum of 4 contact points - assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4); - - // If there are too many contact points in the manifold - if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) { - - uint nbReducedPoints = 0; - - ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD]; - for (int i=0; inormal; - - // Compute a search direction - const Vector3 searchDirection(1, 1, 1); - ContactPointInfo* element = mContactPointsList; - pointsToKeep[0] = element; - decimal maxDotProduct = searchDirection.dot(element->localPoint1); - element = element->next; - nbReducedPoints = 1; - while(element != nullptr) { - - decimal dotProduct = searchDirection.dot(element->localPoint1); - if (dotProduct > maxDotProduct) { - maxDotProduct = dotProduct; - pointsToKeep[0] = element; - } - element = element->next; - } - assert(pointsToKeep[0] != nullptr); - assert(nbReducedPoints == 1); - - // Compute the second contact point we need to keep. - // The second point we keep is the one farthest away from the first point. - - decimal maxDistance = decimal(0.0); - element = mContactPointsList; - while(element != nullptr) { - - if (element == pointsToKeep[0]) { - element = element->next; - continue; - } - - decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare(); - if (distance >= maxDistance) { - maxDistance = distance; - pointsToKeep[1] = element; - nbReducedPoints = 2; - } - element = element->next; - } - assert(pointsToKeep[1] != nullptr); - assert(nbReducedPoints == 2); - - // Compute the third contact point we need to keep. - // The second point is the one producing the triangle with the larger area - // with first and second point. - - // We compute the most positive or most negative triangle area (depending on winding) - ContactPointInfo* thirdPointMaxArea = nullptr; - ContactPointInfo* thirdPointMinArea = nullptr; - decimal minArea = decimal(0.0); - decimal maxArea = decimal(0.0); - bool isPreviousAreaPositive = true; - element = mContactPointsList; - while(element != nullptr) { - - if (element == pointsToKeep[0] || element == pointsToKeep[1]) { - element = element->next; - continue; - } - - const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1; - const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1; - - // Compute the triangle area - decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); - - if (area >= maxArea) { - maxArea = area; - thirdPointMaxArea = element; - } - if (area <= minArea) { - minArea = area; - thirdPointMinArea = element; - } - element = element->next; - } - assert(minArea <= decimal(0.0)); - assert(maxArea >= decimal(0.0)); - if (maxArea > (-minArea)) { - isPreviousAreaPositive = true; - pointsToKeep[2] = thirdPointMaxArea; - nbReducedPoints = 3; - } - else { - isPreviousAreaPositive = false; - pointsToKeep[2] = thirdPointMinArea; - nbReducedPoints = 3; - } - - // Compute the 4th point by choosing the triangle that add the most - // triangle area to the previous triangle and has opposite sign area (opposite winding) - - decimal largestArea = decimal(0.0); // Largest area (positive or negative) - element = mContactPointsList; - - if (nbReducedPoints == 3) { - - // For each remaining point - while(element != nullptr) { - - if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) { - element = element->next; - continue; - } - - // For each edge of the triangle made by the first three points - for (uint i=0; i<3; i++) { - - uint edgeVertex1Index = i; - uint edgeVertex2Index = i < 2 ? i + 1 : 0; - - const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1; - const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1; - - // Compute the triangle area - decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space); - - // We are looking at the triangle with maximal area (positive or negative). - // If the previous area is positive, we are looking at negative area now. - // If the previous area is negative, we are looking at the positive area now. - if (isPreviousAreaPositive && area <= largestArea) { - largestArea = area; - pointsToKeep[3] = element; - nbReducedPoints = 4; - } - else if (!isPreviousAreaPositive && area >= largestArea) { - largestArea = area; - pointsToKeep[3] = element; - nbReducedPoints = 4; - } - } - - element = element->next; - } - } - - // Delete the contact points we do not want to keep from the linked list - element = mContactPointsList; - ContactPointInfo* previousElement = nullptr; - while(element != nullptr) { - - bool deletePoint = true; - - // Skip the points we want to keep - for (uint i=0; inext; - deletePoint = false; - } - } - - if (deletePoint) { - - ContactPointInfo* elementToDelete = element; - if (previousElement != nullptr) { - previousElement->next = elementToDelete->next; - } - else { - mContactPointsList = elementToDelete->next; - } - element = element->next; - - // Call the destructor - elementToDelete->~ContactPointInfo(); - - // Delete the current element - mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); - } - } - - assert(nbReducedPoints > 0 && nbReducedPoints <= 4); - mNbContactPoints = nbReducedPoints; - } -} - - diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h deleted file mode 100644 index a6907f93..00000000 --- a/src/collision/ContactManifoldInfo.h +++ /dev/null @@ -1,117 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2018 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H -#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H - -// Libraries -#include "configuration.h" - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Declarations -class MemoryAllocator; -struct ContactPointInfo; -class Transform; - -// Constants -const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold - -// Class ContactManifoldInfo -/** - * This class is used to collect the list of ContactPointInfo that come - * from a collision test between two shapes. - */ -class ContactManifoldInfo { - - private: - - // -------------------- Attributes -------------------- // - - /// Linked list with all the contact points - ContactPointInfo* mContactPointsList; - - /// Number of contact points in the manifold - uint mNbContactPoints; - - /// Next element in the linked-list of contact manifold info - ContactManifoldInfo* mNext; - - /// Reference the the memory allocator where the contact point infos have been allocated - MemoryAllocator& mAllocator; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - ContactManifoldInfo(MemoryAllocator& allocator); - - /// Destructor - ~ContactManifoldInfo(); - - /// Deleted Copy-constructor - ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete; - - /// Deleted assignment operator - ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete; - - /// Add a new contact point into the manifold - void addContactPoint(ContactPointInfo* contactPointInfo); - - /// Remove all the contact points - void reset(); - - /// Get the first contact point info of the linked list of contact points - ContactPointInfo* getFirstContactPointInfo() const; - - /// Return the largest penetration depth among its contact points - decimal getLargestPenetrationDepth() const; - - /// Return the pointer to the next manifold info in the linked-list - ContactManifoldInfo* getNext(); - - /// Reduce the number of contact points of the currently computed manifold - void reduce(const Transform& shape1ToWorldTransform); - - // Friendship - friend class OverlappingPair; - friend class CollisionDetection; -}; - -// Get the first contact point info of the linked list of contact points -inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const { - return mContactPointsList; -} - -// Return the pointer to the next manifold info in the linked-list -inline ContactManifoldInfo* ContactManifoldInfo::getNext() { - return mNext; -} - -} -#endif - diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 8712bf08..2cbfc409 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -25,8 +25,8 @@ // Libraries #include "ContactManifoldSet.h" +#include "NarrowPhaseInfo.h" #include "constraint/ContactPoint.h" -#include "collision/ContactManifoldInfo.h" #include "ProxyShape.h" #include "collision/ContactManifold.h" @@ -49,24 +49,56 @@ ContactManifoldSet::~ContactManifoldSet() { clear(); } -void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { +void ContactManifoldSet::addContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { - assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr); + assert(narrowPhaseInfo->contactPoints != nullptr); - // Try to find an existing contact manifold with similar contact normal - ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo); + // For each potential contact point to add + ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; + while (contactPoint != nullptr) { - // If a similar manifold has been found - if (similarManifold != nullptr) { + // Look if the contact point correspond to an existing potential manifold + // (if the contact point normal is similar to the normal of an existing manifold) + ContactManifold* manifold = mManifolds; + bool similarManifoldFound = false; + while(manifold != nullptr) { - // Update the old manifold with the new one - updateManifoldWithNewOne(similarManifold, contactManifoldInfo); + // Get the first contact point + const ContactPoint* point = manifold->getContactPoints(); + assert(point != nullptr); + + // If we have found a corresponding manifold for the new contact point + // (a manifold with a similar contact normal direction) + if (point->getNormal().dot(contactPoint->normal) >= mWorldSettings.cosAngleSimilarContactManifold) { + + // Add the contact point to the manifold + manifold->addContactPoint(contactPoint); + + similarManifoldFound = true; + + break; + } + + manifold = manifold->getNext(); + } + + // If we have not found an existing manifold with a similar contact normal + if (!similarManifoldFound) { + + // Create a new contact manifold + ContactManifold* manifold = createManifold(); + + // Add the contact point to the manifold + manifold->addContactPoint(contactPoint); + } + + contactPoint = contactPoint->next; } - else { - // Create a new contact manifold - createManifold(contactManifoldInfo); - } + // All the contact point info of the narrow-phase info have been moved + // into the potential contacts of the overlapping pair. We can now + // remove the contacts points from the narrow phase info object. + narrowPhaseInfo->resetContactPoints(); } // Return the total number of contact points in the set of manifolds @@ -96,51 +128,6 @@ int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape } } - -// Update a previous similar manifold with a new one -void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) { - - assert(oldManifold != nullptr); - assert(newManifold != nullptr); - - // For each contact point of the new manifold - ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo(); - assert(contactPointInfo != nullptr); - while (contactPointInfo != nullptr) { - - // For each contact point in the old manifold - bool isSimilarPointFound = false; - ContactPoint* oldContactPoint = oldManifold->getContactPoints(); - while (oldContactPoint != nullptr) { - - assert(oldContactPoint != nullptr); - - // If the new contact point is similar (very close) to the old contact point - if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) { - - // Replace (update) the old contact point with the new one - oldContactPoint->update(contactPointInfo); - isSimilarPointFound = true; - break; - } - - oldContactPoint = oldContactPoint->getNext(); - } - - // If we have not found a similar contact point - if (!isSimilarPointFound) { - - // Add the contact point to the manifold - oldManifold->addContactPoint(contactPointInfo); - } - - contactPointInfo = contactPointInfo->next; - } - - // The old manifold is no longer obsolete - oldManifold->setIsObsolete(false, false); -} - // Remove a contact manifold that is the least optimal (smaller penetration depth) void ContactManifoldSet::removeNonOptimalManifold() { @@ -171,13 +158,26 @@ void ContactManifoldSet::removeNonOptimalManifold() { removeManifold(minDepthManifold); } +// Create a new contact manifold and add it to the set +ContactManifold* ContactManifoldSet::createManifold() { + + ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) + ContactManifold(mShape1, mShape2, mMemoryAllocator, mWorldSettings); + manifold->setPrevious(nullptr); + manifold->setNext(mManifolds); + if (mManifolds != nullptr) { + mManifolds->setPrevious(manifold); + } + mManifolds = manifold; + + mNbManifolds++; + + return manifold; +} + // Return the contact manifold with a similar contact normal. // If no manifold has close enough contact normal, it returns nullptr -ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const { - - // Get the contact normal of the first point of the manifold - const ContactPointInfo* contactPoint = contactManifold->getFirstContactPointInfo(); - assert(contactPoint != nullptr); +ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const Vector3& contactNormal) const { ContactManifold* manifold = mManifolds; @@ -189,7 +189,7 @@ ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const Conta assert(point != nullptr); // If the contact normal of the two manifolds are close enough - if (contactPoint->normal.dot(point->getNormal()) >= mWorldSettings.cosAngleSimilarContactManifold) { + if (contactNormal.dot(point->getNormal()) >= mWorldSettings.cosAngleSimilarContactManifold) { return manifold; } @@ -216,24 +216,11 @@ void ContactManifoldSet::clear() { mNbManifolds--; } + mManifolds = nullptr; + assert(mNbManifolds == 0); } -// Create a new contact manifold and add it to the set -void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) { - - ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, mWorldSettings); - manifold->setPrevious(nullptr); - manifold->setNext(mManifolds); - if (mManifolds != nullptr) { - mManifolds->setPrevious(manifold); - } - mManifolds = manifold; - - mNbManifolds++; -} - // Remove a contact manifold from the set void ContactManifoldSet::removeManifold(ContactManifold* manifold) { @@ -309,7 +296,7 @@ void ContactManifoldSet::reduce() { // Reduce all the contact manifolds in case they have too many contact points ContactManifold* manifold = mManifolds; while (manifold != nullptr) { - manifold->reduce(); + manifold->reduce(mShape1->getLocalToWorldTransform()); manifold = manifold->getNext(); } } diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 6e12b42e..ac22715c 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -34,7 +34,11 @@ class ContactManifoldInfo; class ProxyShape; class MemoryAllocator; struct WorldSettings; +struct NarrowPhaseInfo; +struct Vector3; class CollisionShape; +class Transform; + // Constants const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set @@ -77,17 +81,14 @@ class ContactManifoldSet { // -------------------- Methods -------------------- // /// Create a new contact manifold and add it to the set - void createManifold(const ContactManifoldInfo* manifoldInfo); + ContactManifold* createManifold(); // Return the contact manifold with a similar contact normal. - ContactManifold* selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const; + ContactManifold* selectManifoldWithSimilarNormal(const Vector3& contactNormal) const; /// Remove a contact manifold that is the least optimal (smaller penetration depth) void removeNonOptimalManifold(); - /// Update a previous similar manifold with a new one - void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold); - /// Return the maximum number of contact manifolds allowed between to collision shapes int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2); @@ -108,8 +109,8 @@ class ContactManifoldSet { /// Destructor ~ContactManifoldSet(); - /// Add a contact manifold in the set - void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); + /// Add the contact points from the narrow phase + void addContactPoints(NarrowPhaseInfo* narrowPhaseInfo); /// Return the first proxy shape ProxyShape* getShape1() const; diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index 10a29332..be4c6ab4 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -78,12 +78,6 @@ void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penD contactPoints = contactPointInfo; } -/// Take all the generated contact points and create a new potential -/// contact manifold into the overlapping pair -void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() { - overlappingPair->addPotentialContactPoints(this); -} - // Reset the remaining contact points void NarrowPhaseInfo::resetContactPoints() { diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index c95d7ccb..99f9ffc7 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -83,9 +83,6 @@ struct NarrowPhaseInfo { void addContactPoint(const Vector3& contactNormal, decimal penDepth, const Vector3& localPt1, const Vector3& localPt2); - /// Create a new potential contact manifold into the overlapping pair using current contact points - void addContactPointsAsPotentialContactManifold(); - /// Reset the remaining contact points void resetContactPoints(); diff --git a/src/containers/Set.h b/src/containers/Set.h index dee9e9bd..fce0a051 100755 --- a/src/containers/Set.h +++ b/src/containers/Set.h @@ -42,7 +42,7 @@ namespace reactphysics3d { * This class represents a simple generic set. This set is implemented * with a hash table. */ -template +template, class KeyEqual = std::equal_to> class Set { private: @@ -94,6 +94,12 @@ class Set { // -------------------- Attributes -------------------- // + /// Object with hash operator + const Hash mHash; + + /// Object with equality operator + const KeyEqual mEqual; + /// Current number of used entries in the set int mNbUsedEntries; @@ -206,11 +212,11 @@ class Set { if (mCapacity > 0) { - size_t hashCode = std::hash()(value); + size_t hashCode = mHash(value); int bucket = hashCode % mCapacity; for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { - if (mEntries[i].hashCode == hashCode && (*mEntries[i].value) == value) { + if (mEntries[i].hashCode == hashCode && mEqual(*mEntries[i].value, value)) { return i; } } @@ -373,8 +379,9 @@ class Set { // -------------------- Methods -------------------- // /// Constructor - Set(MemoryAllocator& allocator, size_t capacity = 0) - : mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), + Set(MemoryAllocator& allocator, size_t capacity = 0, const Hash& hash = Hash(), + const KeyEqual& equal = KeyEqual()) + : mHash(hash), mEqual(equal), mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr), mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) { // If the largest prime has not been computed yet @@ -391,8 +398,8 @@ class Set { } /// Copy constructor - Set(const Set& set) - :mNbUsedEntries(set.mNbUsedEntries), mNbFreeEntries(set.mNbFreeEntries), mCapacity(set.mCapacity), + Set(const Set& set) + :mHash(set.mHash), mEqual(set.mEqual), mNbUsedEntries(set.mNbUsedEntries), mNbFreeEntries(set.mNbFreeEntries), mCapacity(set.mCapacity), mBuckets(nullptr), mEntries(nullptr), mAllocator(set.mAllocator), mFreeIndex(set.mFreeIndex) { if (mCapacity > 0) { @@ -445,15 +452,16 @@ class Set { return findEntry(value) != -1; } - /// Add a value into the set - void add(const V& value) { + /// Add a value into the set. + /// Returns true if the item has been inserted and false otherwise. + bool add(const V& value) { if (mCapacity == 0) { initialize(0); } // Compute the hash code of the value - size_t hashCode = std::hash()(value); + size_t hashCode = mHash(value); // Compute the corresponding bucket index int bucket = hashCode % mCapacity; @@ -462,9 +470,9 @@ class Set { for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { // If there is already an item with the same value in the set - if (mEntries[i].hashCode == hashCode && (*mEntries[i].value) == value) { + if (mEntries[i].hashCode == hashCode && mEqual(*mEntries[i].value, value)) { - return; + return false; } } @@ -500,6 +508,8 @@ class Set { assert(mEntries[entryIndex].value != nullptr); new (mEntries[entryIndex].value) V(value); mBuckets[bucket] = entryIndex; + + return true; } /// Remove the element pointed by some iterator @@ -517,12 +527,12 @@ class Set { if (mCapacity > 0) { - size_t hashcode = std::hash()(value); + size_t hashcode = mHash(value); int bucket = hashcode % mCapacity; int last = -1; for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) { - if (mEntries[i].hashCode == hashcode && (*mEntries[i].value) == value) { + if (mEntries[i].hashCode == hashcode && mEqual(*mEntries[i].value, value)) { if (last < 0 ) { mBuckets[bucket] = mEntries[i].next; @@ -604,11 +614,11 @@ class Set { if (mCapacity > 0) { - size_t hashCode = std::hash()(value); + size_t hashCode = mHash(value); bucket = hashCode % mCapacity; for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) { - if (mEntries[i].hashCode == hashCode && *(mEntries[i].value) == value) { + if (mEntries[i].hashCode == hashCode && mEqual(*(mEntries[i].value), value)) { entry = i; break; } @@ -715,15 +725,15 @@ class Set { } }; -template -const int Set::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, +template +const int Set::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919, 1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591, 17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437, 187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263, 1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559}; -template -int Set::LARGEST_PRIME = -1; +template +int Set::LARGEST_PRIME = -1; } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 91112d74..2e9af682 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -894,8 +894,7 @@ List DynamicsWorld::getContactsList() { List contactManifolds(mMemoryManager.getPoolAllocator()); // For each currently overlapping pair of bodies - Map, OverlappingPair*>::Iterator it; - for (it = mCollisionDetection.mOverlappingPairs.begin(); + for (auto it = mCollisionDetection.mOverlappingPairs.begin(); it != mCollisionDetection.mOverlappingPairs.end(); ++it) { OverlappingPair* pair = it->second; diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index c0ae6957..863bd1a8 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -26,7 +26,6 @@ // Libraries #include #include "OverlappingPair.h" -#include "collision/ContactManifoldInfo.h" #include "collision/NarrowPhaseInfo.h" #include "containers/containers_common.h" #include "collision/ContactPointInfo.h" @@ -37,7 +36,7 @@ using namespace reactphysics3d; OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, const WorldSettings& worldSettings) - : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator, worldSettings), mPotentialContactManifolds(nullptr), + : mPairID(computeID(shape1, shape2)), mContactManifoldSet(shape1, shape2, persistentMemoryAllocator, worldSettings), mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator), mLastFrameCollisionInfos(mPersistentAllocator), mWorldSettings(worldSettings) { @@ -45,7 +44,6 @@ OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, // Destructor OverlappingPair::~OverlappingPair() { - assert(mPotentialContactManifolds == nullptr); // Remove all the remaining last frame collision info for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { @@ -58,98 +56,6 @@ OverlappingPair::~OverlappingPair() { } } -// Create a new potential contact manifold using contact-points from narrow-phase -void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { - - assert(narrowPhaseInfo->contactPoints != nullptr); - - // For each potential contact point to add - ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints; - while (contactPoint != nullptr) { - - ContactPointInfo* nextContactPoint = contactPoint->next; - - // Look if the contact point correspond to an existing potential manifold - // (if the contact point normal is similar to the normal of an existing manifold) - ContactManifoldInfo* manifold = mPotentialContactManifolds; - bool similarManifoldFound = false; - while(manifold != nullptr) { - - // Get the first contact point - const ContactPointInfo* point = manifold->getFirstContactPointInfo(); - assert(point != nullptr); - - // If we have found a corresponding manifold for the new contact point - // (a manifold with a similar contact normal direction) - if (point->normal.dot(contactPoint->normal) >= mWorldSettings.cosAngleSimilarContactManifold) { - - // Add the contact point to the manifold - manifold->addContactPoint(contactPoint); - - similarManifoldFound = true; - - break; - } - - manifold = manifold->getNext(); - } - - // If we have not found an existing manifold with a similar contact normal - if (!similarManifoldFound) { - - // Create a new potential contact manifold - ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo))) - ContactManifoldInfo(mTempMemoryAllocator); - - // Add the manifold into the linked-list of potential contact manifolds - manifoldInfo->mNext = mPotentialContactManifolds; - mPotentialContactManifolds = manifoldInfo; - - // Add the contact point to the manifold - manifoldInfo->addContactPoint(contactPoint); - } - - contactPoint = nextContactPoint; - } - - // All the contact point info of the narrow-phase info have been moved - // into the potential contacts of the overlapping pair - narrowPhaseInfo->contactPoints = nullptr; -} - -// Clear all the potential contact manifolds -void OverlappingPair::clearPotentialContactManifolds() { - - ContactManifoldInfo* element = mPotentialContactManifolds; - while(element != nullptr) { - - // Remove the proxy collision shape - ContactManifoldInfo* elementToRemove = element; - element = element->getNext(); - - // Delete the element - elementToRemove->~ContactManifoldInfo(); - mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo)); - } - - mPotentialContactManifolds = nullptr; -} - -// Reduce the number of contact points of all the potential contact manifolds -void OverlappingPair::reducePotentialContactManifolds() { - - // For each potential contact manifold - ContactManifoldInfo* manifold = mPotentialContactManifolds; - while (manifold != nullptr) { - - // Reduce the number of contact points of the manifold - manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform()); - - manifold = manifold->getNext(); - } -} - - // Add a new last frame collision info if it does not exist for the given shapes already void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index debec3bc..30d1ab94 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -32,6 +32,7 @@ #include "containers/Map.h" #include "containers/Pair.h" #include "containers/containers_common.h" +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -106,12 +107,12 @@ class OverlappingPair { // -------------------- Attributes -------------------- // + /// Pair ID + OverlappingPairId mPairID; + /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Linked-list of potential contact manifold - ContactManifoldInfo* mPotentialContactManifolds; - /// Persistent memory allocator MemoryAllocator& mPersistentAllocator; @@ -144,7 +145,7 @@ class OverlappingPair { /// Deleted assignment operator OverlappingPair& operator=(const OverlappingPair& pair) = delete; - + /// Return the pointer to first proxy collision shape ProxyShape* getShape1() const; @@ -157,15 +158,9 @@ class OverlappingPair { /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); - /// Clear all the potential contact manifolds - void clearPotentialContactManifolds(); - /// Add potential contact-points from narrow-phase into potential contact manifolds void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo); - /// Add a contact to the contact manifold - void addContactManifold(const ContactManifoldInfo* contactManifoldInfo); - /// Return a reference to the temporary memory allocator MemoryAllocator& getTemporaryAllocator(); @@ -175,12 +170,6 @@ class OverlappingPair { /// Return true if the overlapping pair has contact manifolds with contacts bool hasContacts() const; - /// Return a pointer to the first potential contact manifold in the linked-list - ContactManifoldInfo* getPotentialContactManifolds(); - - /// Reduce the number of contact points of all the potential contact manifolds - void reducePotentialContactManifolds(); - /// Make the contact manifolds and contact points obsolete void makeContactsObsolete(); @@ -223,11 +212,6 @@ inline ProxyShape* OverlappingPair::getShape2() const { return mContactManifoldSet.getShape2(); } -// Add a contact to the contact manifold -inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) { - mContactManifoldSet.addContactManifold(contactManifoldInfo); -} - // Return the last frame collision info for a given shape id or nullptr if none is found inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeIdPair& shapeIds) { Map::Iterator it = mLastFrameCollisionInfos.find(shapeIds); @@ -289,11 +273,6 @@ inline bool OverlappingPair::hasContacts() const { return mContactManifoldSet.getContactManifolds() != nullptr; } -// Return a pointer to the first potential contact manifold in the linked-list -inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { - return mPotentialContactManifolds; -} - // Clear the obsolete contact manifold and contact points inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() { mContactManifoldSet.clearObsoleteManifoldsAndContactPoints(); @@ -309,6 +288,11 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint s return mLastFrameCollisionInfos[ShapeIdPair(shapeId1, shapeId2)]; } +// Create a new potential contact manifold using contact-points from narrow-phase +inline void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) { + mContactManifoldSet.addContactPoints(narrowPhaseInfo); +} + } #endif diff --git a/test/tests/containers/TestSet.h b/test/tests/containers/TestSet.h index ddf2c245..23f7663b 100644 --- a/test/tests/containers/TestSet.h +++ b/test/tests/containers/TestSet.h @@ -149,14 +149,22 @@ class TestSet : public Test { // ----- Test add() ----- // Set set1(mAllocator); - set1.add(10); - set1.add(80); - set1.add(130); + bool add1 = set1.add(10); + bool add2 = set1.add(80); + bool add3 = set1.add(130); + test(add1); + test(add2); + test(add3); test(set1.contains(10)); test(set1.contains(80)); test(set1.contains(130)); test(set1.size() == 3); + bool add4 = set1.add(80); + test(!add4); + test(set1.contains(80)); + test(set1.size() == 3); + Set set2(mAllocator, 15); for (int i = 0; i < 1000000; i++) { set2.add(i); @@ -168,7 +176,8 @@ class TestSet : public Test { test(isValid); set1.remove(10); - set1.add(10); + bool add = set1.add(10); + test(add); test(set1.size() == 3); test(set1.contains(10));