From 7dcc86d746270d0cc7f432ec73726301eabc1165 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 18 Nov 2019 07:29:04 +0100 Subject: [PATCH] Working on middle-phase collision detection --- src/components/ProxyShapeComponents.h | 1 + src/engine/OverlappingPairs.cpp | 20 +++++++++-- src/engine/OverlappingPairs.h | 11 +++++- src/systems/CollisionDetectionSystem.cpp | 44 +++++++++++++----------- 4 files changed, 51 insertions(+), 25 deletions(-) diff --git a/src/components/ProxyShapeComponents.h b/src/components/ProxyShapeComponents.h index 1dc72a5a..18ab9053 100644 --- a/src/components/ProxyShapeComponents.h +++ b/src/components/ProxyShapeComponents.h @@ -194,6 +194,7 @@ class ProxyShapeComponents : public Components { // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; + friend class CollisionDetectionSystem; }; // Return the body entity of a given proxy-shape diff --git a/src/engine/OverlappingPairs.cpp b/src/engine/OverlappingPairs.cpp index 7e311df1..a1912e28 100644 --- a/src/engine/OverlappingPairs.cpp +++ b/src/engine/OverlappingPairs.cpp @@ -28,20 +28,22 @@ #include "OverlappingPairs.h" #include "containers/containers_common.h" #include "collision/ContactPointInfo.h" +#include "collision/narrowphase/NarrowPhaseAlgorithm.h" +#include "collision/narrowphase/CollisionDispatch.h" using namespace reactphysics3d; // Constructor OverlappingPairs::OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ProxyShapeComponents& proxyShapeComponents, - CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, Set &noCollisionPairs) + CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, Set &noCollisionPairs, CollisionDispatch &collisionDispatch) : mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator), mNbPairs(0), mConcavePairsStartIndex(0), mPairDataSize(sizeof(uint64) + sizeof(int32) + sizeof(int32) + sizeof(Entity) + sizeof(Entity) + sizeof(Map) + - sizeof(bool) + sizeof(bool)), + sizeof(bool) + sizeof(bool) + sizeof(NarrowPhaseAlgorithmType)), mNbAllocatedPairs(0), mBuffer(nullptr), mMapPairIdToPairIndex(persistentMemoryAllocator), mProxyShapeComponents(proxyShapeComponents), mCollisionBodyComponents(collisionBodyComponents), - mRigidBodyComponents(rigidBodyComponents), mNoCollisionPairs(noCollisionPairs) { + mRigidBodyComponents(rigidBodyComponents), mNoCollisionPairs(noCollisionPairs), mCollisionDispatch(collisionDispatch) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_PAIRS); @@ -206,6 +208,7 @@ void OverlappingPairs::allocate(uint64 nbPairsToAllocate) { Map* newLastFrameCollisionInfos = reinterpret_cast*>(newProxyShapes2 + nbPairsToAllocate); bool* newNeedToTestOverlap = reinterpret_cast(newLastFrameCollisionInfos + nbPairsToAllocate); bool* newIsActive = reinterpret_cast(newNeedToTestOverlap + nbPairsToAllocate); + NarrowPhaseAlgorithmType* newNarrowPhaseAlgorithmType = reinterpret_cast(newIsActive + nbPairsToAllocate); // If there was already pairs before if (mNbPairs > 0) { @@ -219,6 +222,7 @@ void OverlappingPairs::allocate(uint64 nbPairsToAllocate) { memcpy(newLastFrameCollisionInfos, mLastFrameCollisionInfos, mNbPairs * sizeof(Map)); memcpy(newNeedToTestOverlap, mNeedToTestOverlap, mNbPairs * sizeof(bool)); memcpy(newIsActive, mIsActive, mNbPairs * sizeof(bool)); + memcpy(newNarrowPhaseAlgorithmType, mNarrowPhaseAlgorithmType, mNbPairs * sizeof(NarrowPhaseAlgorithmType)); // Deallocate previous memory mPersistentAllocator.release(mBuffer, mNbAllocatedPairs * mPairDataSize); @@ -233,6 +237,7 @@ void OverlappingPairs::allocate(uint64 nbPairsToAllocate) { mLastFrameCollisionInfos = newLastFrameCollisionInfos; mNeedToTestOverlap = newNeedToTestOverlap; mIsActive = newIsActive; + mNarrowPhaseAlgorithmType = newNarrowPhaseAlgorithmType; mNbAllocatedPairs = nbPairsToAllocate; } @@ -256,6 +261,9 @@ uint64 OverlappingPairs::addPair(ProxyShape* shape1, ProxyShape* shape2, bool is assert(!mMapPairIdToPairIndex.containsKey(pairId)); + // Select the narrow phase algorithm to use according to the two collision shapes + NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), + collisionShape2->getType()); // Insert the new component data new (mPairIds + index) uint64(pairId); new (mPairBroadPhaseId1 + index) int32(shape1->getBroadPhaseId()); @@ -265,6 +273,8 @@ uint64 OverlappingPairs::addPair(ProxyShape* shape1, ProxyShape* shape2, bool is new (mLastFrameCollisionInfos + index) Map(mPersistentAllocator); new (mNeedToTestOverlap + index) bool(false); new (mIsActive + index) bool(isActive); + new (mNarrowPhaseAlgorithmType + index) NarrowPhaseAlgorithmType(algorithmType); + // Map the entity with the new component lookup index mMapPairIdToPairIndex.add(Pair(pairId, index)); @@ -298,6 +308,7 @@ void OverlappingPairs::movePairToIndex(uint64 srcIndex, uint64 destIndex) { new (mLastFrameCollisionInfos + destIndex) Map(mLastFrameCollisionInfos[srcIndex]); mNeedToTestOverlap[destIndex] = mNeedToTestOverlap[srcIndex]; mIsActive[destIndex] = mIsActive[srcIndex]; + new (mNarrowPhaseAlgorithmType + destIndex) NarrowPhaseAlgorithmType(mNarrowPhaseAlgorithmType[srcIndex]); // Destroy the source pair destroyPair(srcIndex); @@ -322,6 +333,7 @@ void OverlappingPairs::swapPairs(uint64 index1, uint64 index2) { Map lastFrameCollisionInfo(mLastFrameCollisionInfos[index1]); bool needTestOverlap = mNeedToTestOverlap[index1]; bool isActive = mIsActive[index1]; + NarrowPhaseAlgorithmType narrowPhaseAlgorithmType = mNarrowPhaseAlgorithmType[index1]; // Destroy pair 1 destroyPair(index1); @@ -337,6 +349,7 @@ void OverlappingPairs::swapPairs(uint64 index1, uint64 index2) { new (mLastFrameCollisionInfos + index2) Map(lastFrameCollisionInfo); mNeedToTestOverlap[index2] = needTestOverlap; mIsActive[index2] = isActive; + new (mNarrowPhaseAlgorithmType + index2) NarrowPhaseAlgorithmType(narrowPhaseAlgorithmType); // Update the pairID to pair index mapping mMapPairIdToPairIndex.add(Pair(pairId, index2)); @@ -358,6 +371,7 @@ void OverlappingPairs::destroyPair(uint64 index) { mProxyShapes1[index].~Entity(); mProxyShapes2[index].~Entity(); mLastFrameCollisionInfos[index].~Map(); + mNarrowPhaseAlgorithmType[index].~NarrowPhaseAlgorithmType(); } // Add a new last frame collision info if it does not exist for the given shapes already diff --git a/src/engine/OverlappingPairs.h b/src/engine/OverlappingPairs.h index 6dec1fd6..34fe478f 100644 --- a/src/engine/OverlappingPairs.h +++ b/src/engine/OverlappingPairs.h @@ -43,7 +43,9 @@ namespace reactphysics3d { // Declarations struct NarrowPhaseInfoBatch; +enum class NarrowPhaseAlgorithmType; class CollisionShape; +class CollisionDispatch; // Structure LastFrameCollisionInfo /** @@ -171,6 +173,9 @@ class OverlappingPairs { /// True if the overlapping pair is active (at least one body of the pair is active and not static) bool* mIsActive; + /// Array with the pointer to the narrow-phase algorithm for each overlapping pair + NarrowPhaseAlgorithmType* mNarrowPhaseAlgorithmType; + /// Reference to the proxy-shapes components ProxyShapeComponents& mProxyShapeComponents; @@ -183,6 +188,9 @@ class OverlappingPairs { /// Reference to the set of bodies that cannot collide with each others Set& mNoCollisionPairs; + /// Reference to the collision dispatch + CollisionDispatch& mCollisionDispatch; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -214,7 +222,8 @@ class OverlappingPairs { /// Constructor OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ProxyShapeComponents& proxyShapeComponents, CollisionBodyComponents& collisionBodyComponents, - RigidBodyComponents& rigidBodyComponents, Set& noCollisionPairs); + RigidBodyComponents& rigidBodyComponents, Set& noCollisionPairs, + CollisionDispatch& collisionDispatch); /// Destructor ~OverlappingPairs(); diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index 7d5ee9a3..2894dceb 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -57,7 +57,7 @@ CollisionDetectionSystem::CollisionDetectionSystem(CollisionWorld* world, ProxyS mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mProxyShapesComponents, - collisionBodyComponents, rigidBodyComponents, mNoCollisionPairs), + collisionBodyComponents, rigidBodyComponents, mNoCollisionPairs, mCollisionDispatch), mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, rigidBodyComponents), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()), mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), @@ -220,12 +220,15 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP const Entity proxyShape1Entity = overlappingPairs.mProxyShapes1[i]; const Entity proxyShape2Entity = overlappingPairs.mProxyShapes2[i]; - assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); - assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); - assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity)); + const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1Entity); + const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2Entity); - const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity); - const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity); + assert(mProxyShapesComponents.mBroadPhaseId[proxyShape1Index] != -1); + assert(mProxyShapesComponents.mBroadPhaseId[proxyShape2Index] != -1); + assert(mProxyShapesComponents.mBroadPhaseId[proxyShape1Index] != mProxyShapesComponents.mBroadPhaseId[proxyShape2Index]); + + const Entity body1Entity = mProxyShapesComponents.mBodiesEntities[proxyShape1Index]; + const Entity body2Entity = mProxyShapesComponents.mBodiesEntities[proxyShape2Index]; const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) && mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC; @@ -243,18 +246,16 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; - CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity); - CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity); + CollisionShape* collisionShape1 = mProxyShapesComponents.mCollisionShapes[proxyShape1Index]; + CollisionShape* collisionShape2 = mProxyShapesComponents.mCollisionShapes[proxyShape2Index]; - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), - collisionShape2->getType()); + NarrowPhaseAlgorithmType algorithmType = overlappingPairs.mNarrowPhaseAlgorithmType[i]; // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection narrowPhaseInput.addNarrowPhaseTest(overlappingPairs.mPairIds[i], proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2, - mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), - mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), + mProxyShapesComponents.mLocalToWorldTransforms[proxyShape1Index], + mProxyShapesComponents.mLocalToWorldTransforms[proxyShape2Index], algorithmType, mMemoryManager.getSingleFrameAllocator()); } @@ -266,12 +267,15 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP const Entity proxyShape1Entity = overlappingPairs.mProxyShapes1[i]; const Entity proxyShape2Entity = overlappingPairs.mProxyShapes2[i]; - assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); - assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); - assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity)); + const uint proxyShape1Index = mProxyShapesComponents.getEntityIndex(proxyShape1Entity); + const uint proxyShape2Index = mProxyShapesComponents.getEntityIndex(proxyShape2Entity); - const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity); - const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity); + assert(mProxyShapesComponents.mBroadPhaseId[proxyShape1Index] != -1); + assert(mProxyShapesComponents.mBroadPhaseId[proxyShape2Index] != -1); + assert(mProxyShapesComponents.mBroadPhaseId[proxyShape1Index] != mProxyShapesComponents.mBroadPhaseId[proxyShape2Index]); + + const Entity body1Entity = mProxyShapesComponents.mBodiesEntities[proxyShape1Index]; + const Entity body2Entity = mProxyShapesComponents.mBodiesEntities[proxyShape2Index]; const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) && mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC; @@ -342,9 +346,7 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity); CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity); - // Select the narrow phase algorithm to use according to the two collision shapes - NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(), - collisionShape2->getType()); + NarrowPhaseAlgorithmType algorithmType = mOverlappingPairs.mNarrowPhaseAlgorithmType[pairIndex]; // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection