diff --git a/src/components/Components.cpp b/src/components/Components.cpp index ea3e3521..3749b0f4 100644 --- a/src/components/Components.cpp +++ b/src/components/Components.cpp @@ -72,8 +72,6 @@ uint32 Components::prepareAddComponent(bool isSleeping) { // Add the component at the end of the array index = mNbComponents; - - mDisabledStartIndex = index; } // If the component to add is not part of a disabled entity else { diff --git a/src/components/Components.h b/src/components/Components.h index af721f5c..3a950af7 100644 --- a/src/components/Components.h +++ b/src/components/Components.h @@ -52,9 +52,6 @@ class Components { /// Number of components to allocated at the beginning const uint32 INIT_NB_ALLOCATED_COMPONENTS = 10; - /// Number of valid entities to hit before stopping garbage collection - const uint32 GARBAGE_COLLECTION_MAX_VALID_ENTITIES = 5; - // -------------------- Attributes -------------------- // /// Memory allocator diff --git a/src/components/ProxyShapeComponents.cpp b/src/components/ProxyShapeComponents.cpp index ab98115b..18089f1d 100644 --- a/src/components/ProxyShapeComponents.cpp +++ b/src/components/ProxyShapeComponents.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; ProxyShapeComponents::ProxyShapeComponents(MemoryAllocator& allocator) :Components(allocator, sizeof(Entity) + sizeof(Entity) + sizeof(ProxyShape*) + sizeof(int) + sizeof(Transform) + sizeof(CollisionShape*) + sizeof(decimal) + sizeof(unsigned short) + - sizeof(unsigned short) + sizeof(Transform)) { + sizeof(unsigned short) + sizeof(Transform) + sizeof(List)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -66,6 +66,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) { unsigned short* newCollisionCategoryBits = reinterpret_cast(newMasses + nbComponentsToAllocate); unsigned short* newCollideWithMaskBits = reinterpret_cast(newCollisionCategoryBits + nbComponentsToAllocate); Transform* newLocalToWorldTransforms = reinterpret_cast(newCollideWithMaskBits + nbComponentsToAllocate); + List* newOverlappingPairs = reinterpret_cast*>(newLocalToWorldTransforms + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -81,6 +82,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newCollisionCategoryBits, mCollisionCategoryBits, mNbComponents * sizeof(unsigned short)); memcpy(newCollideWithMaskBits, mCollideWithMaskBits, mNbComponents * sizeof(unsigned short)); memcpy(newLocalToWorldTransforms, mLocalToWorldTransforms, mNbComponents * sizeof(Transform)); + memcpy(newOverlappingPairs, mOverlappingPairs, mNbComponents * sizeof(List)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -98,6 +100,7 @@ void ProxyShapeComponents::allocate(uint32 nbComponentsToAllocate) { mCollisionCategoryBits = newCollisionCategoryBits; mCollideWithMaskBits = newCollideWithMaskBits; mLocalToWorldTransforms = newLocalToWorldTransforms; + mOverlappingPairs = newOverlappingPairs; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -119,6 +122,7 @@ void ProxyShapeComponents::addComponent(Entity proxyShapeEntity, bool isSleeping new (mCollisionCategoryBits + index) unsigned short(component.collisionCategoryBits); new (mCollideWithMaskBits + index) unsigned short(component.collideWithMaskBits); new (mLocalToWorldTransforms + index) Transform(component.localToWorldTransform); + new (mOverlappingPairs + index) List(mMemoryAllocator); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(proxyShapeEntity, index)); @@ -145,6 +149,7 @@ void ProxyShapeComponents::moveComponentToIndex(uint32 srcIndex, uint32 destInde new (mCollisionCategoryBits + destIndex) unsigned short(mCollisionCategoryBits[srcIndex]); new (mCollideWithMaskBits + destIndex) unsigned short(mCollideWithMaskBits[srcIndex]); new (mLocalToWorldTransforms + destIndex) Transform(mLocalToWorldTransforms[srcIndex]); + new (mOverlappingPairs + destIndex) List(mOverlappingPairs[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -171,6 +176,7 @@ void ProxyShapeComponents::swapComponents(uint32 index1, uint32 index2) { unsigned short collisionCategoryBits1 = mCollisionCategoryBits[index1]; unsigned short collideWithMaskBits1 = mCollideWithMaskBits[index1]; Transform localToWorldTransform1 = mLocalToWorldTransforms[index1]; + List overlappingPairs = mOverlappingPairs[index1]; // Destroy component 1 destroyComponent(index1); @@ -188,6 +194,7 @@ void ProxyShapeComponents::swapComponents(uint32 index1, uint32 index2) { new (mCollisionCategoryBits + index2) unsigned short(collisionCategoryBits1); new (mCollideWithMaskBits + index2) unsigned short(collideWithMaskBits1); new (mLocalToWorldTransforms + index2) Transform(localToWorldTransform1); + new (mOverlappingPairs + index2) List(overlappingPairs); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(proxyShapeEntity1, index2)); @@ -212,4 +219,5 @@ void ProxyShapeComponents::destroyComponent(uint32 index) { mLocalToBodyTransforms[index].~Transform(); mCollisionShapes[index] = nullptr; mLocalToWorldTransforms[index].~Transform(); + mOverlappingPairs[index].~List(); } diff --git a/src/components/ProxyShapeComponents.h b/src/components/ProxyShapeComponents.h index 9c7e5650..1dc72a5a 100644 --- a/src/components/ProxyShapeComponents.h +++ b/src/components/ProxyShapeComponents.h @@ -92,6 +92,9 @@ class ProxyShapeComponents : public Components { /// Array with the local-to-world transforms of the proxy-shapes Transform* mLocalToWorldTransforms; + /// Array with the list of involved overlapping pairs for each proxy-shape + List* mOverlappingPairs; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -185,6 +188,9 @@ class ProxyShapeComponents : public Components { /// Set the local-to-world transform of a proxy-shape void setLocalToWorldTransform(Entity proxyShapeEntity, const Transform& transform); + /// Return a reference to the list of overlapping pairs for a given proxy-shape + List& getOverlappingPairs(Entity proxyShapeEntity); + // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; @@ -302,6 +308,14 @@ inline void ProxyShapeComponents::setLocalToWorldTransform(Entity proxyShapeEnti mLocalToWorldTransforms[mMapEntityToComponentIndex[proxyShapeEntity]] = transform; } +// Return a reference to the list of overlapping pairs for a given proxy-shape +inline List& ProxyShapeComponents::getOverlappingPairs(Entity proxyShapeEntity) { + + assert(mMapEntityToComponentIndex.containsKey(proxyShapeEntity)); + + return mOverlappingPairs[mMapEntityToComponentIndex[proxyShapeEntity]]; +} + } #endif diff --git a/src/containers/Map.h b/src/containers/Map.h index c07fbb95..a6457b82 100755 --- a/src/containers/Map.h +++ b/src/containers/Map.h @@ -123,6 +123,7 @@ class Map { // Compute the next larger prime size mCapacity = getPrimeSize(capacity); + assert(mCapacity >= 0); // Allocate memory for the buckets mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); @@ -142,6 +143,8 @@ class Map { mNbUsedEntries = 0; mNbFreeEntries = 0; mFreeIndex = -1; + + assert(size() >= 0); } /// Expand the capacity of the map @@ -200,6 +203,8 @@ class Map { mCapacity = newCapacity; mBuckets = newBuckets; mEntries = newEntries; + + assert(mCapacity >= 0); } /// Return the index of the entry with a given key or -1 if there is no entry with this key @@ -371,6 +376,8 @@ class Map { :mNbUsedEntries(map.mNbUsedEntries), mNbFreeEntries(map.mNbFreeEntries), mCapacity(map.mCapacity), mBuckets(nullptr), mEntries(nullptr), mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) { + assert(capacity() >= 0); + if (mCapacity > 0) { // Allocate memory for the buckets @@ -380,7 +387,7 @@ class Map { mEntries = static_cast(mAllocator.allocate(mCapacity * sizeof(Entry))); // Copy the buckets - std::uninitialized_copy(map.mBuckets, map.mBuckets + mCapacity, mBuckets); + std::uninitialized_copy(map.mBuckets, map.mBuckets + mCapacity, mBuckets); // Copy the entries for (int i=0; i < mCapacity; i++) { @@ -392,7 +399,11 @@ class Map { new (mEntries[i].keyValue) Pair(*(map.mEntries[i].keyValue)); } } + } + + assert(size() >= 0); + assert((*this) == map); } /// Destructor @@ -483,6 +494,7 @@ class Map { mNbUsedEntries++; } + assert(size() >= 0); assert(mEntries[entryIndex].keyValue == nullptr); mEntries[entryIndex].hashCode = hashCode; mEntries[entryIndex].next = mBuckets[bucket]; @@ -551,6 +563,8 @@ class Map { } } + assert(size() >= 0); + // Return the end iterator return end(); } @@ -575,6 +589,8 @@ class Map { mFreeIndex = -1; mNbUsedEntries = 0; mNbFreeEntries = 0; + + assert(size() >= 0); } // If elements have been allocated @@ -599,6 +615,7 @@ class Map { /// Return the number of elements in the map int size() const { + assert(mNbUsedEntries - mNbFreeEntries >= 0); return mNbUsedEntries - mNbFreeEntries; } @@ -649,6 +666,7 @@ class Map { } if (entry == -1) { + assert(false); throw std::runtime_error("No item with given key has been found in the map"); } @@ -716,6 +734,7 @@ class Map { // Compute the next larger prime size mCapacity = getPrimeSize(map.mCapacity); + assert(mCapacity >= 0); // Allocate memory for the buckets mBuckets = static_cast(mAllocator.allocate(mCapacity * sizeof(int))); @@ -743,6 +762,8 @@ class Map { } } + assert(size() >= 0); + return *this; } diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 902e8fa9..79f96318 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -43,7 +43,7 @@ CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logge mJointsComponents(mMemoryManager.getBaseAllocator()), mBallAndSocketJointsComponents(mMemoryManager.getBaseAllocator()), mFixedJointsComponents(mMemoryManager.getBaseAllocator()), mHingeJointsComponents(mMemoryManager.getBaseAllocator()), mSliderJointsComponents(mMemoryManager.getBaseAllocator()), - mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mRigidBodyComponents, mMemoryManager), + mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mCollisionBodyComponents, mRigidBodyComponents, mMemoryManager), mBodies(mMemoryManager.getPoolAllocator()), mEventListener(nullptr), mName(worldSettings.worldName), mIsProfilerCreatedByUser(profiler != nullptr), mIsLoggerCreatedByUser(logger != nullptr) { diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index e6acd13c..34248f97 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -273,7 +273,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { rigidBody->removeAllCollisionShapes(); // Destroy all the joints in which the rigid body to be destroyed is involved - JointListElement* element; const List& joints = mRigidBodyComponents.getJoints(rigidBody->getEntity()); for (uint32 i=0; i < joints.size(); i++) { destroyJoint(mJointsComponents.getJoint(joints[i])); diff --git a/src/engine/OverlappingPairs.cpp b/src/engine/OverlappingPairs.cpp index 007af276..7e311df1 100644 --- a/src/engine/OverlappingPairs.cpp +++ b/src/engine/OverlappingPairs.cpp @@ -32,124 +32,107 @@ using namespace reactphysics3d; // Constructor -OverlappingPairs::OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ProxyShapeComponents& proxyShapeComponents) +OverlappingPairs::OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ProxyShapeComponents& proxyShapeComponents, + CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, Set &noCollisionPairs) : 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)), + mNbAllocatedPairs(0), mBuffer(nullptr), mMapPairIdToPairIndex(persistentMemoryAllocator), - mConvexPairIds(mPersistentAllocator), mConvexProxyShapes1(mPersistentAllocator), mConvexProxyShapes2(mPersistentAllocator), - mConcavePairIds(mPersistentAllocator), mConcaveProxyShapes1(mPersistentAllocator), mConcaveProxyShapes2(mPersistentAllocator), - mConvexLastFrameCollisionInfos(mPersistentAllocator), mConcaveLastFrameCollisionInfos(mPersistentAllocator), - mConvexNeedToTestOverlap(mPersistentAllocator), mConcaveNeedToTestOverlap(mPersistentAllocator), - mProxyShapeComponents(proxyShapeComponents) { + mProxyShapeComponents(proxyShapeComponents), mCollisionBodyComponents(collisionBodyComponents), + mRigidBodyComponents(rigidBodyComponents), mNoCollisionPairs(noCollisionPairs) { -} + // Allocate memory for the components data + allocate(INIT_NB_ALLOCATED_PAIRS); +} // Destructor OverlappingPairs::~OverlappingPairs() { + // If there are allocated pairs + if (mNbAllocatedPairs > 0) { + + // Destroy all the remaining pairs + for (uint32 i = 0; i < mNbPairs; i++) { + + // Remove all the remaining last frame collision info + for (auto it = mLastFrameCollisionInfos[i].begin(); it != mLastFrameCollisionInfos[i].end(); ++it) { + + // Call the constructor + it->second->~LastFrameCollisionInfo(); + + // Release memory + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + } + + // Remove the involved overlapping pair to the two proxy-shapes + assert(mProxyShapeComponents.getOverlappingPairs(mProxyShapes1[i]).find(mPairIds[i]) != mProxyShapeComponents.getOverlappingPairs(mProxyShapes1[i]).end()); + assert(mProxyShapeComponents.getOverlappingPairs(mProxyShapes2[i]).find(mPairIds[i]) != mProxyShapeComponents.getOverlappingPairs(mProxyShapes2[i]).end()); + mProxyShapeComponents.getOverlappingPairs(mProxyShapes1[i]).remove(mPairIds[i]); + mProxyShapeComponents.getOverlappingPairs(mProxyShapes2[i]).remove(mPairIds[i]); + + destroyPair(i); + } + + // Size for the data of a single pair (in bytes) + const size_t totalSizeBytes = mNbAllocatedPairs * mPairDataSize; + + // Release the allocated memory + mPersistentAllocator.release(mBuffer, totalSizeBytes); + } } -/// Add an overlapping pair -uint64 OverlappingPairs::addPair(ProxyShape* shape1, ProxyShape* shape2) { +// Compute the index where we need to insert the new pair +uint64 OverlappingPairs::prepareAddPair(bool isConvexVsConvex) { - // TODO : Maybe use entities in parameters - - const CollisionShape* collisionShape1 = mProxyShapeComponents.getCollisionShape(shape1->getEntity()); - const CollisionShape* collisionShape2 = mProxyShapeComponents.getCollisionShape(shape2->getEntity()); - - const uint32 shape1Id = static_cast(shape1->getBroadPhaseId()); - const uint32 shape2Id = static_cast(shape2->getBroadPhaseId()); - - // Compute a unique id for the overlapping pair - const uint64 pairId = pairNumbers(std::max(shape1Id, shape2Id), std::min(shape1Id, shape2Id)); - - // If both shapes are convex - if (collisionShape1->isConvex() && collisionShape2->isConvex()) { - - const uint nbConvexPairs = static_cast(mConvexPairIds.size()); - - mConvexPairIds.add(pairId); - mConvexProxyShapes1.add(shape1->getEntity()); - mConvexProxyShapes2.add(shape2->getEntity()); - mConvexNeedToTestOverlap.add(false); - - // TODO: Make sure we use the correct allocator here - mConvexLastFrameCollisionInfos.add(Map(mPersistentAllocator)); - - // Add a mapping to the index in the internal arrays - mMapPairIdToPairIndex.add(Pair(pairId, PairLocation(true, nbConvexPairs))); + // If we need to allocate more components + if (mNbPairs == mNbAllocatedPairs) { + allocate(mNbAllocatedPairs * 2); } + + uint64 index; + + // If the pair to add is not convex vs convex or there are no concave pairs yet + if (!isConvexVsConvex) { + + // Add the component at the end of the array + index = mNbPairs; + } + // If the pair to add is convex vs convex else { - const uint nbConcavePairs = static_cast(mConcavePairIds.size()); + // If there already are convex vs concave pairs + if (mConcavePairsStartIndex != mNbPairs) { - mConcavePairIds.add(pairId); - mConcaveProxyShapes1.add(shape1->getEntity()); - mConcaveProxyShapes2.add(shape2->getEntity()); - mConcaveNeedToTestOverlap.add(true); + // Move the first convex vs concave pair to the end of the array + movePairToIndex(mConcavePairsStartIndex, mNbPairs); + } - // TODO: Make sure we use the correct allocator here - mConcaveLastFrameCollisionInfos.add(Map(mPersistentAllocator)); + index = mConcavePairsStartIndex; - // Add a mapping to the index in the internal arrays - mMapPairIdToPairIndex.add(Pair(pairId, PairLocation(false, nbConcavePairs))); + mConcavePairsStartIndex++; } - return pairId; + return index; } -// Remove an overlapping pair -uint64 OverlappingPairs::removePair(uint64 pairId) { +// Remove a component at a given index +void OverlappingPairs::removePair(uint64 pairId) { RP3D_PROFILE("OverlappingPairs::removePair()", mProfiler); assert(mMapPairIdToPairIndex.containsKey(pairId)); - const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; + uint64 index = mMapPairIdToPairIndex[pairId]; + assert(index < mNbPairs); - if (pairLocation.isConvexVsConvex) { - - assert(pairLocation.pairIndex < mConvexPairIds.size()); - - const uint64 lastPairId = mConvexPairIds[mConvexPairIds.size() - 1]; - const PairLocation lastPairLocation = mMapPairIdToPairIndex[lastPairId]; - - // Remap the last pair location - if (pairLocation.pairIndex < mConvexPairIds.size() - 1) { - mMapPairIdToPairIndex[lastPairId] = PairLocation(lastPairLocation.isConvexVsConvex, pairLocation.pairIndex); - } - - // Remove the pair (the pair is replaced by the last one of the lists) - mConvexPairIds.removeAtAndReplaceWithLast(pairLocation.pairIndex); - mConvexProxyShapes1.removeAtAndReplaceWithLast(pairLocation.pairIndex); - mConvexProxyShapes2.removeAtAndReplaceWithLast(pairLocation.pairIndex); - mConvexNeedToTestOverlap.removeAtAndReplaceWithLast(pairLocation.pairIndex); - } - else { - - assert(pairLocation.pairIndex < mConcavePairIds.size()); - - const uint64 lastPairId = mConcavePairIds[mConcavePairIds.size() - 1]; - const PairLocation lastPairLocation = mMapPairIdToPairIndex[lastPairId]; - - // Remap the last pair location - if (pairLocation.pairIndex < mConcavePairIds.size() - 1) { - mMapPairIdToPairIndex[lastPairId] = PairLocation(lastPairLocation.isConvexVsConvex, pairLocation.pairIndex); - } - - // Remove the pair (the pair is replaced by the last one of the lists) - mConcavePairIds.removeAtAndReplaceWithLast(pairLocation.pairIndex); - mConcaveProxyShapes1.removeAtAndReplaceWithLast(pairLocation.pairIndex); - mConcaveProxyShapes2.removeAtAndReplaceWithLast(pairLocation.pairIndex); - mConcaveNeedToTestOverlap.removeAtAndReplaceWithLast(pairLocation.pairIndex); - } - - mMapPairIdToPairIndex.remove(pairId); - - List>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ? - mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos; + // We want to keep the arrays tightly packed. Therefore, when a pair is removed, + // we replace it with the last element of the array. But we need to make sure that convex + // and concave pairs stay grouped together. // Remove all the remaining last frame collision info - for (auto it = lastFrameCollisionInfos[pairLocation.pairIndex].begin(); it != lastFrameCollisionInfos[pairLocation.pairIndex].end(); ++it) { + for (auto it = mLastFrameCollisionInfos[index].begin(); it != mLastFrameCollisionInfos[index].end(); ++it) { // Call the constructor it->second->~LastFrameCollisionInfo(); @@ -158,19 +141,223 @@ uint64 OverlappingPairs::removePair(uint64 pairId) { mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); } - lastFrameCollisionInfos.removeAtAndReplaceWithLast(pairLocation.pairIndex); -} + // Remove the involved overlapping pair to the two proxy-shapes + assert(mProxyShapeComponents.getOverlappingPairs(mProxyShapes1[index]).find(pairId) != mProxyShapeComponents.getOverlappingPairs(mProxyShapes1[index]).end()); + assert(mProxyShapeComponents.getOverlappingPairs(mProxyShapes2[index]).find(pairId) != mProxyShapeComponents.getOverlappingPairs(mProxyShapes2[index]).end()); + mProxyShapeComponents.getOverlappingPairs(mProxyShapes1[index]).remove(pairId); + mProxyShapeComponents.getOverlappingPairs(mProxyShapes2[index]).remove(pairId); -// Try to find a pair with a given id, return true if the pair is found and the corresponding PairLocation -bool OverlappingPairs::findPair(uint64 pairId, PairLocation& pairLocation) { + // Destroy the pair + destroyPair(index); - auto it = mMapPairIdToPairIndex.find(pairId); - if (it != mMapPairIdToPairIndex.end()) { - pairLocation = it->second; - return true; + // If the pair to remove is convex vs concave + if (index >= mConcavePairsStartIndex) { + + // If the pair is not the last one + if (index != mNbPairs - 1) { + + // We replace it by the last convex vs concave pair + movePairToIndex(mNbPairs - 1, index); + } + } + else { // If the pair to remove is convex vs convex + + // If it not the last convex vs convex pair + if (index != mConcavePairsStartIndex - 1) { + + // We replace it by the last convex vs convex pair + movePairToIndex(mConcavePairsStartIndex - 1, index); + } + + // If there are convex vs concave pairs at the end + if (mConcavePairsStartIndex != mNbPairs) { + + // We replace the last convex vs convex pair by the last convex vs concave pair + movePairToIndex(mNbPairs - 1, mConcavePairsStartIndex - 1); + } + + mConcavePairsStartIndex--; } - return false; + mNbPairs--; + + assert(mConcavePairsStartIndex <= mNbPairs); + assert(mNbPairs == static_cast(mMapPairIdToPairIndex.size())); +} + +// Allocate memory for a given number of pairs +void OverlappingPairs::allocate(uint64 nbPairsToAllocate) { + + assert(nbPairsToAllocate > mNbAllocatedPairs); + + // Size for the data of a single component (in bytes) + const size_t totalSizeBytes = nbPairsToAllocate * mPairDataSize; + + // Allocate memory + void* newBuffer = mPersistentAllocator.allocate(totalSizeBytes); + assert(newBuffer != nullptr); + + // New pointers to components data + uint64* newPairIds = static_cast(newBuffer); + int32* newPairBroadPhaseId1 = reinterpret_cast(newPairIds + nbPairsToAllocate); + int32* newPairBroadPhaseId2 = reinterpret_cast(newPairBroadPhaseId1 + nbPairsToAllocate); + Entity* newProxyShapes1 = reinterpret_cast(newPairBroadPhaseId2 + nbPairsToAllocate); + Entity* newProxyShapes2 = reinterpret_cast(newProxyShapes1 + nbPairsToAllocate); + Map* newLastFrameCollisionInfos = reinterpret_cast*>(newProxyShapes2 + nbPairsToAllocate); + bool* newNeedToTestOverlap = reinterpret_cast(newLastFrameCollisionInfos + nbPairsToAllocate); + bool* newIsActive = reinterpret_cast(newNeedToTestOverlap + nbPairsToAllocate); + + // If there was already pairs before + if (mNbPairs > 0) { + + // Copy component data from the previous buffer to the new one + memcpy(newPairIds, mPairIds, mNbPairs * sizeof(uint64)); + memcpy(newPairBroadPhaseId1, mPairBroadPhaseId1, mNbPairs * sizeof(int32)); + memcpy(newPairBroadPhaseId2, mPairBroadPhaseId2, mNbPairs * sizeof(int32)); + memcpy(newProxyShapes1, mProxyShapes1, mNbPairs * sizeof(Entity)); + memcpy(newProxyShapes2, mProxyShapes2, mNbPairs * sizeof(Entity)); + memcpy(newLastFrameCollisionInfos, mLastFrameCollisionInfos, mNbPairs * sizeof(Map)); + memcpy(newNeedToTestOverlap, mNeedToTestOverlap, mNbPairs * sizeof(bool)); + memcpy(newIsActive, mIsActive, mNbPairs * sizeof(bool)); + + // Deallocate previous memory + mPersistentAllocator.release(mBuffer, mNbAllocatedPairs * mPairDataSize); + } + + mBuffer = newBuffer; + mPairIds = newPairIds; + mPairBroadPhaseId1 = newPairBroadPhaseId1; + mPairBroadPhaseId2 = newPairBroadPhaseId2; + mProxyShapes1 = newProxyShapes1; + mProxyShapes2 = newProxyShapes2; + mLastFrameCollisionInfos = newLastFrameCollisionInfos; + mNeedToTestOverlap = newNeedToTestOverlap; + mIsActive = newIsActive; + + mNbAllocatedPairs = nbPairsToAllocate; +} + +// Add an overlapping pair +uint64 OverlappingPairs::addPair(ProxyShape* shape1, ProxyShape* shape2, bool isActive) { + + RP3D_PROFILE("OverlappingPairs::addPair()", mProfiler); + + const CollisionShape* collisionShape1 = mProxyShapeComponents.getCollisionShape(shape1->getEntity()); + const CollisionShape* collisionShape2 = mProxyShapeComponents.getCollisionShape(shape2->getEntity()); + + // Prepare to add new pair (allocate memory if necessary and compute insertion index) + uint64 index = prepareAddPair(collisionShape1->isConvex() && collisionShape2->isConvex()); + + const uint32 shape1Id = static_cast(shape1->getBroadPhaseId()); + const uint32 shape2Id = static_cast(shape2->getBroadPhaseId()); + + // Compute a unique id for the overlapping pair + const uint64 pairId = pairNumbers(std::max(shape1Id, shape2Id), std::min(shape1Id, shape2Id)); + + assert(!mMapPairIdToPairIndex.containsKey(pairId)); + + // Insert the new component data + new (mPairIds + index) uint64(pairId); + new (mPairBroadPhaseId1 + index) int32(shape1->getBroadPhaseId()); + new (mPairBroadPhaseId2 + index) int32(shape2->getBroadPhaseId()); + new (mProxyShapes1 + index) Entity(shape1->getEntity()); + new (mProxyShapes2 + index) Entity(shape2->getEntity()); + new (mLastFrameCollisionInfos + index) Map(mPersistentAllocator); + new (mNeedToTestOverlap + index) bool(false); + new (mIsActive + index) bool(isActive); + + // Map the entity with the new component lookup index + mMapPairIdToPairIndex.add(Pair(pairId, index)); + + // Add the involved overlapping pair to the two proxy-shapes + assert(mProxyShapeComponents.getOverlappingPairs(shape1->getEntity()).find(pairId) == mProxyShapeComponents.getOverlappingPairs(shape1->getEntity()).end()); + assert(mProxyShapeComponents.getOverlappingPairs(shape2->getEntity()).find(pairId) == mProxyShapeComponents.getOverlappingPairs(shape2->getEntity()).end()); + mProxyShapeComponents.getOverlappingPairs(shape1->getEntity()).add(pairId); + mProxyShapeComponents.getOverlappingPairs(shape2->getEntity()).add(pairId); + + mNbPairs++; + + assert(mConcavePairsStartIndex <= mNbPairs); + assert(mNbPairs == static_cast(mMapPairIdToPairIndex.size())); + + return pairId; +} + +// Move a pair from a source to a destination index in the pairs array +// The destination location must contain a constructed object +void OverlappingPairs::movePairToIndex(uint64 srcIndex, uint64 destIndex) { + + const uint64 pairId = mPairIds[srcIndex]; + + // Copy the data of the source pair to the destination location + mPairIds[destIndex] = mPairIds[srcIndex]; + mPairBroadPhaseId1[destIndex] = mPairBroadPhaseId1[srcIndex]; + mPairBroadPhaseId2[destIndex] = mPairBroadPhaseId2[srcIndex]; + new (mProxyShapes1 + destIndex) Entity(mProxyShapes1[srcIndex]); + new (mProxyShapes2 + destIndex) Entity(mProxyShapes2[srcIndex]); + new (mLastFrameCollisionInfos + destIndex) Map(mLastFrameCollisionInfos[srcIndex]); + mNeedToTestOverlap[destIndex] = mNeedToTestOverlap[srcIndex]; + mIsActive[destIndex] = mIsActive[srcIndex]; + + // Destroy the source pair + destroyPair(srcIndex); + + assert(!mMapPairIdToPairIndex.containsKey(pairId)); + + // Update the pairId to pair index mapping + mMapPairIdToPairIndex.add(Pair(pairId, destIndex)); + + assert(mMapPairIdToPairIndex[mPairIds[destIndex]] == destIndex); +} + +// Swap two pairs in the array +void OverlappingPairs::swapPairs(uint64 index1, uint64 index2) { + + // Copy pair 1 data + uint64 pairId = mPairIds[index1]; + int32 pairBroadPhaseId1 = mPairBroadPhaseId1[index1]; + int32 pairBroadPhaseId2 = mPairBroadPhaseId2[index1]; + Entity proxyShape1 = mProxyShapes1[index1]; + Entity proxyShape2 = mProxyShapes2[index1]; + Map lastFrameCollisionInfo(mLastFrameCollisionInfos[index1]); + bool needTestOverlap = mNeedToTestOverlap[index1]; + bool isActive = mIsActive[index1]; + + // Destroy pair 1 + destroyPair(index1); + + movePairToIndex(index2, index1); + + // Reconstruct pair 1 at pair 2 location + mPairIds[index2] = pairId; + mPairBroadPhaseId1[index2] = pairBroadPhaseId1; + mPairBroadPhaseId2[index2] = pairBroadPhaseId2; + new (mProxyShapes1 + index2) Entity(proxyShape1); + new (mProxyShapes2 + index2) Entity(proxyShape2); + new (mLastFrameCollisionInfos + index2) Map(lastFrameCollisionInfo); + mNeedToTestOverlap[index2] = needTestOverlap; + mIsActive[index2] = isActive; + + // Update the pairID to pair index mapping + mMapPairIdToPairIndex.add(Pair(pairId, index2)); + + assert(mMapPairIdToPairIndex[mPairIds[index1]] == index1); + assert(mMapPairIdToPairIndex[mPairIds[index2]] == index2); + assert(mNbPairs == static_cast(mMapPairIdToPairIndex.size())); +} + +// Destroy a pair at a given index +void OverlappingPairs::destroyPair(uint64 index) { + + assert(index < mNbPairs); + + assert(mMapPairIdToPairIndex[mPairIds[index]] == index); + + mMapPairIdToPairIndex.remove(mPairIds[index]); + + mProxyShapes1[index].~Entity(); + mProxyShapes2[index].~Entity(); + mLastFrameCollisionInfos[index].~Map(); } // Add a new last frame collision info if it does not exist for the given shapes already @@ -180,24 +367,22 @@ LastFrameCollisionInfo* OverlappingPairs::addLastFrameInfoIfNecessary(uint64 pai assert(mMapPairIdToPairIndex.containsKey(pairId)); - PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; - List>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ? - mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos; + const uint64 index = mMapPairIdToPairIndex[pairId]; + assert(index < mNbPairs); // Try to get the corresponding last frame collision info const ShapeIdPair shapeIdPair(shapeId1, shapeId2); - // TODO : Remove test - auto it = lastFrameCollisionInfos[pairLocation.pairIndex].find(shapeIdPair); // If there is no collision info for those two shapes already - if (it == lastFrameCollisionInfos[pairLocation.pairIndex].end()) { + auto it = mLastFrameCollisionInfos[index].find(shapeIdPair); + if (it == mLastFrameCollisionInfos[index].end()) { // Create a new collision info LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) LastFrameCollisionInfo(); // Add it into the map of collision infos - lastFrameCollisionInfos[pairLocation.pairIndex].add(Pair(shapeIdPair, collisionInfo)); + mLastFrameCollisionInfos[index].add(Pair(shapeIdPair, collisionInfo)); return collisionInfo; } @@ -216,10 +401,10 @@ void OverlappingPairs::clearObsoleteLastFrameCollisionInfos() { RP3D_PROFILE("OverlappingPairs::clearObsoleteLastFrameCollisionInfos()", mProfiler); // For each convex vs convex overlapping pair - for (uint p=0; p < mConvexLastFrameCollisionInfos.size(); p++) { + for (uint64 i=0; i < mNbPairs; i++) { // For each collision info - for (auto it = mConvexLastFrameCollisionInfos[p].begin(); it != mConvexLastFrameCollisionInfos[p].end(); ) { + for (auto it = mLastFrameCollisionInfos[i].begin(); it != mLastFrameCollisionInfos[i].end(); ) { // If the collision info is obsolete if (it->second->isObsolete) { @@ -228,32 +413,7 @@ void OverlappingPairs::clearObsoleteLastFrameCollisionInfos() { it->second->~LastFrameCollisionInfo(); mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - it = mConvexLastFrameCollisionInfos[p].remove(it); - } - else { // If the collision info is not obsolete - - // Do not delete it but mark it as obsolete - it->second->isObsolete = true; - - ++it; - } - } - } - - // For each convex vs concave overlapping pair - for (uint p=0; p < mConcaveLastFrameCollisionInfos.size(); p++) { - - // For each collision info - for (auto it = mConcaveLastFrameCollisionInfos[p].begin(); it != mConcaveLastFrameCollisionInfos[p].end(); ) { - - // If the collision info is obsolete - if (it->second->isObsolete) { - - // Delete it - it->second->~LastFrameCollisionInfo(); - mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); - - it = mConcaveLastFrameCollisionInfos[p].remove(it); + it = mLastFrameCollisionInfos[i].remove(it); } else { // If the collision info is not obsolete @@ -265,3 +425,28 @@ void OverlappingPairs::clearObsoleteLastFrameCollisionInfos() { } } } + +// Return true if the overlapping pair between two shapes is active +bool OverlappingPairs::computeIsPairActive(ProxyShape* shape1, ProxyShape* shape2) { + + const Entity body1Entity = mProxyShapeComponents.getBody(shape1->getEntity()); + const Entity body2Entity = mProxyShapeComponents.getBody(shape2->getEntity()); + + const bool isStaticRigidBody1 = mRigidBodyComponents.hasComponent(body1Entity) && + mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC; + const bool isStaticRigidBody2 = mRigidBodyComponents.hasComponent(body2Entity) && + mRigidBodyComponents.getBodyType(body2Entity) == BodyType::STATIC; + + // Check that at least one body is enabled (active and awake) and not static + // TODO : Do not test this every frame + bool isBody1Active = !mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1; + bool isBody2Active = !mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2; + if (!isBody1Active && !isBody2Active) return false; + + // Check if the bodies are in the set of bodies that cannot collide between each other + // TODO : Do not check this every frame but remove and do not create overlapping pairs of bodies in this situation + bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); + if (mNoCollisionPairs.contains(bodiesIndex) > 0) return false; + + return true; +} diff --git a/src/engine/OverlappingPairs.h b/src/engine/OverlappingPairs.h index 23800c63..6dec1fd6 100644 --- a/src/engine/OverlappingPairs.h +++ b/src/engine/OverlappingPairs.h @@ -30,9 +30,12 @@ #include "collision/ProxyShape.h" #include "containers/Map.h" #include "containers/Pair.h" +#include "containers/Set.h" #include "containers/containers_common.h" #include "utils/Profiler.h" #include "components/ProxyShapeComponents.h" +#include "components/CollisionBodyComponents.h" +#include "components/RigidBodyComponents.h" #include /// ReactPhysics3D namespace @@ -106,26 +109,11 @@ class OverlappingPairs { private: - /// Structure PairLocation - struct PairLocation { + // -------------------- Constants -------------------- // - /// True if the pair is a convex vs convex overlap - bool isConvexVsConvex; - /// Index of the overlapping pair in the internal arrays - uint32 pairIndex; - - /// Constructor - PairLocation() :isConvexVsConvex(true), pairIndex(0) { - - } - - /// Constructor - PairLocation(bool isConvexVsConvex, uint32 index) - :isConvexVsConvex(isConvexVsConvex), pairIndex(index) { - - } - }; + /// Number of pairs to allocated at the beginning + const uint32 INIT_NB_ALLOCATED_PAIRS = 10; // -------------------- Attributes -------------------- // @@ -133,54 +121,68 @@ class OverlappingPairs { MemoryAllocator& mPersistentAllocator; /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo + // TODO : Do we need to keep this ? MemoryAllocator& mTempMemoryAllocator; - /// Map a pair id to its local location - Map mMapPairIdToPairIndex; + /// Current number of components + uint64 mNbPairs; + + /// Index in the array of the first convex vs concave pair + uint64 mConcavePairsStartIndex; + + /// Size (in bytes) of a single pair + size_t mPairDataSize; + + /// Number of allocated pairs + uint64 mNbAllocatedPairs; + + /// Allocated memory for all the data of the pairs + void* mBuffer; + + /// Map a pair id to the internal array index + Map mMapPairIdToPairIndex; /// Ids of the convex vs convex pairs // TODO : Check if we need this array - List mConvexPairIds; + uint64* mPairIds; - /// List of Entity of the first proxy-shape of the convex vs convex pairs - List mConvexProxyShapes1; + /// Array with the broad-phase Ids of the first shape + int32* mPairBroadPhaseId1; - /// List of Entity of the second proxy-shape of the convex vs convex pairs - List mConvexProxyShapes2; + /// Array with the broad-phase Ids of the second shape + int32* mPairBroadPhaseId2; - /// Ids of the convex vs concave pairs - // TODO : Check if we need this array - List mConcavePairIds; + /// Array of Entity of the first proxy-shape of the convex vs convex pairs + Entity* mProxyShapes1; - /// List of Entity of the first proxy-shape of the convex vs concave pairs - List mConcaveProxyShapes1; - - /// List of Entity of the second proxy-shape of the convex vs concave pairs - List mConcaveProxyShapes2; + /// Array of Entity of the second proxy-shape of the convex vs convex pairs + Entity* mProxyShapes2; /// Temporal coherence collision data for each overlapping collision shapes of this pair. /// Temporal coherence data store collision information about the last frame. /// If two convex shapes overlap, we have a single collision data but if one shape is concave, /// we might have collision data for several overlapping triangles. The key in the map is the /// shape Ids of the two collision shapes. - List> mConvexLastFrameCollisionInfos; - - /// Temporal coherence collision data for each overlapping collision shapes of this pair. - /// Temporal coherence data store collision information about the last frame. - /// If two convex shapes overlap, we have a single collision data but if one shape is concave, - /// we might have collision data for several overlapping triangles. The key in the map is the - /// shape Ids of the two collision shapes. - List> mConcaveLastFrameCollisionInfos; + Map* mLastFrameCollisionInfos; /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap - List mConvexNeedToTestOverlap; + bool* mNeedToTestOverlap; - /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap - List mConcaveNeedToTestOverlap; + /// True if the overlapping pair is active (at least one body of the pair is active and not static) + bool* mIsActive; /// Reference to the proxy-shapes components ProxyShapeComponents& mProxyShapeComponents; + /// Reference to the collision body components + CollisionBodyComponents& mCollisionBodyComponents; + + /// Reference to the rigid bodies components + RigidBodyComponents& mRigidBodyComponents; + + /// Reference to the set of bodies that cannot collide with each others + Set& mNoCollisionPairs; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -188,13 +190,31 @@ class OverlappingPairs { #endif + // -------------------- Methods -------------------- // + + /// Allocate memory for a given number of pairs + void allocate(uint64 nbPairsToAllocate); + + /// Compute the index where we need to insert the new pair + uint64 prepareAddPair(bool isConvexVsConvex); + + /// Destroy a pair at a given index + void destroyPair(uint64 index); + + // Move a pair from a source to a destination index in the pairs array + void movePairToIndex(uint64 srcIndex, uint64 destIndex); + + /// Swap two pairs in the array + void swapPairs(uint64 index1, uint64 index2); + public: // -------------------- Methods -------------------- // /// Constructor OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, - ProxyShapeComponents& proxyShapeComponents); + ProxyShapeComponents& proxyShapeComponents, CollisionBodyComponents& collisionBodyComponents, + RigidBodyComponents& rigidBodyComponents, Set& noCollisionPairs); /// Destructor ~OverlappingPairs(); @@ -206,13 +226,22 @@ class OverlappingPairs { OverlappingPairs& operator=(const OverlappingPairs& pair) = delete; /// Add an overlapping pair - uint64 addPair(ProxyShape* shape1, ProxyShape* shape2); + uint64 addPair(ProxyShape* shape1, ProxyShape* shape2, bool isActive); - /// Remove an overlapping pair - uint64 removePair(uint64 pairId); + /// Remove a component at a given index + void removePair(uint64 pairId); - /// Try to find a pair with a given id, return true if the pair is found and the corresponding PairLocation - bool findPair(uint64 pairId, PairLocation& pairLocation); + /// Return the number of pairs + uint64 getNbPairs() const; + + /// Return the number of convex vs convex pairs + uint64 getNbConvexVsConvexPairs() const; + + /// Return the number of convex vs concave pairs + uint64 getNbConvexVsConcavePairs() const; + + /// Return the starting index of the convex vs concave pairs + uint64 getConvexVsConcavePairsStartIndex() const; /// Return the entity of the first proxy-shape Entity getProxyShape1(uint64 pairId) const; @@ -220,6 +249,9 @@ class OverlappingPairs { /// Return the entity of the second proxy-shape Entity getProxyShape2(uint64 pairId) const; + /// Notify if a given pair is active or not + void setIsPairActive(uint64 pairId, bool isActive); + /// Return the last frame collision info LastFrameCollisionInfo* getLastFrameCollisionInfo(uint64, ShapeIdPair& shapeIds); @@ -235,6 +267,12 @@ class OverlappingPairs { /// Return the pair of bodies index of the pair static bodypair computeBodiesIndexPair(Entity body1Entity, Entity body2Entity); + /// Return true if the overlapping pair between two shapes is active + bool computeIsPairActive(ProxyShape* shape1, ProxyShape* shape2); + + /// Set if we need to test a given pair for overlap + void setNeedToTestOverlap(uint64 pairId, bool needToTestOverlap); + #ifdef IS_PROFILING_ACTIVE /// Set the profiler @@ -251,29 +289,34 @@ class OverlappingPairs { // Return the entity of the first proxy-shape inline Entity OverlappingPairs::getProxyShape1(uint64 pairId) const { assert(mMapPairIdToPairIndex.containsKey(pairId)); - const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; - const List proxyShapes1 = pairLocation.isConvexVsConvex ? mConvexProxyShapes1 : mConcaveProxyShapes1; - return proxyShapes1[pairLocation.pairIndex]; + assert(mMapPairIdToPairIndex[pairId] < mNbPairs); + return mProxyShapes1[mMapPairIdToPairIndex[pairId]]; } // Return the entity of the second proxy-shape inline Entity OverlappingPairs::getProxyShape2(uint64 pairId) const { assert(mMapPairIdToPairIndex.containsKey(pairId)); - const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; - const List proxyShapes2 = pairLocation.isConvexVsConvex ? mConvexProxyShapes2 : mConcaveProxyShapes2; - return proxyShapes2[pairLocation.pairIndex]; + assert(mMapPairIdToPairIndex[pairId] < mNbPairs); + return mProxyShapes2[mMapPairIdToPairIndex[pairId]]; +} + +// Notify if a given pair is active or not +inline void OverlappingPairs::setIsPairActive(uint64 pairId, bool isActive) { + + assert(mMapPairIdToPairIndex.containsKey(pairId)); + assert(mMapPairIdToPairIndex[pairId] < mNbPairs); + mIsActive[mMapPairIdToPairIndex[pairId]] = isActive; } // Return the last frame collision info for a given shape id or nullptr if none is found inline LastFrameCollisionInfo* OverlappingPairs::getLastFrameCollisionInfo(uint64 pairId, ShapeIdPair& shapeIds) { assert(mMapPairIdToPairIndex.containsKey(pairId)); - const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId]; - const List>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ? - mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos; + const uint64 index = mMapPairIdToPairIndex[pairId]; + assert(index < mNbPairs); - Map::Iterator it = lastFrameCollisionInfos[pairLocation.pairIndex].find(shapeIds); - if (it != lastFrameCollisionInfos[pairLocation.pairIndex].end()) { + Map::Iterator it = mLastFrameCollisionInfos[index].find(shapeIds); + if (it != mLastFrameCollisionInfos[index].end()) { return it->second; } @@ -291,11 +334,37 @@ inline bodypair OverlappingPairs::computeBodiesIndexPair(Entity body1Entity, Ent return indexPair; } +// Return the number of pairs +inline uint64 OverlappingPairs::getNbPairs() const { + return mNbPairs; +} + +// Return the number of convex vs convex pairs +inline uint64 OverlappingPairs::getNbConvexVsConvexPairs() const { + return mConcavePairsStartIndex; +} + +// Return the number of convex vs concave pairs +inline uint64 OverlappingPairs::getNbConvexVsConcavePairs() const { + return mNbPairs - mConcavePairsStartIndex; +} + +// Return the starting index of the convex vs concave pairs +inline uint64 OverlappingPairs::getConvexVsConcavePairsStartIndex() const { + return mConcavePairsStartIndex; +} + // Return a reference to the temporary memory allocator inline MemoryAllocator& OverlappingPairs::getTemporaryAllocator() { return mTempMemoryAllocator; } +// Set if we need to test a given pair for overlap +inline void OverlappingPairs::setNeedToTestOverlap(uint64 pairId, bool needToTestOverlap) { + assert(mMapPairIdToPairIndex.containsKey(pairId)); + mNeedToTestOverlap[mMapPairIdToPairIndex[pairId]] = needToTestOverlap; +} + #ifdef IS_PROFILING_ACTIVE // Set the profiler diff --git a/src/memory/PoolAllocator.cpp b/src/memory/PoolAllocator.cpp index 7e9c0a93..3126ff9e 100644 --- a/src/memory/PoolAllocator.cpp +++ b/src/memory/PoolAllocator.cpp @@ -54,7 +54,7 @@ PoolAllocator::PoolAllocator() { // If the mMapSizeToHeapIndex has not been initialized yet if (!isMapSizeToHeadIndexInitialized) { - // Initialize the array that contains the sizes the memory units that will + // Initialize the array that contains the sizes of the memory units that will // be allocated in each different heap for (uint i=0; i < NB_HEAPS; i++) { mUnitSizes[i] = (i+1) * 8; @@ -100,6 +100,8 @@ PoolAllocator::~PoolAllocator() { // allocated memory. void* PoolAllocator::allocate(size_t size) { + assert(size > 0); + // We cannot allocate zero bytes if (size == 0) return nullptr; @@ -173,6 +175,8 @@ void* PoolAllocator::allocate(size_t size) { // Release previously allocated memory. void PoolAllocator::release(void* pointer, size_t size) { + assert(size > 0); + // Cannot release a 0-byte allocated memory if (size == 0) return; diff --git a/src/systems/BroadPhaseSystem.cpp b/src/systems/BroadPhaseSystem.cpp index 1d3b9968..dbcdaf16 100644 --- a/src/systems/BroadPhaseSystem.cpp +++ b/src/systems/BroadPhaseSystem.cpp @@ -51,13 +51,10 @@ BroadPhaseSystem::BroadPhaseSystem(CollisionDetectionSystem& collisionDetection, } // Return true if the two broad-phase collision shapes are overlapping -bool BroadPhaseSystem::testOverlappingShapes(Entity proxyShape1Entity, Entity proxyShape2Entity) const { +bool BroadPhaseSystem::testOverlappingShapes(int32 shape1BroadPhaseId, int32 shape2BroadPhaseId) const { RP3D_PROFILE("CollisionDetectionSystem::testOverlappingShapes()", mProfiler); - const int32 shape1BroadPhaseId = mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity); - const int32 shape2BroadPhaseId = mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity); - assert(shape1BroadPhaseId != -1 && shape2BroadPhaseId != -1); // Get the two AABBs of the collision shapes @@ -92,7 +89,7 @@ void BroadPhaseSystem::addProxyCollisionShape(ProxyShape* proxyShape, const AABB // Add the collision shape into the array of bodies that have moved (or have been created) // during the last simulation step - addMovedCollisionShape(proxyShape->getBroadPhaseId()); + addMovedCollisionShape(proxyShape->getBroadPhaseId(), proxyShape); } // Remove a proxy collision shape from the broad-phase collision detection @@ -130,11 +127,13 @@ void BroadPhaseSystem::updateProxyShapes(decimal timeStep) { RP3D_PROFILE("BroadPhaseSystem::updateProxyShapes()", mProfiler); // Update all the enabled proxy-shape components - updateProxyShapesComponents(0, mProxyShapesComponents.getNbEnabledComponents(), timeStep); + if (mProxyShapesComponents.getNbEnabledComponents() > 0) { + updateProxyShapesComponents(0, mProxyShapesComponents.getNbEnabledComponents(), timeStep); + } } // Notify the broad-phase that a collision shape has moved and need to be updated -void BroadPhaseSystem::updateProxyShapeInternal(int32 broadPhaseId, const AABB& aabb, const Vector3& displacement) { +void BroadPhaseSystem::updateProxyShapeInternal(int32 broadPhaseId, ProxyShape* proxyShape, const AABB& aabb, const Vector3& displacement) { assert(broadPhaseId >= 0); @@ -147,7 +146,7 @@ void BroadPhaseSystem::updateProxyShapeInternal(int32 broadPhaseId, const AABB& // Add the collision shape into the array of shapes that have moved (or have been created) // during the last simulation step - addMovedCollisionShape(broadPhaseId); + addMovedCollisionShape(broadPhaseId, proxyShape); } } @@ -167,6 +166,7 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbI // For each proxy-shape component to update for (uint32 i = startIndex; i < startIndex + nbItems; i++) { + // TODO : Can we remove this test const int32 broadPhaseId = mProxyShapesComponents.mBroadPhaseIds[i]; if (broadPhaseId != -1) { @@ -188,13 +188,27 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbI mProxyShapesComponents.mCollisionShapes[i]->computeAABB(aabb, transform * mProxyShapesComponents.mLocalToBodyTransforms[i]); // Update the broad-phase state for the proxy collision shape - updateProxyShapeInternal(broadPhaseId, aabb, displacement); + updateProxyShapeInternal(broadPhaseId, mProxyShapesComponents.mProxyShapes[i], aabb, displacement); } } } + +// Add a collision shape in the array of shapes that have moved in the last simulation step +// and that need to be tested again for broad-phase overlapping. +void BroadPhaseSystem::addMovedCollisionShape(int broadPhaseID, ProxyShape* proxyShape) { + + assert(broadPhaseID != -1); + + // Store the broad-phase ID into the array of shapes that have moved + mMovedShapes.add(broadPhaseID); + + // Notify that the overlapping pairs where this shape is involved need to be tested for overlap + mCollisionDetection.notifyOverlappingPairsToTestOverlap(proxyShape); +} + // Compute all the overlapping pairs of collision shapes -void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager, List>& overlappingNodes) { +void BroadPhaseSystem::computeOverlappingPairs(MemoryManager& memoryManager, List>& overlappingNodes) { RP3D_PROFILE("CollisionDetectionSystem::computeOverlappingPairs()", mProfiler); diff --git a/src/systems/BroadPhaseSystem.h b/src/systems/BroadPhaseSystem.h index 8e44f19e..c3d8b060 100644 --- a/src/systems/BroadPhaseSystem.h +++ b/src/systems/BroadPhaseSystem.h @@ -140,7 +140,7 @@ class BroadPhaseSystem { // -------------------- Methods -------------------- // /// Notify the Dynamic AABB tree that a proxy-shape needs to be updated - void updateProxyShapeInternal(int32 broadPhaseId, const AABB& aabb, const Vector3& displacement); + void updateProxyShapeInternal(int32 broadPhaseId, ProxyShape* proxyShape, const AABB& aabb, const Vector3& displacement); /// Update the broad-phase state of some proxy-shapes components void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems, decimal timeStep); @@ -176,20 +176,20 @@ class BroadPhaseSystem { /// Add a collision shape in the array of shapes that have moved in the last simulation step /// and that need to be tested again for broad-phase overlapping. - void addMovedCollisionShape(int broadPhaseID); + void addMovedCollisionShape(int broadPhaseID, ProxyShape* proxyShape); /// Remove a collision shape from the array of shapes that have moved in the last simulation /// step and that need to be tested again for broad-phase overlapping. void removeMovedCollisionShape(int broadPhaseID); /// Compute all the overlapping pairs of collision shapes - void computeOverlappingPairs(MemoryManager& memoryManager, List>& overlappingNodes); + void computeOverlappingPairs(MemoryManager& memoryManager, List>& overlappingNodes); /// Return the proxy shape corresponding to the broad-phase node id in parameter ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const; /// Return true if the two broad-phase collision shapes are overlapping - bool testOverlappingShapes(Entity proxyShape1Entity, Entity proxyShape2Entity) const; + bool testOverlappingShapes(int32 shape1BroadPhaseId, int32 shape2BroadPhaseId) const; /// Return the fat AABB of a given broad-phase shape const AABB& getFatAABB(int broadPhaseId) const; @@ -211,16 +211,6 @@ inline const AABB& BroadPhaseSystem::getFatAABB(int broadPhaseId) const { return mDynamicAABBTree.getFatAABB(broadPhaseId); } -// Add a collision shape in the array of shapes that have moved in the last simulation step -// and that need to be tested again for broad-phase overlapping. -inline void BroadPhaseSystem::addMovedCollisionShape(int broadPhaseID) { - - assert(broadPhaseID != -1); - - // Store the broad-phase ID into the array of shapes that have moved - mMovedShapes.add(broadPhaseID); -} - // Remove a collision shape from the array of shapes that have moved in the last simulation step // and that need to be tested again for broad-phase overlapping. inline void BroadPhaseSystem::removeMovedCollisionShape(int broadPhaseID) { diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index e98f7978..7d5ee9a3 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -52,12 +52,14 @@ using namespace std; // Constructor CollisionDetectionSystem::CollisionDetectionSystem(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents, - RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager) + CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager) : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world), - mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mProxyShapesComponents), + mNoCollisionPairs(mMemoryManager.getPoolAllocator()), + mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mProxyShapesComponents, + collisionBodyComponents, rigidBodyComponents, mNoCollisionPairs), mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, rigidBodyComponents), - mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()), + mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()), mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()), // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds, mMapPairIdToOverlappingPairContacts mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()), @@ -100,12 +102,10 @@ void CollisionDetectionSystem::computeBroadPhase() { RP3D_PROFILE("CollisionDetectionSystem::computeBroadPhase()", mProfiler); - resetNeedToTestOverlap(); - // Ask the broad-phase to compute all the shapes overlapping the shapes that // have moved or have been added in the last frame. This call can only add new // overlapping pairs in the collision detection. - List> overlappingNodes(mMemoryManager.getPoolAllocator(), 32); + List> overlappingNodes(mMemoryManager.getPoolAllocator(), 32); mBroadPhaseSystem.computeOverlappingPairs(mMemoryManager, overlappingNodes); // Create new overlapping pairs if necessary @@ -115,63 +115,38 @@ void CollisionDetectionSystem::computeBroadPhase() { removeNonOverlappingPairs(); } -// Set the needToTestOverlap value of each overlapping pair to true -void CollisionDetectionSystem::resetNeedToTestOverlap() { - - RP3D_PROFILE("CollisionDetectionSystem::resetNeedToTestOverlap()", mProfiler); - - // For each possible convex vs convex collision pair of bodies - for (uint i=0; i < mOverlappingPairs.mConvexNeedToTestOverlap.size(); i++) { - mOverlappingPairs.mConvexNeedToTestOverlap[i] = true; - } - - // For each possible convex vs concave collision pair of bodies - for (uint i=0; i < mOverlappingPairs.mConcaveNeedToTestOverlap.size(); i++) { - mOverlappingPairs.mConcaveNeedToTestOverlap[i] = true; - } -} - // Remove pairs that are not overlapping anymore void CollisionDetectionSystem::removeNonOverlappingPairs() { RP3D_PROFILE("CollisionDetectionSystem::removeNonOverlappingPairs()", mProfiler); // For each possible convex vs convex pair of bodies - for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { + for (uint64 i=0; i < mOverlappingPairs.getNbPairs(); i++) { // Check if we need to test overlap. If so, test if the two shapes are still overlapping. // Otherwise, we destroy the overlapping pair - if (mOverlappingPairs.mConvexNeedToTestOverlap[i] && - !mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mConvexProxyShapes1[i], mOverlappingPairs.mConvexProxyShapes2[i])) { + if (mOverlappingPairs.mNeedToTestOverlap[i]) { - mOverlappingPairs.removePair(mOverlappingPairs.mConvexPairIds[i]); - i--; - } - } - - // For each possible convex vs concave pair of bodies - for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { - - // Check if we need to test overlap. If so, test if the two shapes are still overlapping. - // Otherwise, we destroy the overlapping pair - if (mOverlappingPairs.mConcaveNeedToTestOverlap[i] && - !mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mConcaveProxyShapes1[i], mOverlappingPairs.mConcaveProxyShapes2[i])) { - - mOverlappingPairs.removePair(mOverlappingPairs.mConcavePairIds[i]); - i--; + if(mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mPairBroadPhaseId1[i], mOverlappingPairs.mPairBroadPhaseId2[i])) { + mOverlappingPairs.mNeedToTestOverlap[i] = false; + } + else { + mOverlappingPairs.removePair(mOverlappingPairs.mPairIds[i]); + i--; + } } } } // Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary -void CollisionDetectionSystem::updateOverlappingPairs(const List>& overlappingNodes) { +void CollisionDetectionSystem::updateOverlappingPairs(const List>& overlappingNodes) { RP3D_PROFILE("CollisionDetectionSystem::updateOverlappingPairs()", mProfiler); // For each overlapping pair of nodes for (uint i=0; i < overlappingNodes.size(); i++) { - Pair nodePair = overlappingNodes[i]; + Pair nodePair = overlappingNodes[i]; assert(nodePair.first != -1); assert(nodePair.second != -1); @@ -194,8 +169,8 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List> const uint64 pairId = pairNumbers(std::max(nodePair.first, nodePair.second), std::min(nodePair.first, nodePair.second)); // Check if the overlapping pair already exists - OverlappingPairs::PairLocation pairLocation; - if (!mOverlappingPairs.findPair(pairId, pairLocation)) { + auto it = mOverlappingPairs.mMapPairIdToPairIndex.find(pairId); + if (it == mOverlappingPairs.mMapPairIdToPairIndex.end()) { unsigned short shape1CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity); unsigned short shape2CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity); @@ -214,17 +189,14 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List> if (shape1->getCollisionShape()->isConvex() || shape2->getCollisionShape()->isConvex()) { // Add the new overlapping pair - mOverlappingPairs.addPair(shape1, shape2); + mOverlappingPairs.addPair(shape1, shape2, mOverlappingPairs.computeIsPairActive(shape1, shape2)); } } } else { // We do not need to test the pair for overlap because it has just been reported that they still overlap - List& pairsNeedToTestOverlap = pairLocation.isConvexVsConvex ? mOverlappingPairs.mConvexNeedToTestOverlap : - mOverlappingPairs.mConcaveNeedToTestOverlap; - - pairsNeedToTestOverlap[pairLocation.pairIndex] = false; + mOverlappingPairs.mNeedToTestOverlap[it->second] = false; } } } @@ -243,10 +215,10 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP overlappingPairs.clearObsoleteLastFrameCollisionInfos(); // For each possible convex vs convex pair of bodies - for (uint i=0; i < overlappingPairs.mConvexPairIds.size(); i++) { + for (uint64 i=0; i < overlappingPairs.getNbConvexVsConvexPairs(); i++) { - const Entity proxyShape1Entity = overlappingPairs.mConvexProxyShapes1[i]; - const Entity proxyShape2Entity = overlappingPairs.mConvexProxyShapes2[i]; + const Entity proxyShape1Entity = overlappingPairs.mProxyShapes1[i]; + const Entity proxyShape2Entity = overlappingPairs.mProxyShapes2[i]; assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); @@ -280,7 +252,7 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP // No middle-phase is necessary, simply create a narrow phase info // for the narrow-phase collision detection - narrowPhaseInput.addNarrowPhaseTest(overlappingPairs.mConvexPairIds[i], proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2, + narrowPhaseInput.addNarrowPhaseTest(overlappingPairs.mPairIds[i], proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2, mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity), mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity), algorithmType, mMemoryManager.getSingleFrameAllocator()); @@ -288,10 +260,11 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP } // For each possible convex vs concave pair of bodies - for (uint i=0; i < overlappingPairs.mConcavePairIds.size(); i++) { + uint64 convexVsConcaveStartIndex = overlappingPairs.getConvexVsConcavePairsStartIndex(); + for (uint64 i=convexVsConcaveStartIndex; i < convexVsConcaveStartIndex + overlappingPairs.getNbConvexVsConcavePairs(); i++) { - const Entity proxyShape1Entity = overlappingPairs.mConcaveProxyShapes1[i]; - const Entity proxyShape2Entity = overlappingPairs.mConcaveProxyShapes2[i]; + const Entity proxyShape1Entity = overlappingPairs.mProxyShapes1[i]; + const Entity proxyShape2Entity = overlappingPairs.mProxyShapes2[i]; assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); @@ -316,7 +289,7 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingP bodypair bodiesIndex = OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity); if (mNoCollisionPairs.contains(bodiesIndex) > 0) continue; - computeConvexVsConcaveMiddlePhase(overlappingPairs.mConcavePairIds[i], proxyShape1Entity, proxyShape2Entity, + computeConvexVsConcaveMiddlePhase(overlappingPairs.mPairIds[i], proxyShape1Entity, proxyShape2Entity, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput); } } @@ -333,16 +306,15 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& mOverlappingPairs.clearObsoleteLastFrameCollisionInfos(); // For each possible convex vs convex pair of bodies - for (uint p=0; p < convexPairs.size(); p++) { + for (uint64 p=0; p < convexPairs.size(); p++) { const uint64 pairId = convexPairs[p]; - OverlappingPairs::PairLocation& pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; - assert(pairLoc.isConvexVsConvex); - const uint pairIndex = pairLoc.pairIndex; + const uint64 pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; + assert(pairIndex < mOverlappingPairs.getNbPairs()); - const Entity proxyShape1Entity = mOverlappingPairs.mConvexProxyShapes1[pairIndex]; - const Entity proxyShape2Entity = mOverlappingPairs.mConvexProxyShapes2[pairIndex]; + const Entity proxyShape1Entity = mOverlappingPairs.mProxyShapes1[pairIndex]; + const Entity proxyShape2Entity = mOverlappingPairs.mProxyShapes2[pairIndex]; assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); @@ -388,12 +360,10 @@ void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List& const uint64 pairId = concavePairs[p]; - OverlappingPairs::PairLocation& pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; - assert(!pairLoc.isConvexVsConvex); - const uint pairIndex = pairLoc.pairIndex; + const uint pairIndex = mOverlappingPairs.mMapPairIdToPairIndex[pairId]; - const Entity proxyShape1Entity = mOverlappingPairs.mConcaveProxyShapes1[pairIndex]; - const Entity proxyShape2Entity = mOverlappingPairs.mConcaveProxyShapes2[pairIndex]; + const Entity proxyShape1Entity = mOverlappingPairs.mProxyShapes1[pairIndex]; + const Entity proxyShape2Entity = mOverlappingPairs.mProxyShapes2[pairIndex]; assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1); assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1); @@ -651,6 +621,19 @@ void CollisionDetectionSystem::computeSnapshotContactPairs(NarrowPhaseInput& nar computeSnapshotContactPairs(convexPolyhedronVsConvexPolyhedronBatch, overlapPairs); } +// Notify that the overlapping pairs where a given proxy-shape is involved need to be tested for overlap +void CollisionDetectionSystem::notifyOverlappingPairsToTestOverlap(ProxyShape* proxyShape) { + + // Get the overlapping pairs involved with this proxy-shape + List& overlappingPairs = mProxyShapesComponents.getOverlappingPairs(proxyShape->getEntity()); + + for (uint i=0; i < overlappingPairs.size(); i++) { + + // Notify that the overlapping pair needs to be testbed for overlap + mOverlappingPairs.setNeedToTestOverlap(overlappingPairs[i], true); + } +} + // Convert the potential overlapping bodies for the testOverlap() methods void CollisionDetectionSystem::computeSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List>& overlapPairs) const { @@ -963,32 +946,14 @@ void CollisionDetectionSystem::removeProxyCollisionShape(ProxyShape* proxyShape) assert(proxyShapeBroadPhaseId != -1); assert(mMapBroadPhaseIdToProxyShapeEntity.containsKey(proxyShapeBroadPhaseId)); - // Remove all the convex vs convex overlapping pairs involving this proxy shape - for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { + // Remove all the overlapping pairs involving this proxy shape + List& overlappingPairs = mProxyShapesComponents.getOverlappingPairs(proxyShape->getEntity()); + while(overlappingPairs.size() > 0) { - if (mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConvexProxyShapes1[i]) == proxyShapeBroadPhaseId || - mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConvexProxyShapes2[i]) == proxyShapeBroadPhaseId) { + // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - - // Remove the overlapping pair - mOverlappingPairs.removePair(mOverlappingPairs.mConvexPairIds[i]); - i--; - } - } - - // Remove all the convex vs concave overlapping pairs involving this proxy shape - for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { - - if (mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConcaveProxyShapes1[i]) == proxyShapeBroadPhaseId || - mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConcaveProxyShapes2[i]) == proxyShapeBroadPhaseId) { - - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - - // Remove the overlapping pair - mOverlappingPairs.removePair(mOverlappingPairs.mConcavePairIds[i]); - i--; - } + // Remove the overlapping pair + mOverlappingPairs.removePair(overlappingPairs[0]); } mMapBroadPhaseIdToProxyShapeEntity.remove(proxyShapeBroadPhaseId); @@ -1205,9 +1170,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List // If there are two many contact points in the manifold if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) { - OverlappingPairs::PairLocation pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]; - Entity proxyShape1 = pairLoc.isConvexVsConvex ? mOverlappingPairs.mConvexProxyShapes1[pairLoc.pairIndex] : - mOverlappingPairs.mConcaveProxyShapes1[pairLoc.pairIndex]; + Entity proxyShape1 = mOverlappingPairs.mProxyShapes1[mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]]; Transform shape1LocalToWorldTransoform = mProxyShapesComponents.getLocalToWorldTransform(proxyShape1); @@ -1572,22 +1535,12 @@ void CollisionDetectionSystem::testCollision(CollisionCallback& callback) { void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, List& convexPairs, List& concavePairs) const { // For each possible collision pair of bodies - for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { + for (uint i=0; i < mOverlappingPairs.getNbPairs(); i++) { - if (mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == bodyEntity || - mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == bodyEntity) { + if (mProxyShapesComponents.getBody(mOverlappingPairs.mProxyShapes1[i]) == bodyEntity || + mProxyShapesComponents.getBody(mOverlappingPairs.mProxyShapes2[i]) == bodyEntity) { - convexPairs.add(mOverlappingPairs.mConvexPairIds[i]); - } - } - - // For each possible collision pair of bodies - for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { - - if (mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes1[i]) == bodyEntity || - mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes2[i]) == bodyEntity) { - - concavePairs.add(mOverlappingPairs.mConcavePairIds[i]); + convexPairs.add(mOverlappingPairs.mPairIds[i]); } } } @@ -1598,22 +1551,12 @@ void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity // TODO : Do not go through all the overlapping pairs here but get all the involded overlapping pairs directly from the bodies // For each possible collision pair of bodies - for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) { + for (uint i=0; i < mOverlappingPairs.getNbPairs(); i++) { - if ((mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == body1Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == body2Entity) || - (mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == body2Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == body1Entity)) { + if ((mProxyShapesComponents.getBody(mOverlappingPairs.mProxyShapes1[i]) == body1Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mProxyShapes1[i]) == body2Entity) || + (mProxyShapesComponents.getBody(mOverlappingPairs.mProxyShapes2[i]) == body2Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mProxyShapes2[i]) == body1Entity)) { - convexPairs.add(mOverlappingPairs.mConvexPairIds[i]); - } - } - - // For each possible collision pair of bodies - for (uint i=0; i < mOverlappingPairs.mConcavePairIds.size(); i++) { - - if ((mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes1[i]) == body1Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes2[i]) == body2Entity) || - (mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes1[i]) == body2Entity && mProxyShapesComponents.getBody(mOverlappingPairs.mConcaveProxyShapes2[i]) == body1Entity)) { - - concavePairs.add(mOverlappingPairs.mConcavePairIds[i]); + convexPairs.add(mOverlappingPairs.mPairIds[i]); } } } diff --git a/src/systems/CollisionDetectionSystem.h b/src/systems/CollisionDetectionSystem.h index 5ac964d9..a15a6fde 100644 --- a/src/systems/CollisionDetectionSystem.h +++ b/src/systems/CollisionDetectionSystem.h @@ -89,15 +89,15 @@ class CollisionDetectionSystem { /// Pointer to the physics world CollisionWorld* mWorld; + /// Set of pair of bodies that cannot collide between each other + Set mNoCollisionPairs; + /// Broad-phase overlapping pairs OverlappingPairs mOverlappingPairs; /// Broad-phase system BroadPhaseSystem mBroadPhaseSystem; - /// Set of pair of bodies that cannot collide between each other - Set mNoCollisionPairs; - /// Map a broad-phase id with the corresponding entity of the proxy-shape Map mMapBroadPhaseIdToProxyShapeEntity; @@ -197,10 +197,7 @@ class CollisionDetectionSystem { void computeSnapshotContactPairs(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, List>& overlapPairs) const; /// Take a list of overlapping nodes in the broad-phase and create new overlapping pairs if necessary - void updateOverlappingPairs(const List>& overlappingNodes); - - /// Set the needToTestOverlap value of each overlapping pair to true - void resetNeedToTestOverlap(); + void updateOverlappingPairs(const List >& overlappingNodes); /// Remove pairs that are not overlapping anymore void removeNonOverlappingPairs(); @@ -271,7 +268,7 @@ class CollisionDetectionSystem { /// Constructor CollisionDetectionSystem(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, - TransformComponents& transformComponents, RigidBodyComponents& rigidBodyComponents, + TransformComponents& transformComponents, CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager); /// Destructor @@ -290,7 +287,7 @@ class CollisionDetectionSystem { void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); /// Remove a proxy collision shape from the collision detection - void removeProxyCollisionShape(ProxyShape* proxyShape); + void removeProxyCollisionShape(ProxyShape *proxyShape); /// Update a proxy collision shape (that has moved for instance) void updateProxyShape(Entity proxyShapeEntity, decimal timeStep); @@ -307,6 +304,9 @@ class CollisionDetectionSystem { /// Ask for a collision shape to be tested again during broad-phase. void askForBroadPhaseCollisionCheck(ProxyShape* shape); + /// Notify that the overlapping pairs where a given proxy-shape is involved need to be tested for overlap + void notifyOverlappingPairsToTestOverlap(ProxyShape* proxyShape); + /// Report contacts void reportContacts(); @@ -395,7 +395,7 @@ inline void CollisionDetectionSystem::removeNoCollisionPair(Entity body1Entity, inline void CollisionDetectionSystem::askForBroadPhaseCollisionCheck(ProxyShape* shape) { if (shape->getBroadPhaseId() != -1) { - mBroadPhaseSystem.addMovedCollisionShape(shape->getBroadPhaseId()); + mBroadPhaseSystem.addMovedCollisionShape(shape->getBroadPhaseId(), shape); } }