Working on making middle-phase collision detection faster

CMakeLists.txt

@@ -141,7 +141,7 @@ SET (REACTPHYSICS3D_HEADERS
     "src/engine/Island.h"
     "src/engine/Islands.h"
     "src/engine/Material.h"
-    "src/engine/OverlappingPair.h"
+    "src/engine/OverlappingPairs.h"
     "src/engine/Timer.h"
     "src/systems/BroadPhaseSystem.h"
     "src/components/Components.h"
@@ -239,7 +239,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/engine/DynamicsWorld.cpp"
     "src/engine/Island.cpp"
     "src/engine/Material.cpp"
-    "src/engine/OverlappingPair.cpp"
+    "src/engine/OverlappingPairs.cpp"
     "src/engine/Timer.cpp"
     "src/engine/Entity.cpp"
     "src/engine/EntityManager.cpp"

src/body/CollisionBody.cpp

@@ -183,6 +183,9 @@ void CollisionBody::removeCollisionShape(ProxyShape* proxyShape) {
     // Remove the proxy-shape component
     mWorld.mProxyShapesComponents.removeComponent(proxyShape->getEntity());
 
+    // Destroy the entity
+    mWorld.mEntityManager.destroyEntity(proxyShape->getEntity());
+
     // Call the constructor of the proxy-shape
     proxyShape->~ProxyShape();
 

src/collision/ContactManifoldInfo.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "mathematics/mathematics.h"
 #include "configuration.h"
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -49,12 +49,12 @@ struct ContactManifoldInfo {
         List<uint> potentialContactPointsIndices;
 
         /// Overlapping pair id
-        OverlappingPair::OverlappingPairId pairId;
+        uint64 pairId;
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifoldInfo(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
+        ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator)
                : potentialContactPointsIndices(allocator), pairId(pairId) {
 
         }

src/collision/ContactPair.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "mathematics/mathematics.h"
 #include "configuration.h"
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -45,7 +45,7 @@ struct ContactPair {
         // -------------------- Attributes -------------------- //
 
         /// Overlapping pair Id
-        OverlappingPair::OverlappingPairId pairId;
+        uint64 pairId;
 
         /// Indices of the potential contact manifolds
         List<uint> potentialContactManifoldsIndices;
@@ -83,7 +83,7 @@ struct ContactPair {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity,
+        ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity,
                     Entity proxyShape2Entity, uint contactPairIndex, MemoryAllocator& allocator)
             : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
               proxyShape1Entity(proxyShape1Entity), proxyShape2Entity(proxyShape2Entity),

src/collision/broadphase/DynamicAABBTree.cpp

@@ -645,6 +645,8 @@ void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const List<int32>& no
 // Report all shapes overlapping with the AABB given in parameter.
 void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, List<int32>& overlappingNodes) const {
 
+    RP3D_PROFILE("DynamicAABBTree::reportAllShapesOverlappingWithAABB()", mProfiler);
+
     // Create a stack with the nodes to visit
     Stack<int32> stack(mAllocator, 64);
     stack.push(mRootNodeID);

src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.cpp

@@ -30,14 +30,15 @@
 using namespace reactphysics3d;
 
 // Constructor
-CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator)
+CapsuleVsCapsuleNarrowPhaseInfoBatch::CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator,
-      : NarrowPhaseInfoBatch(allocator), capsule1Radiuses(allocator), capsule2Radiuses(allocator),
+                                                                           OverlappingPairs& overlappingPairs)
+      : NarrowPhaseInfoBatch(allocator, overlappingPairs), capsule1Radiuses(allocator), capsule2Radiuses(allocator),
         capsule1Heights(allocator), capsule2Heights(allocator) {
 
 }
 
 // Add shapes to be tested during narrow-phase collision detection into the batch
-void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
+void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2,
                                                             const Transform& shape1Transform, const Transform& shape2Transform) {
 
     assert(shape1->getType() == CollisionShapeType::CAPSULE);
@@ -46,25 +47,29 @@ void CapsuleVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* p
     const CapsuleShape* capsule1 = static_cast<const CapsuleShape*>(shape1);
     const CapsuleShape* capsule2 = static_cast<const CapsuleShape*>(shape2);
 
+    proxyShapeEntities1.add(proxyShape1);
+    proxyShapeEntities2.add(proxyShape2);
     capsule1Radiuses.add(capsule1->getRadius());
     capsule2Radiuses.add(capsule2->getRadius());
     capsule1Heights.add(capsule1->getHeight());
     capsule2Heights.add(capsule2->getHeight());
     shape1ToWorldTransforms.add(shape1Transform);
     shape2ToWorldTransforms.add(shape2Transform);
-    overlappingPairs.add(pair);
+    overlappingPairIds.add(pairId);
     contactPoints.add(List<ContactPointInfo*>(mMemoryAllocator));
     isColliding.add(false);
 
     // Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
-    LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
+    LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId());
     lastFrameCollisionInfos.add(lastFrameInfo);
 }
 
 // Initialize the containers using cached capacity
 void CapsuleVsCapsuleNarrowPhaseInfoBatch::reserveMemory() {
 
-    overlappingPairs.reserve(mCachedCapacity);
+    overlappingPairIds.reserve(mCachedCapacity);
+    proxyShapeEntities1.reserve(mCachedCapacity);
+    proxyShapeEntities2.reserve(mCachedCapacity);
     shape1ToWorldTransforms.reserve(mCachedCapacity);
     shape2ToWorldTransforms.reserve(mCachedCapacity);
     lastFrameCollisionInfos.reserve(mCachedCapacity);
@@ -85,9 +90,11 @@ void CapsuleVsCapsuleNarrowPhaseInfoBatch::clear() {
     // allocated in the next frame at a possibly different location in memory (remember that the
     // location of the allocated memory of a single frame allocator might change between two frames)
 
-    mCachedCapacity = overlappingPairs.size();
+    mCachedCapacity = overlappingPairIds.size();
 
-    overlappingPairs.clear(true);
+    overlappingPairIds.clear(true);
+    proxyShapeEntities1.clear(true);
+    proxyShapeEntities2.clear(true);
     shape1ToWorldTransforms.clear(true);
     shape2ToWorldTransforms.clear(true);
     lastFrameCollisionInfos.clear(true);

src/collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h

@@ -55,13 +55,13 @@ struct CapsuleVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch {
         List<decimal> capsule2Heights;
 
         /// Constructor
-        CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator);
+        CapsuleVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
 
         /// Destructor
         virtual ~CapsuleVsCapsuleNarrowPhaseInfoBatch() = default;
 
         /// Add shapes to be tested during narrow-phase collision detection into the batch
-        virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
+        virtual void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1,
                                         CollisionShape* shape2, const Transform& shape1Transform,
                                         const Transform& shape2Transform);
 

src/collision/narrowphase/CollisionDispatch.cpp

@@ -200,6 +200,8 @@ void CollisionDispatch::fillInCollisionMatrix() {
 NarrowPhaseAlgorithmType CollisionDispatch::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
                                                                        const CollisionShapeType& shape2Type) const {
 
+    RP3D_PROFILE("CollisionDispatch::selectNarrowPhaseAlgorithm()", mProfiler);
+
     uint shape1Index = static_cast<unsigned int>(shape1Type);
     uint shape2Index = static_cast<unsigned int>(shape2Type);
 

src/collision/narrowphase/CollisionDispatch.h

@@ -106,6 +106,13 @@ class CollisionDispatch {
         /// use between two types of collision shapes.
         NarrowPhaseAlgorithmType selectAlgorithm(int type1, int type2);
 
+#ifdef IS_PROFILING_ACTIVE
+
+    /// Pointer to the profiler
+    Profiler* mProfiler;
+
+#endif
+
     public:
 
         /// Constructor
@@ -201,6 +208,7 @@ inline ConvexPolyhedronVsConvexPolyhedronAlgorithm* CollisionDispatch::getConvex
 // Set the profiler
 inline void CollisionDispatch::setProfiler(Profiler* profiler) {
 
+    mProfiler = profiler;
     mSphereVsSphereAlgorithm->setProfiler(profiler);
     mCapsuleVsCapsuleAlgorithm->setProfiler(profiler);
     mSphereVsCapsuleAlgorithm->setProfiler(profiler);

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -26,7 +26,7 @@
 // Libraries
 #include "GJKAlgorithm.h"
 #include "constraint/ContactPoint.h"
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
 #include "collision/shapes/TriangleShape.h"
 #include "configuration.h"
 #include "utils/Profiler.h"

src/collision/narrowphase/NarrowPhaseInfoBatch.cpp

@@ -27,14 +27,15 @@
 #include "NarrowPhaseInfoBatch.h"
 #include "collision/ContactPointInfo.h"
 #include "collision/shapes/TriangleShape.h"
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
 #include <iostream>
 
 using namespace reactphysics3d;
 
 // Constructor
-NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator)
+NarrowPhaseInfoBatch::NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs)
-      : mMemoryAllocator(allocator), overlappingPairs(allocator), collisionShapes1(allocator), collisionShapes2(allocator),
+      : mMemoryAllocator(allocator), mOverlappingPairs(overlappingPairs), overlappingPairIds(allocator),
+        proxyShapeEntities1(allocator), proxyShapeEntities2(allocator), collisionShapes1(allocator), collisionShapes2(allocator),
         shape1ToWorldTransforms(allocator), shape2ToWorldTransforms(allocator),
         isColliding(allocator), contactPoints(allocator), collisionShapeAllocators(allocator),
         lastFrameCollisionInfos(allocator) {
@@ -47,11 +48,13 @@ NarrowPhaseInfoBatch::~NarrowPhaseInfoBatch() {
 }
 
 // Add shapes to be tested during narrow-phase collision detection into the batch
-void NarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
+void NarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2,
                                               const Transform& shape1Transform, const Transform& shape2Transform,
                                               MemoryAllocator& shapeAllocator) {
 
-    overlappingPairs.add(pair);
+    overlappingPairIds.add(pairId);
+    proxyShapeEntities1.add(proxyShape1);
+    proxyShapeEntities2.add(proxyShape2);
     collisionShapes1.add(shape1);
     collisionShapes2.add(shape2);
     shape1ToWorldTransforms.add(shape1Transform);
@@ -61,7 +64,7 @@ void NarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionSh
     isColliding.add(false);
 
     // Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
-    LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
+    LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId());
     lastFrameCollisionInfos.add(lastFrameInfo);
 }
 
@@ -72,7 +75,7 @@ void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNor
     assert(penDepth > decimal(0.0));
 
     // Get the memory allocator
-    MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator();
+    MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator();
 
     // Create the contact point info
     ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
@@ -86,7 +89,7 @@ void NarrowPhaseInfoBatch::addContactPoint(uint index, const Vector3& contactNor
 void NarrowPhaseInfoBatch::resetContactPoints(uint index) {
 
     // Get the memory allocator
-    MemoryAllocator& allocator = overlappingPairs[index]->getTemporaryAllocator();
+    MemoryAllocator& allocator = mOverlappingPairs.getTemporaryAllocator();
 
     // For each remaining contact point info
     for (uint i=0; i < contactPoints[index].size(); i++) {
@@ -106,7 +109,9 @@ void NarrowPhaseInfoBatch::resetContactPoints(uint index) {
 // Initialize the containers using cached capacity
 void NarrowPhaseInfoBatch::reserveMemory() {
 
-    overlappingPairs.reserve(mCachedCapacity);
+    overlappingPairIds.reserve(mCachedCapacity);
+    proxyShapeEntities1.reserve(mCachedCapacity);
+    proxyShapeEntities2.reserve(mCachedCapacity);
     collisionShapes1.reserve(mCachedCapacity);
     collisionShapes2.reserve(mCachedCapacity);
     shape1ToWorldTransforms.reserve(mCachedCapacity);
@@ -120,7 +125,7 @@ void NarrowPhaseInfoBatch::reserveMemory() {
 // Clear all the objects in the batch
 void NarrowPhaseInfoBatch::clear() {
 
-    for (uint i=0; i < overlappingPairs.size(); i++) {
+    for (uint i=0; i < overlappingPairIds.size(); i++) {
 
         assert(contactPoints[i].size() == 0);
 
@@ -141,9 +146,11 @@ void NarrowPhaseInfoBatch::clear() {
     // allocated in the next frame at a possibly different location in memory (remember that the
     // location of the allocated memory of a single frame allocator might change between two frames)
 
-    mCachedCapacity = overlappingPairs.size();
+    mCachedCapacity = overlappingPairIds.size();
 
-    overlappingPairs.clear(true);
+    overlappingPairIds.clear(true);
+    proxyShapeEntities1.clear(true);
+    proxyShapeEntities2.clear(true);
     collisionShapes1.clear(true);
     collisionShapes2.clear(true);
     shape1ToWorldTransforms.clear(true);

src/collision/narrowphase/NarrowPhaseInfoBatch.h

@@ -27,7 +27,7 @@
 #define REACTPHYSICS3D_NARROW_PHASE_INFO_BATCH_H
 
 // Libraries
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -52,13 +52,22 @@ struct NarrowPhaseInfoBatch {
         /// Memory allocator
         MemoryAllocator& mMemoryAllocator;
 
+        /// Reference to all the broad-phase overlapping pairs
+        OverlappingPairs& mOverlappingPairs;
+
         /// Cached capacity
         uint mCachedCapacity = 0;
 
     public:
 
-        /// List of Broadphase overlapping pairs
+        /// List of Broadphase overlapping pairs ids
-        List<OverlappingPair*> overlappingPairs;
+        List<uint64> overlappingPairIds;
+
+        /// List of pointers to the first proxy-shapes to test collision with
+        List<Entity> proxyShapeEntities1;
+
+        /// List of pointers to the second proxy-shapes to test collision with
+        List<Entity> proxyShapeEntities2;
 
         /// List of pointers to the first collision shapes to test collision with
         List<CollisionShape*> collisionShapes1;
@@ -85,7 +94,7 @@ struct NarrowPhaseInfoBatch {
         List<LastFrameCollisionInfo*> lastFrameCollisionInfos;
 
         /// Constructor
-        NarrowPhaseInfoBatch(MemoryAllocator& allocator);
+        NarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
 
         /// Destructor
         virtual ~NarrowPhaseInfoBatch();
@@ -94,7 +103,7 @@ struct NarrowPhaseInfoBatch {
         uint getNbObjects() const;
 
         /// Add shapes to be tested during narrow-phase collision detection into the batch
-        void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
+        void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1,
                                 CollisionShape* shape2, const Transform& shape1Transform,
                                 const Transform& shape2Transform, MemoryAllocator& shapeAllocator);
 
@@ -114,7 +123,7 @@ struct NarrowPhaseInfoBatch {
 
 /// Return the number of objects in the batch
 inline uint NarrowPhaseInfoBatch::getNbObjects() const {
-    return overlappingPairs.size();
+    return overlappingPairIds.size();
 }
 
 }

src/collision/narrowphase/NarrowPhaseInput.cpp

@@ -30,36 +30,37 @@
 using namespace reactphysics3d;
 
 /// Constructor
-NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator)
+NarrowPhaseInput::NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs)
-    :mSphereVsSphereBatch(allocator), mSphereVsCapsuleBatch(allocator), mCapsuleVsCapsuleBatch(allocator),
+    :mSphereVsSphereBatch(allocator, overlappingPairs), mSphereVsCapsuleBatch(allocator, overlappingPairs),
-     mSphereVsConvexPolyhedronBatch(allocator), mCapsuleVsConvexPolyhedronBatch(allocator),
+     mCapsuleVsCapsuleBatch(allocator, overlappingPairs), mSphereVsConvexPolyhedronBatch(allocator, overlappingPairs),
-     mConvexPolyhedronVsConvexPolyhedronBatch(allocator) {
+     mCapsuleVsConvexPolyhedronBatch(allocator, overlappingPairs),
+     mConvexPolyhedronVsConvexPolyhedronBatch(allocator, overlappingPairs) {
 
 }
 
 // Add shapes to be tested during narrow-phase collision detection into the batch
-void NarrowPhaseInput::addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
+void NarrowPhaseInput::addNarrowPhaseTest(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2,
                                           const Transform& shape1Transform, const Transform& shape2Transform,
                                           NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, MemoryAllocator& shapeAllocator) {
 
     switch (narrowPhaseAlgorithmType) {
         case NarrowPhaseAlgorithmType::SphereVsSphere:
-            mSphereVsSphereBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform);
+            mSphereVsSphereBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform);
             break;
         case NarrowPhaseAlgorithmType::SphereVsCapsule:
-            mSphereVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform);
+            mSphereVsCapsuleBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform);
             break;
         case NarrowPhaseAlgorithmType::CapsuleVsCapsule:
-            mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform);
+            mCapsuleVsCapsuleBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform);
             break;
         case NarrowPhaseAlgorithmType::SphereVsConvexPolyhedron:
-            mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
+            mSphereVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
             break;
         case NarrowPhaseAlgorithmType::CapsuleVsConvexPolyhedron:
-            mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
+            mCapsuleVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
             break;
         case NarrowPhaseAlgorithmType::ConvexPolyhedronVsConvexPolyhedron:
-            mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pair, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
+            mConvexPolyhedronVsConvexPolyhedronBatch.addNarrowPhaseInfo(pairId, proxyShape1, proxyShape2, shape1, shape2, shape1Transform, shape2Transform, shapeAllocator);
             break;
         case NarrowPhaseAlgorithmType::None:
             // Must never happen

src/collision/narrowphase/NarrowPhaseInput.h

@@ -64,10 +64,10 @@ class NarrowPhaseInput {
     public:
 
         /// Constructor
-        NarrowPhaseInput(MemoryAllocator& allocator);
+        NarrowPhaseInput(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
 
         /// Add shapes to be tested during narrow-phase collision detection into the batch
-        void addNarrowPhaseTest(OverlappingPair* pair, CollisionShape* shape1,
+        void addNarrowPhaseTest(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1,
                         CollisionShape* shape2, const Transform& shape1Transform,
                         const Transform& shape2Transform, NarrowPhaseAlgorithmType narrowPhaseAlgorithmType,
                         MemoryAllocator& shapeAllocator);

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -29,7 +29,7 @@
 #include "collision/PolyhedronMesh.h"
 #include "collision/shapes/CapsuleShape.h"
 #include "collision/shapes/SphereShape.h"
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
 #include "collision/narrowphase/NarrowPhaseInfoBatch.h"
 #include "collision/shapes/TriangleShape.h"
 #include "configuration.h"

src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.cpp

@@ -31,14 +31,15 @@
 using namespace reactphysics3d;
 
 // Constructor
-SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator)
+SphereVsCapsuleNarrowPhaseInfoBatch::SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator,
-      : NarrowPhaseInfoBatch(allocator), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator),
+                                                                         OverlappingPairs& overlappingPairs)
+      : NarrowPhaseInfoBatch(allocator, overlappingPairs), isSpheresShape1(allocator), sphereRadiuses(allocator), capsuleRadiuses(allocator),
         capsuleHeights(allocator) {
 
 }
 
 // Add shapes to be tested during narrow-phase collision detection into the batch
-void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
+void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2,
                                                             const Transform& shape1Transform, const Transform& shape2Transform) {
 
     bool isSphereShape1 = shape1->getType() == CollisionShapeType::SPHERE;
@@ -55,19 +56,23 @@ void SphereVsCapsuleNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pa
     capsuleHeights.add(capsuleShape->getHeight());
     shape1ToWorldTransforms.add(shape1Transform);
     shape2ToWorldTransforms.add(shape2Transform);
-    overlappingPairs.add(pair);
+    overlappingPairIds.add(pairId);
+    proxyShapeEntities1.add(proxyShape1);
+    proxyShapeEntities2.add(proxyShape2);
     contactPoints.add(List<ContactPointInfo*>(mMemoryAllocator));
     isColliding.add(false);
 
     // Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
-    LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
+    LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId());
     lastFrameCollisionInfos.add(lastFrameInfo);
 }
 
 // Initialize the containers using cached capacity
 void SphereVsCapsuleNarrowPhaseInfoBatch::reserveMemory() {
 
-    overlappingPairs.reserve(mCachedCapacity);
+    overlappingPairIds.reserve(mCachedCapacity);
+    proxyShapeEntities1.reserve(mCachedCapacity);
+    proxyShapeEntities2.reserve(mCachedCapacity);
     shape1ToWorldTransforms.reserve(mCachedCapacity);
     shape2ToWorldTransforms.reserve(mCachedCapacity);
     lastFrameCollisionInfos.reserve(mCachedCapacity);
@@ -88,9 +93,11 @@ void SphereVsCapsuleNarrowPhaseInfoBatch::clear() {
     // allocated in the next frame at a possibly different location in memory (remember that the
     // location of the allocated memory of a single frame allocator might change between two frames)
 
-    mCachedCapacity = overlappingPairs.size();
+    mCachedCapacity = overlappingPairIds.size();
 
-    overlappingPairs.clear(true);
+    overlappingPairIds.clear(true);
+    proxyShapeEntities1.clear(true);
+    proxyShapeEntities2.clear(true);
     shape1ToWorldTransforms.clear(true);
     shape2ToWorldTransforms.clear(true);
     lastFrameCollisionInfos.clear(true);

src/collision/narrowphase/SphereVsCapsuleNarrowPhaseInfoBatch.h

@@ -55,13 +55,13 @@ struct SphereVsCapsuleNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch {
         List<decimal> capsuleHeights;
 
         /// Constructor
-        SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator);
+        SphereVsCapsuleNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
 
         /// Destructor
         virtual ~SphereVsCapsuleNarrowPhaseInfoBatch() = default;
 
         /// Add shapes to be tested during narrow-phase collision detection into the batch
-        virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
+        virtual void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1,
                                         CollisionShape* shape2, const Transform& shape1Transform,
                                         const Transform& shape2Transform);
 

src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.cpp

@@ -30,13 +30,13 @@
 using namespace reactphysics3d;
 
 // Constructor
-SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator)
+SphereVsSphereNarrowPhaseInfoBatch::SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs)
-      : NarrowPhaseInfoBatch(allocator), sphere1Radiuses(allocator), sphere2Radiuses(allocator) {
+      : NarrowPhaseInfoBatch(allocator, overlappingPairs), sphere1Radiuses(allocator), sphere2Radiuses(allocator) {
 
 }
 
 // Add shapes to be tested during narrow-phase collision detection into the batch
-void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2,
+void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1, CollisionShape* shape2,
                                                             const Transform& shape1Transform, const Transform& shape2Transform) {
 
     assert(shape1->getType() == CollisionShapeType::SPHERE);
@@ -47,21 +47,25 @@ void SphereVsSphereNarrowPhaseInfoBatch::addNarrowPhaseInfo(OverlappingPair* pai
 
     sphere1Radiuses.add(sphere1->getRadius());
     sphere2Radiuses.add(sphere2->getRadius());
+    proxyShapeEntities1.add(proxyShape1);
+    proxyShapeEntities2.add(proxyShape2);
     shape1ToWorldTransforms.add(shape1Transform);
     shape2ToWorldTransforms.add(shape2Transform);
-    overlappingPairs.add(pair);
+    overlappingPairIds.add(pairId);
     contactPoints.add(List<ContactPointInfo*>(mMemoryAllocator));
     isColliding.add(false);
 
     // Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
-    LastFrameCollisionInfo* lastFrameInfo = pair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
+    LastFrameCollisionInfo* lastFrameInfo = mOverlappingPairs.addLastFrameInfoIfNecessary(pairId, shape1->getId(), shape2->getId());
     lastFrameCollisionInfos.add(lastFrameInfo);
 }
 
 // Initialize the containers using cached capacity
 void SphereVsSphereNarrowPhaseInfoBatch::reserveMemory() {
 
-    overlappingPairs.reserve(mCachedCapacity);
+    overlappingPairIds.reserve(mCachedCapacity);
+    proxyShapeEntities1.reserve(mCachedCapacity);
+    proxyShapeEntities2.reserve(mCachedCapacity);
     shape1ToWorldTransforms.reserve(mCachedCapacity);
     shape2ToWorldTransforms.reserve(mCachedCapacity);
     lastFrameCollisionInfos.reserve(mCachedCapacity);
@@ -80,9 +84,11 @@ void SphereVsSphereNarrowPhaseInfoBatch::clear() {
     // allocated in the next frame at a possibly different location in memory (remember that the
     // location of the allocated memory of a single frame allocator might change between two frames)
 
-    mCachedCapacity = overlappingPairs.size();
+    mCachedCapacity = overlappingPairIds.size();
 
-    overlappingPairs.clear(true);
+    overlappingPairIds.clear(true);
+    proxyShapeEntities1.clear(true);
+    proxyShapeEntities2.clear(true);
     shape1ToWorldTransforms.clear(true);
     shape2ToWorldTransforms.clear(true);
     lastFrameCollisionInfos.clear(true);

src/collision/narrowphase/SphereVsSphereNarrowPhaseInfoBatch.h

@@ -49,13 +49,13 @@ struct SphereVsSphereNarrowPhaseInfoBatch : public NarrowPhaseInfoBatch {
         List<decimal> sphere2Radiuses;
 
         /// Constructor
-        SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator);
+        SphereVsSphereNarrowPhaseInfoBatch(MemoryAllocator& allocator, OverlappingPairs& overlappingPairs);
 
         /// Destructor
         virtual ~SphereVsSphereNarrowPhaseInfoBatch() override = default;
 
         /// Add shapes to be tested during narrow-phase collision detection into the batch
-        virtual void addNarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
+        virtual void addNarrowPhaseInfo(uint64 pairId, Entity proxyShape1, Entity proxyShape2, CollisionShape* shape1,
                                         CollisionShape* shape2, const Transform& shape1Transform,
                                         const Transform& shape2Transform);
 

src/collision/shapes/CollisionShape.h

@@ -29,6 +29,7 @@
 // Libraries
 #include <cassert>
 #include "configuration.h"
+#include "utils/Profiler.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {

src/collision/shapes/ConcaveMeshShape.cpp

@@ -120,6 +120,8 @@ void ConcaveMeshShape::computeOverlappingTriangles(const AABB& localAABB, List<V
                                                    List<Vector3>& triangleVerticesNormals, List<uint>& shapeIds,
                                                    MemoryAllocator& allocator) const {
 
+    RP3D_PROFILE("ConcaveMeshShape::computeOverlappingTriangles()", mProfiler);
+
     // Compute the nodes of the internal AABB tree that are overlapping with the AABB
     List<int> overlappingNodes(allocator);
     mDynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, overlappingNodes);
@@ -181,6 +183,8 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
 // Compute the shape Id for a given triangle of the mesh
 uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const {
 
+    RP3D_PROFILE("ConcaveMeshShape::computeTriangleShapeId()", mProfiler);
+
     uint shapeId = 0;
 
     uint i=0;

src/collision/shapes/HeightFieldShape.cpp

@@ -95,6 +95,8 @@ void HeightFieldShape::computeOverlappingTriangles(const AABB& localAABB, List<V
                                                    List<Vector3>& triangleVerticesNormals, List<uint>& shapeIds,
                                                    MemoryAllocator& allocator) const {
 
+    RP3D_PROFILE("HeightFieldShape::computeOverlappingTriangles()", mProfiler);
+
    // Compute the non-scaled AABB
    Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z);
    AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);

src/collision/shapes/SphereShape.cpp

@@ -49,6 +49,8 @@ SphereShape::SphereShape(decimal radius)
  */
 void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const {
 
+    RP3D_PROFILE("SphereShape::computeAABB()", mProfiler);
+
     // Get the local extents in x,y and z direction
     Vector3 extents(mMargin, mMargin, mMargin);
 

src/collision/shapes/TriangleShape.cpp

@@ -221,6 +221,8 @@ Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3&
  */
 void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const {
 
+    RP3D_PROFILE("TriangleShape::computeAABB()", mProfiler);
+
     const Vector3 worldPoint1 = transform * mPoints[0];
     const Vector3 worldPoint2 = transform * mPoints[1];
     const Vector3 worldPoint3 = transform * mPoints[2];

src/configuration.h

@@ -60,6 +60,8 @@ using int16 = std::int16_t;
 using uint16 = std::uint16_t;
 using int32 = std::int32_t;
 using uint32 = std::uint32_t;
+using int64 = std::int64_t;
+using uint64 = std::uint64_t;
 
 // TODO : Delete this
 struct Entity;

src/containers/List.h

@@ -320,8 +320,7 @@ class List {
            return removeAt(it.mCurrentIndex);
         }
 
-        /// Remove an element from the list at a given index and return an
+        /// Remove an element from the list at a given index (all the following items will be moved)
-        /// iterator pointing to the element after the removed one (or end() if none)
         Iterator removeAt(uint index) {
 
           assert(index >= 0 && index < mSize);
@@ -343,6 +342,27 @@ class List {
           return Iterator(mBuffer, index, mSize);
         }
 
+        /// Remove an element from the list at a given index (the deleted item will be replaced by the last one of the list)
+        void removeAtAndReplaceWithLast(uint index) {
+
+          assert(index >= 0 && index < mSize);
+
+          // Call the destructor on the item to remove
+          (static_cast<T*>(mBuffer)[index]).~T();
+
+          // If the item to remove is not the last one
+          if (index < mSize - 1) {
+
+              // Copy the last item of the array to the location of the deleted item
+              new (static_cast<char*>(mBuffer) + index * sizeof(T)) T(static_cast<T*>(mBuffer)[mSize - 1]);
+
+              // Call the destructor of the last item of the array
+              (static_cast<T*>(mBuffer)[mSize - 1]).~T();
+          }
+
+          mSize--;
+        }
+
         /// Append another list at the end of the current one
         void addRange(const List<T>& list) {
 

src/engine/Entity.h

@@ -136,7 +136,7 @@ namespace std {
 
     size_t operator()(const reactphysics3d::Entity& entity) const {
 
-        return entity.id;
+        return std::hash<reactphysics3d::uint32>{}(entity.id);
     }
   };
 }

src/engine/OverlappingPair.cpp

@@ -1,115 +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 <cassert>
-#include "OverlappingPair.h"
-#include "containers/containers_common.h"
-#include "collision/ContactPointInfo.h"
-
-using namespace reactphysics3d;
-
-// Constructor
-OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
-                                 MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator,
-                                 const WorldSettings& worldSettings)
-                : mPairID(computeID(shape1->getBroadPhaseId(), shape2->getBroadPhaseId())), mProxyShape1(shape1->getEntity()), mProxyShape2(shape2->getEntity()),
-                  mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator),
-                  mLastFrameCollisionInfos(mPersistentAllocator), mWorldSettings(worldSettings), mNeedToTestOverlap(false) {
-    
-}         
-
-// Destructor
-OverlappingPair::~OverlappingPair() {
-
-    RP3D_PROFILE("OverlappingPair::~OverlappingPair()", mProfiler);
-
-    // Remove all the remaining last frame collision info
-    for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) {
-
-        // Call the constructor
-        it->second->~LastFrameCollisionInfo();
-
-        // Release memory
-        mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
-    }
-}
-
-// Add a new last frame collision info if it does not exist for the given shapes already
-LastFrameCollisionInfo* OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) {
-
-    RP3D_PROFILE("OverlappingPair::addLastFrameInfoIfNecessary()", mProfiler);
-
-    // Try to get the corresponding last frame collision info
-    const ShapeIdPair shapeIdPair(shapeId1, shapeId2);
-    auto it = mLastFrameCollisionInfos.find(shapeIdPair);
-
-    // If there is no collision info for those two shapes already
-    if (it == mLastFrameCollisionInfos.end()) {
-
-        // Create a new collision info
-        LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo)))
-                                                LastFrameCollisionInfo();
-
-        // Add it into the map of collision infos
-        mLastFrameCollisionInfos.add(Pair<ShapeIdPair, LastFrameCollisionInfo*>(shapeIdPair, collisionInfo));
-
-        return collisionInfo;
-    }
-    else {
-
-       // The existing collision info is not obsolete
-       it->second->isObsolete = false;
-
-       return it->second;
-    }
-}
-
-// Delete all the obsolete last frame collision info
-void OverlappingPair::clearObsoleteLastFrameCollisionInfos() {
-
-    RP3D_PROFILE("OverlappingPair::clearObsoleteLastFrameCollisionInfos()", mProfiler);
-
-    // For each collision info
-    for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) {
-
-        // If the collision info is obsolete
-        if (it->second->isObsolete) {
-
-            // Delete it
-            it->second->~LastFrameCollisionInfo();
-            mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
-
-            it = mLastFrameCollisionInfos.remove(it);
-        }
-        else {  // If the collision info is not obsolete
-
-            // Do not delete it but mark it as obsolete
-            it->second->isObsolete = true;
-
-            ++it;
-        }
-    }
-}

src/engine/OverlappingPairs.cpp

@@ -0,0 +1,267 @@
+/********************************************************************************
+* 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 <cassert>
+#include "OverlappingPairs.h"
+#include "containers/containers_common.h"
+#include "collision/ContactPointInfo.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+OverlappingPairs::OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator, ProxyShapeComponents& proxyShapeComponents)
+                : mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator),
+                  mMapPairIdToPairIndex(persistentMemoryAllocator),
+                  mConvexPairIds(mPersistentAllocator), mConvexProxyShapes1(mPersistentAllocator), mConvexProxyShapes2(mPersistentAllocator),
+                  mConcavePairIds(mPersistentAllocator), mConcaveProxyShapes1(mPersistentAllocator), mConcaveProxyShapes2(mPersistentAllocator),
+                  mConvexLastFrameCollisionInfos(mPersistentAllocator), mConcaveLastFrameCollisionInfos(mPersistentAllocator),
+                  mConvexNeedToTestOverlap(mPersistentAllocator), mConcaveNeedToTestOverlap(mPersistentAllocator),
+                  mProxyShapeComponents(proxyShapeComponents) {
+    
+}         
+
+// Destructor
+OverlappingPairs::~OverlappingPairs() {
+
+}
+
+/// Add an overlapping pair
+uint64 OverlappingPairs::addPair(ProxyShape* shape1, ProxyShape* shape2) {
+
+    // 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<uint32>(shape1->getBroadPhaseId());
+    const uint32 shape2Id = static_cast<uint32>(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<uint>(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<ShapeIdPair, LastFrameCollisionInfo*>(mPersistentAllocator));
+
+        // Add a mapping to the index in the internal arrays
+        mMapPairIdToPairIndex.add(Pair<uint64, PairLocation>(pairId, PairLocation(true, nbConvexPairs)));
+    }
+    else {
+
+        const uint nbConcavePairs = static_cast<uint>(mConcavePairIds.size());
+
+        mConcavePairIds.add(pairId);
+        mConcaveProxyShapes1.add(shape1->getEntity());
+        mConcaveProxyShapes2.add(shape2->getEntity());
+        mConcaveNeedToTestOverlap.add(true);
+
+        // TODO: Make sure we use the correct allocator here
+        mConcaveLastFrameCollisionInfos.add(Map<ShapeIdPair, LastFrameCollisionInfo*>(mPersistentAllocator));
+
+        // Add a mapping to the index in the internal arrays
+        mMapPairIdToPairIndex.add(Pair<uint64, PairLocation>(pairId, PairLocation(false, nbConcavePairs)));
+    }
+
+    return pairId;
+}
+
+// Remove an overlapping pair
+uint64 OverlappingPairs::removePair(uint64 pairId) {
+
+    RP3D_PROFILE("OverlappingPairs::removePair()", mProfiler);
+
+    assert(mMapPairIdToPairIndex.containsKey(pairId));
+
+    const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId];
+
+    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<Map<ShapeIdPair, LastFrameCollisionInfo*>>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ?
+                    mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos;
+
+    // Remove all the remaining last frame collision info
+    for (auto it = lastFrameCollisionInfos[pairLocation.pairIndex].begin(); it != lastFrameCollisionInfos[pairLocation.pairIndex].end(); ++it) {
+
+        // Call the constructor
+        it->second->~LastFrameCollisionInfo();
+
+        // Release memory
+        mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo));
+    }
+
+    lastFrameCollisionInfos.removeAtAndReplaceWithLast(pairLocation.pairIndex);
+}
+
+// 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) {
+
+    auto it = mMapPairIdToPairIndex.find(pairId);
+    if (it != mMapPairIdToPairIndex.end()) {
+        pairLocation = it->second;
+        return true;
+    }
+
+    return false;
+}
+
+// Add a new last frame collision info if it does not exist for the given shapes already
+LastFrameCollisionInfo* OverlappingPairs::addLastFrameInfoIfNecessary(uint64 pairId, uint shapeId1, uint shapeId2) {
+
+    RP3D_PROFILE("OverlappingPairs::addLastFrameInfoIfNecessary()", mProfiler);
+
+    assert(mMapPairIdToPairIndex.containsKey(pairId));
+
+    PairLocation& pairLocation = mMapPairIdToPairIndex[pairId];
+    List<Map<ShapeIdPair, LastFrameCollisionInfo*>>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ?
+                    mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos;
+
+    // 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()) {
+
+        // 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, LastFrameCollisionInfo*>(shapeIdPair, collisionInfo));
+
+        return collisionInfo;
+    }
+    else {
+
+       // The existing collision info is not obsolete
+       it->second->isObsolete = false;
+
+       return it->second;
+    }
+}
+
+// Delete all the obsolete last frame collision info
+void OverlappingPairs::clearObsoleteLastFrameCollisionInfos() {
+
+    RP3D_PROFILE("OverlappingPairs::clearObsoleteLastFrameCollisionInfos()", mProfiler);
+
+    // For each convex vs convex overlapping pair
+    for (uint p=0; p < mConvexLastFrameCollisionInfos.size(); p++) {
+
+        // For each collision info
+        for (auto it = mConvexLastFrameCollisionInfos[p].begin(); it != mConvexLastFrameCollisionInfos[p].end(); ) {
+
+            // If the collision info is obsolete
+            if (it->second->isObsolete) {
+
+                // Delete it
+                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);
+            }
+            else {  // If the collision info is not obsolete
+
+                // Do not delete it but mark it as obsolete
+                it->second->isObsolete = true;
+
+                ++it;
+            }
+        }
+    }
+}

src/engine/OverlappingPairs.h

@@ -32,6 +32,7 @@
 #include "containers/Pair.h"
 #include "containers/containers_common.h"
 #include "utils/Profiler.h"
+#include "components/ProxyShapeComponents.h"
 #include <cstddef>
 
 /// ReactPhysics3D namespace
@@ -88,52 +89,97 @@ struct LastFrameCollisionInfo {
     }
 };
 
-// Class OverlappingPair
+// Class OverlappingPairs
 /**
- * This class represents a pair of two proxy collision shapes that are overlapping
+ * This class contains pairs of two proxy collision shapes that are overlapping
- * during the broad-phase collision detection. It is created when
+ * during the broad-phase collision detection. A pair is created when
  * the two proxy collision shapes start to overlap and is destroyed when they do not
- * overlap anymore. This class contains a contact manifold that
+ * overlap anymore. Each contains a contact manifold that
  * store all the contact points between the two bodies.
  */
-class OverlappingPair {
+class OverlappingPairs {
 
     public:
 
-        using OverlappingPairId = Pair<uint, uint>;
+        // TODO : Try to use a pairing function like pairNumbers() here
         using ShapeIdPair = Pair<uint, uint>;
 
     private:
 
+        /// Structure PairLocation
+        struct PairLocation {
+
+            /// 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) {
+
+            }
+        };
+
         // -------------------- Attributes -------------------- //
 
-        /// Pair ID
-        OverlappingPairId mPairID;
-
-        /// Entity of the first proxy-shape of the overlapping pair
-        Entity mProxyShape1;
-
-        /// Entity of the second proxy-shape of the overlapping pair
-        Entity mProxyShape2;
-
         /// Persistent memory allocator
         MemoryAllocator& mPersistentAllocator;
 
         /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
         MemoryAllocator& mTempMemoryAllocator;
 
+        /// Map a pair id to its local location
+        Map<uint64, PairLocation> mMapPairIdToPairIndex;
+
+        /// Ids of the convex vs convex pairs
+        // TODO : Check if we need this array
+        List<uint64> mConvexPairIds;
+
+        /// List of Entity of the first proxy-shape of the convex vs convex pairs
+        List<Entity> mConvexProxyShapes1;
+
+        /// List of Entity of the second proxy-shape of the convex vs convex pairs
+        List<Entity> mConvexProxyShapes2;
+
+        /// Ids of the convex vs concave pairs
+        // TODO : Check if we need this array
+        List<uint64> mConcavePairIds;
+
+        /// List of Entity of the first proxy-shape of the convex vs concave pairs
+        List<Entity> mConcaveProxyShapes1;
+
+        /// List of Entity of the second proxy-shape of the convex vs concave pairs
+        List<Entity> mConcaveProxyShapes2;
+
         /// 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.
-        Map<ShapeIdPair, LastFrameCollisionInfo*> mLastFrameCollisionInfos;
+        List<Map<ShapeIdPair, LastFrameCollisionInfo*>> mConvexLastFrameCollisionInfos;
 
-        /// World settings
+        /// Temporal coherence collision data for each overlapping collision shapes of this pair.
-        const WorldSettings& mWorldSettings;
+        /// 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<Map<ShapeIdPair, LastFrameCollisionInfo*>> mConcaveLastFrameCollisionInfos;
 
-        /// True if we need to test if the overlapping pair of shapes still overlap
+        /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap
-        bool mNeedToTestOverlap;
+        List<bool> mConvexNeedToTestOverlap;
+
+        /// True if we need to test if the convex vs convex overlapping pairs of shapes still overlap
+        List<bool> mConcaveNeedToTestOverlap;
+
+        /// Reference to the proxy-shapes components
+        ProxyShapeComponents& mProxyShapeComponents;
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -147,51 +193,45 @@ class OverlappingPair {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,  MemoryAllocator& persistentMemoryAllocator,
+        OverlappingPairs(MemoryAllocator& persistentMemoryAllocator, MemoryAllocator& temporaryMemoryAllocator,
-                        MemoryAllocator& temporaryMemoryAllocator, const WorldSettings& worldSettings);
+                         ProxyShapeComponents& proxyShapeComponents);
 
         /// Destructor
-        ~OverlappingPair();
+        ~OverlappingPairs();
 
         /// Deleted copy-constructor
-        OverlappingPair(const OverlappingPair& pair) = delete;
+        OverlappingPairs(const OverlappingPairs& pair) = delete;
 
         /// Deleted assignment operator
-        OverlappingPair& operator=(const OverlappingPair& pair) = delete;
+        OverlappingPairs& operator=(const OverlappingPairs& pair) = delete;
 
-        /// Return the Id of the pair
+        /// Add an overlapping pair
-        OverlappingPairId getId() const;
+        uint64 addPair(ProxyShape* shape1, ProxyShape* shape2);
+
+        /// Remove an overlapping pair
+        uint64 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 entity of the first proxy-shape
-        Entity getProxyShape1() const;
+        Entity getProxyShape1(uint64 pairId) const;
 
         /// Return the entity of the second proxy-shape
-        Entity getProxyShape2() const;
+        Entity getProxyShape2(uint64 pairId) const;
 
         /// Return the last frame collision info
-        LastFrameCollisionInfo* getLastFrameCollisionInfo(ShapeIdPair& shapeIds);
+        LastFrameCollisionInfo* getLastFrameCollisionInfo(uint64, ShapeIdPair& shapeIds);
 
         /// Return a reference to the temporary memory allocator
         MemoryAllocator& getTemporaryAllocator();
 
-        /// Return true if we need to test if the overlapping pair of shapes still overlap
-        bool needToTestOverlap() const;
-
-        /// Set to true if we need to test if the overlapping pair of shapes still overlap
-        void setNeedToTestOverlap(bool needToTestOverlap);
-
         /// Add a new last frame collision info if it does not exist for the given shapes already
-        LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2);
+        LastFrameCollisionInfo* addLastFrameInfoIfNecessary(uint64 pairId, uint shapeId1, uint shapeId2);
-
-        /// Return the last frame collision info for a given pair of shape ids
-        LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const;
 
         /// Delete all the obsolete last frame collision info
         void clearObsoleteLastFrameCollisionInfos();
 
-        /// Return the pair of bodies index
-        static OverlappingPairId computeID(int shape1BroadPhaseId, int shape2BroadPhaseId);
-
         /// Return the pair of bodies index of the pair
         static bodypair computeBodiesIndexPair(Entity body1Entity, Entity body2Entity);
 
@@ -205,27 +245,35 @@ class OverlappingPair {
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
+        friend class CollisionDetectionSystem;
 };
 
-// Return the Id of the pair
-inline OverlappingPair::OverlappingPairId OverlappingPair::getId() const {
-    return mPairID;
-}
-
 // Return the entity of the first proxy-shape
-inline Entity OverlappingPair::getProxyShape1() const {
+inline Entity OverlappingPairs::getProxyShape1(uint64 pairId) const {
-    return mProxyShape1;
+    assert(mMapPairIdToPairIndex.containsKey(pairId));
+    const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId];
+    const List<Entity> proxyShapes1 = pairLocation.isConvexVsConvex ? mConvexProxyShapes1 : mConcaveProxyShapes1;
+    return proxyShapes1[pairLocation.pairIndex];
 }
 
 // Return the entity of the second proxy-shape
-inline Entity OverlappingPair::getProxyShape2() const {
+inline Entity OverlappingPairs::getProxyShape2(uint64 pairId) const {
-    return mProxyShape2;
+    assert(mMapPairIdToPairIndex.containsKey(pairId));
+    const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId];
+    const List<Entity> proxyShapes2 = pairLocation.isConvexVsConvex ? mConvexProxyShapes2 : mConcaveProxyShapes2;
+    return proxyShapes2[pairLocation.pairIndex];
 }
 
 // Return the last frame collision info for a given shape id or nullptr if none is found
-inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeIdPair& shapeIds) {
+inline LastFrameCollisionInfo* OverlappingPairs::getLastFrameCollisionInfo(uint64 pairId, ShapeIdPair& shapeIds) {
-    Map<ShapeIdPair, LastFrameCollisionInfo*>::Iterator it = mLastFrameCollisionInfos.find(shapeIds);
+
-    if (it != mLastFrameCollisionInfos.end()) {
+    assert(mMapPairIdToPairIndex.containsKey(pairId));
+    const PairLocation& pairLocation = mMapPairIdToPairIndex[pairId];
+    const List<Map<ShapeIdPair, LastFrameCollisionInfo*>>& lastFrameCollisionInfos = pairLocation.isConvexVsConvex ?
+                                                            mConvexLastFrameCollisionInfos : mConcaveLastFrameCollisionInfos;
+
+    Map<ShapeIdPair, LastFrameCollisionInfo*>::Iterator it = lastFrameCollisionInfos[pairLocation.pairIndex].find(shapeIds);
+    if (it != lastFrameCollisionInfos[pairLocation.pairIndex].end()) {
         return it->second;
     }
 
@@ -233,19 +281,7 @@ inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(ShapeI
 }
 
 // Return the pair of bodies index
-inline OverlappingPair::OverlappingPairId OverlappingPair::computeID(int shape1BroadPhaseId, int shape2BroadPhaseId) {
+inline bodypair OverlappingPairs::computeBodiesIndexPair(Entity body1Entity, Entity body2Entity) {
-    assert(shape1BroadPhaseId >= 0 && shape2BroadPhaseId >= 0);
-
-    // Construct the pair of body index
-    OverlappingPairId pairID = shape1BroadPhaseId < shape2BroadPhaseId ?
-                             OverlappingPairId(shape1BroadPhaseId, shape2BroadPhaseId) :
-                             OverlappingPairId(shape2BroadPhaseId, shape1BroadPhaseId);
-    assert(pairID.first != pairID.second);
-    return pairID;
-}
-
-// Return the pair of bodies index
-inline bodypair OverlappingPair::computeBodiesIndexPair(Entity body1Entity, Entity body2Entity) {
 
     // Construct the pair of body index
     bodypair indexPair = body1Entity.id < body2Entity.id ?
@@ -256,29 +292,14 @@ inline bodypair OverlappingPair::computeBodiesIndexPair(Entity body1Entity, Enti
 }
 
 // Return a reference to the temporary memory allocator
-inline MemoryAllocator& OverlappingPair::getTemporaryAllocator() {
+inline MemoryAllocator& OverlappingPairs::getTemporaryAllocator() {
     return mTempMemoryAllocator;
 }
 
-// Return true if we need to test if the overlapping pair of shapes still overlap
-inline bool OverlappingPair::needToTestOverlap() const {
-   return mNeedToTestOverlap;
-}
-
-// Set to true if we need to test if the overlapping pair of shapes still overlap
-inline void OverlappingPair::setNeedToTestOverlap(bool needToTestOverlap)  {
-   mNeedToTestOverlap = needToTestOverlap;
-}
-
-// Return the last frame collision info for a given pair of shape ids
-inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const {
-    return mLastFrameCollisionInfos[ShapeIdPair(shapeId1, shapeId2)];
-}
-
 #ifdef IS_PROFILING_ACTIVE
 
 // Set the profiler
-inline void OverlappingPair::setProfiler(Profiler* profiler) {
+inline void OverlappingPairs::setProfiler(Profiler* profiler) {
     mProfiler = profiler;
 }
 

src/mathematics/mathematics_functions.cpp

@@ -404,4 +404,12 @@ bool reactphysics3d::isPrimeNumber(int number) {
     return number == 2;
 }
 
+// Return an unique integer from two integer numbers (pairing function)
+/// Here we assume that the two parameter numbers are sorted such that
+/// number1 = max(number1, number2)
+/// http://szudzik.com/ElegantPairing.pdf
+uint64 reactphysics3d::pairNumbers(uint32 number1, uint32 number2) {
+    assert(number1 == std::max(number1, number2));
+    return number1 * number1 + number1 + number2;
+}
 

src/mathematics/mathematics_functions.h

@@ -129,6 +129,12 @@ decimal computePointToPlaneDistance(const Vector3& point, const Vector3& planeNo
 /// Return true if the given number is prime
 bool isPrimeNumber(int number);
 
+/// Return an unique integer from two integer numbers (pairing function)
+/// Here we assume that the two parameter numbers are sorted such that
+/// number1 = max(number1, number2)
+/// http://szudzik.com/ElegantPairing.pdf
+uint64 pairNumbers(uint32 number1, uint32 number2);
+
 }
 
 

src/systems/CollisionDetectionSystem.cpp

@@ -55,10 +55,10 @@ CollisionDetectionSystem::CollisionDetectionSystem(CollisionWorld* world, ProxyS
                                        RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager)
                    : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents),
                      mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
-                     mOverlappingPairs(mMemoryManager.getPoolAllocator()),
+                     mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mProxyShapesComponents),
                      mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, rigidBodyComponents),
                      mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()),
-                     mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()),
+                     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()),
                      mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
@@ -74,6 +74,7 @@ CollisionDetectionSystem::CollisionDetectionSystem(CollisionWorld* world, ProxyS
 
 	mProfiler = nullptr;
     mCollisionDispatch.setProfiler(mProfiler);
+    mOverlappingPairs.setProfiler(mProfiler);
 
 #endif
 
@@ -119,9 +120,14 @@ void CollisionDetectionSystem::resetNeedToTestOverlap() {
 
     RP3D_PROFILE("CollisionDetectionSystem::resetNeedToTestOverlap()", mProfiler);
 
-    // For each possible collision pair of bodies
+    // For each possible convex vs convex collision pair of bodies
-    for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+    for (uint i=0; i < mOverlappingPairs.mConvexNeedToTestOverlap.size(); i++) {
-       it->second->setNeedToTestOverlap(true);
+       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;
     }
 }
 
@@ -130,25 +136,29 @@ void CollisionDetectionSystem::removeNonOverlappingPairs() {
 
     RP3D_PROFILE("CollisionDetectionSystem::removeNonOverlappingPairs()", mProfiler);
 
-    // For each possible collision pair of bodies
+    // For each possible convex vs convex pair of bodies
-    for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+    for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) {
-
-        OverlappingPair* pair = it->second;
 
         // Check if we need to test overlap. If so, test if the two shapes are still overlapping.
         // Otherwise, we destroy the overlapping pair
-        if (pair->needToTestOverlap() &&
+        if (mOverlappingPairs.mConvexNeedToTestOverlap[i] &&
-            !mBroadPhaseSystem.testOverlappingShapes(pair->getProxyShape1(), pair->getProxyShape2())) {
+            !mBroadPhaseSystem.testOverlappingShapes(mOverlappingPairs.mConvexProxyShapes1[i], mOverlappingPairs.mConvexProxyShapes2[i])) {
 
-            // Destroy the overlapping pair
+            mOverlappingPairs.removePair(mOverlappingPairs.mConvexPairIds[i]);
-            pair->~OverlappingPair();
+            i--;
-
-            mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
-            it = mOverlappingPairs.remove(it);
-            continue;
         }
-        else {
+    }
-            ++it;
+
+    // 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--;
         }
     }
 }
@@ -181,11 +191,11 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int, int>>
             if (body1Entity != body2Entity) {
 
                 // Compute the overlapping pair ID
-                Pair<uint, uint> pairID = OverlappingPair::computeID(nodePair.first, nodePair.second);
+                const uint64 pairId = pairNumbers(std::max(nodePair.first, nodePair.second), std::min(nodePair.first, nodePair.second));
 
                 // Check if the overlapping pair already exists
-                auto itPair = mOverlappingPairs.find(pairID);
+                OverlappingPairs::PairLocation pairLocation;
-                if (itPair == mOverlappingPairs.end()) {
+                if (!mOverlappingPairs.findPair(pairId, pairLocation)) {
 
                     unsigned short shape1CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity);
                     unsigned short shape2CollideWithMaskBits = mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity);
@@ -200,26 +210,21 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int, int>>
                         ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(proxyShape1Entity);
                         ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(proxyShape2Entity);
 
-                        // Create the overlapping pair and add it into the set of overlapping pairs
+                        // Check that at least one collision shape is convex
-                        OverlappingPair* newPair = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool, sizeof(OverlappingPair)))
+                        if (shape1->getCollisionShape()->isConvex() || shape2->getCollisionShape()->isConvex()) {
-                                                  OverlappingPair(shape1, shape2, mMemoryManager.getPoolAllocator(),
-                                                                  mMemoryManager.getSingleFrameAllocator(), mWorld->mConfig);
-
-                        assert(newPair != nullptr);
 
                             // Add the new overlapping pair
-                        mOverlappingPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(pairID, newPair));
+                            mOverlappingPairs.addPair(shape1, shape2);
-
+                        }
-#ifdef IS_PROFILING_ACTIVE
-                        newPair->setProfiler(mProfiler);
-#endif
-
                     }
                 }
                 else {
 
                     // We do not need to test the pair for overlap because it has just been reported that they still overlap
-                    itPair->second->setNeedToTestOverlap(false);
+                    List<bool>& pairsNeedToTestOverlap = pairLocation.isConvexVsConvex ? mOverlappingPairs.mConvexNeedToTestOverlap :
+                                                                                         mOverlappingPairs.mConcaveNeedToTestOverlap;
+
+                    pairsNeedToTestOverlap[pairLocation.pairIndex] = false;
                 }
             }
         }
@@ -227,23 +232,21 @@ void CollisionDetectionSystem::updateOverlappingPairs(const List<Pair<int, int>>
 }
 
 // Compute the middle-phase collision detection
-void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) {
+void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput) {
 
     RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler);
 
     // Reserve memory for the narrow-phase input using cached capacity from previous frame
     narrowPhaseInput.reserveMemory();
 
-    // For each possible collision pair of bodies
-    for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
-
-        OverlappingPair* pair = it->second;
-
     // Remove the obsolete last frame collision infos and mark all the others as obsolete
-        pair->clearObsoleteLastFrameCollisionInfos();
+    overlappingPairs.clearObsoleteLastFrameCollisionInfos();
 
-        const Entity proxyShape1Entity = pair->getProxyShape1();
+    // For each possible convex vs convex pair of bodies
-        const Entity proxyShape2Entity = pair->getProxyShape2();
+    for (uint i=0; i < overlappingPairs.mConvexPairIds.size(); i++) {
+
+        const Entity proxyShape1Entity = overlappingPairs.mConvexProxyShapes1[i];
+        const Entity proxyShape2Entity = overlappingPairs.mConvexProxyShapes2[i];
 
         assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
         assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
@@ -258,64 +261,185 @@ void CollisionDetectionSystem::computeMiddlePhase(OverlappingPairMap& overlappin
                                         mWorld->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 = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1;
         bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2;
         if (!isBody1Active && !isBody2Active) continue;
 
         // Check if the bodies are in the set of bodies that cannot collide between each other
-        bodypair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1Entity, body2Entity);
+        // 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) continue;
 
         CollisionShape* collisionShape1 = mProxyShapesComponents.getCollisionShape(proxyShape1Entity);
         CollisionShape* collisionShape2 = mProxyShapesComponents.getCollisionShape(proxyShape2Entity);
 
-        const bool isShape1Convex = collisionShape1->isConvex();
-        const bool isShape2Convex = collisionShape2->isConvex();
-
-        // If both shapes are convex
-        if (isShape1Convex && isShape2Convex) {
-
         // Select the narrow phase algorithm to use according to the two collision shapes
         NarrowPhaseAlgorithmType algorithmType = mCollisionDispatch.selectNarrowPhaseAlgorithm(collisionShape1->getType(),
                                                                                                collisionShape2->getType());
 
         // No middle-phase is necessary, simply create a narrow phase info
         // for the narrow-phase collision detection
-            narrowPhaseInput.addNarrowPhaseTest(pair, collisionShape1, collisionShape2,
+        narrowPhaseInput.addNarrowPhaseTest(overlappingPairs.mConvexPairIds[i], proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2,
                                                   mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity),
                                                   mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity),
                                                   algorithmType, mMemoryManager.getSingleFrameAllocator());
 
     }
-        // Concave vs Convex algorithm
-        else if ((!isShape1Convex && isShape2Convex) || (!isShape2Convex && isShape1Convex)) {
 
-            computeConvexVsConcaveMiddlePhase(pair, mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput);
+    // For each possible convex vs concave pair of bodies
-        }
+    for (uint i=0; i < overlappingPairs.mConcavePairIds.size(); i++) {
-        // Concave vs Concave shape
+
-        else {
+        const Entity proxyShape1Entity = overlappingPairs.mConcaveProxyShapes1[i];
-            // Not handled
+        const Entity proxyShape2Entity = overlappingPairs.mConcaveProxyShapes2[i];
-            continue;
+
-        }
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
+
+        const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity);
+        const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity);
+
+        const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) &&
+                                        mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC;
+        const bool isStaticRigidBody2 = mWorld->mRigidBodyComponents.hasComponent(body2Entity) &&
+                                        mWorld->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 = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1;
+        bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2;
+        if (!isBody1Active && !isBody2Active) continue;
+
+        // 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) continue;
+
+        computeConvexVsConcaveMiddlePhase(overlappingPairs.mConcavePairIds[i], proxyShape1Entity, proxyShape2Entity,
+                                          mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput);
     }
 }
 
+// Compute the middle-phase collision detection
+void CollisionDetectionSystem::computeMiddlePhaseCollisionSnapshot(List<uint64>& convexPairs, List<uint64>& concavePairs, NarrowPhaseInput& narrowPhaseInput) {
+
+    RP3D_PROFILE("CollisionDetectionSystem::computeMiddlePhase()", mProfiler);
+
+    // Reserve memory for the narrow-phase input using cached capacity from previous frame
+    narrowPhaseInput.reserveMemory();
+
+    // Remove the obsolete last frame collision infos and mark all the others as obsolete
+    mOverlappingPairs.clearObsoleteLastFrameCollisionInfos();
+
+    // For each possible convex vs convex pair of bodies
+    for (uint 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 Entity proxyShape1Entity = mOverlappingPairs.mConvexProxyShapes1[pairIndex];
+        const Entity proxyShape2Entity = mOverlappingPairs.mConvexProxyShapes2[pairIndex];
+
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
+
+        const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity);
+        const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity);
+
+        const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) &&
+                                        mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC;
+        const bool isStaticRigidBody2 = mWorld->mRigidBodyComponents.hasComponent(body2Entity) &&
+                                        mWorld->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 = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1;
+        bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2;
+        if (!isBody1Active && !isBody2Active) continue;
+
+        // 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) continue;
+
+        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());
+
+        // No middle-phase is necessary, simply create a narrow phase info
+        // for the narrow-phase collision detection
+        narrowPhaseInput.addNarrowPhaseTest(pairId, proxyShape1Entity, proxyShape2Entity, collisionShape1, collisionShape2,
+                                                  mProxyShapesComponents.getLocalToWorldTransform(proxyShape1Entity),
+                                                  mProxyShapesComponents.getLocalToWorldTransform(proxyShape2Entity),
+                                                  algorithmType, mMemoryManager.getSingleFrameAllocator());
+
+    }
+
+    // For each possible convex vs concave pair of bodies
+    for (uint p=0; p < concavePairs.size(); p++) {
+
+        const uint64 pairId = concavePairs[p];
+
+        OverlappingPairs::PairLocation& pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[pairId];
+        assert(!pairLoc.isConvexVsConvex);
+        const uint pairIndex = pairLoc.pairIndex;
+
+        const Entity proxyShape1Entity = mOverlappingPairs.mConcaveProxyShapes1[pairIndex];
+        const Entity proxyShape2Entity = mOverlappingPairs.mConcaveProxyShapes2[pairIndex];
+
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
+
+        const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity);
+        const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity);
+
+        const bool isStaticRigidBody1 = mWorld->mRigidBodyComponents.hasComponent(body1Entity) &&
+                                        mWorld->mRigidBodyComponents.getBodyType(body1Entity) == BodyType::STATIC;
+        const bool isStaticRigidBody2 = mWorld->mRigidBodyComponents.hasComponent(body2Entity) &&
+                                        mWorld->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 = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) && !isStaticRigidBody1;
+        bool isBody2Active = !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity) && !isStaticRigidBody2;
+        if (!isBody1Active && !isBody2Active) continue;
+
+        // 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) continue;
+
+        computeConvexVsConcaveMiddlePhase(pairId, proxyShape1Entity, proxyShape2Entity,
+                                          mMemoryManager.getSingleFrameAllocator(), narrowPhaseInput);
+    }
+}
 // Compute the concave vs convex middle-phase algorithm for a given pair of bodies
-void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
+void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(uint64 pairId, Entity proxyShape1, Entity proxyShape2,
-                                                           NarrowPhaseInput& narrowPhaseInput) {
+                                                                 MemoryAllocator& allocator, NarrowPhaseInput& narrowPhaseInput) {
 
     RP3D_PROFILE("CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase()", mProfiler);
 
-    ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(pair->getProxyShape1());
+    ProxyShape* shape1 = mProxyShapesComponents.getProxyShape(proxyShape1);
-    ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(pair->getProxyShape2());
+    ProxyShape* shape2 = mProxyShapesComponents.getProxyShape(proxyShape2);
 
     ProxyShape* convexProxyShape;
     ProxyShape* concaveProxyShape;
     ConvexShape* convexShape;
     ConcaveShape* concaveShape;
 
+    const bool isShape1Convex = shape1->getCollisionShape()->isConvex();
+
     // Collision shape 1 is convex, collision shape 2 is concave
-    if (shape1->getCollisionShape()->isConvex()) {
+    if (isShape1Convex) {
         convexProxyShape = shape1;
         convexShape = static_cast<ConvexShape*>(shape1->getCollisionShape());
         concaveProxyShape = shape2;
@@ -366,15 +490,11 @@ void CollisionDetectionSystem::computeConvexVsConcaveMiddlePhase(OverlappingPair
 
     #endif
 
-        bool isShape1Convex = mProxyShapesComponents.getCollisionShape(pair->getProxyShape1())->isConvex();
-        ProxyShape* shape1 = isShape1Convex ? convexProxyShape : concaveProxyShape;
-        ProxyShape* shape2 = isShape1Convex ? concaveProxyShape : convexProxyShape;
-
         // Create a narrow phase info for the narrow-phase collision detection
-        narrowPhaseInput.addNarrowPhaseTest(pair, isShape1Convex ? convexShape : triangleShape,
+        narrowPhaseInput.addNarrowPhaseTest(pairId, proxyShape1, proxyShape2, isShape1Convex ? convexShape : triangleShape,
                                                 isShape1Convex ? triangleShape : convexShape,
-                                                mProxyShapesComponents.getLocalToWorldTransform(shape1->getEntity()),
+                                                mProxyShapesComponents.getLocalToWorldTransform(proxyShape1),
-                                                mProxyShapesComponents.getLocalToWorldTransform(shape2->getEntity()),
+                                                mProxyShapesComponents.getLocalToWorldTransform(proxyShape2),
                                                 algorithmType, allocator);
     }
 }
@@ -427,7 +547,7 @@ bool CollisionDetectionSystem::testNarrowPhaseCollision(NarrowPhaseInput& narrow
 // Process the potential contacts after narrow-phase collision detection
 void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo,
                                                      List<ContactPointInfo>& potentialContactPoints,
-                                                     Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+                                                     Map<uint64, uint>* mapPairIdToContactPairIndex,
                                                      List<ContactManifoldInfo>& potentialContactManifolds,
                                                      List<ContactPair>* contactPairs,
                                                      Map<Entity, List<uint>>& mapBodyToContactPairs) {
@@ -543,8 +663,8 @@ void CollisionDetectionSystem::computeSnapshotContactPairs(NarrowPhaseInfoBatch&
         if (narrowPhaseInfoBatch.isColliding[i]) {
 
             // Add the pair of bodies in contact into the list
-            overlapPairs.add(Pair<Entity, Entity>(mProxyShapesComponents.getBody(narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape1()),
+            overlapPairs.add(Pair<Entity, Entity>(mProxyShapesComponents.getBody(narrowPhaseInfoBatch.proxyShapeEntities1[i]),
-                                                  mProxyShapesComponents.getBody(narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape2())));
+                                                  mProxyShapesComponents.getBody(narrowPhaseInfoBatch.proxyShapeEntities2[i])));
         }
 
         narrowPhaseInfoBatch.resetContactPoints(i);
@@ -567,7 +687,7 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn
 
         List<ContactPointInfo> potentialContactPoints(allocator);
         List<ContactManifoldInfo> potentialContactManifolds(allocator);
-        Map<OverlappingPair::OverlappingPairId, uint> mapPairIdToContactPairIndex(allocator);
+        Map<uint64, uint> mapPairIdToContactPairIndex(allocator);
         List<ContactPair> contactPairs(allocator);
         List<ContactManifold> contactManifolds(allocator);
         List<ContactPoint> contactPoints(allocator);
@@ -838,30 +958,40 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() {
 // Remove a body from the collision detection
 void CollisionDetectionSystem::removeProxyCollisionShape(ProxyShape* proxyShape) {
 
-    assert(proxyShape->getBroadPhaseId() != -1);
+    const int proxyShapeBroadPhaseId = proxyShape->getBroadPhaseId();
-    assert(mMapBroadPhaseIdToProxyShapeEntity.containsKey(proxyShape->getBroadPhaseId()));
 
-    // Remove all the overlapping pairs involving this proxy shape
+    assert(proxyShapeBroadPhaseId != -1);
-    for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
+    assert(mMapBroadPhaseIdToProxyShapeEntity.containsKey(proxyShapeBroadPhaseId));
 
-        OverlappingPair* pair = it->second;
+    // Remove all the convex vs convex overlapping pairs involving this proxy shape
+    for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) {
 
-        if (mProxyShapesComponents.getBroadPhaseId(pair->getProxyShape1()) == proxyShape->getBroadPhaseId() ||
+        if (mProxyShapesComponents.getBroadPhaseId(mOverlappingPairs.mConvexProxyShapes1[i]) == proxyShapeBroadPhaseId ||
-            mProxyShapesComponents.getBroadPhaseId(pair->getProxyShape2()) == proxyShape->getBroadPhaseId()) {
+            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
 
-            // Destroy the overlapping pair
+            // Remove the overlapping pair
-            pair->~OverlappingPair();
+            mOverlappingPairs.removePair(mOverlappingPairs.mConvexPairIds[i]);
-            mWorld->mMemoryManager.release(MemoryManager::AllocationType::Pool, pair, sizeof(OverlappingPair));
+            i--;
-            it = mOverlappingPairs.remove(it);
-        }
-        else {
-            ++it;
         }
     }
 
-    mMapBroadPhaseIdToProxyShapeEntity.remove(proxyShape->getBroadPhaseId());
+    // 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--;
+        }
+    }
+
+    mMapBroadPhaseIdToProxyShapeEntity.remove(proxyShapeBroadPhaseId);
 
     // Remove the body from the broad-phase
     mBroadPhaseSystem.removeProxyCollisionShape(proxyShape);
@@ -884,7 +1014,7 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback,
 // Convert the potential contact into actual contacts
 void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo,
                                                   List<ContactPointInfo>& potentialContactPoints,
-                                                  Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+                                                  Map<uint64, uint>* mapPairIdToContactPairIndex,
                                                   List<ContactManifoldInfo>& potentialContactManifolds,
                                                   List<ContactPair>* contactPairs,
                                                   Map<Entity, List<uint>>& mapBodyToContactPairs) {
@@ -918,7 +1048,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
             bool similarManifoldFound = false;
 
             // If there is already a contact pair for this overlapping pair
-            OverlappingPair::OverlappingPairId pairId = narrowPhaseInfoBatch.overlappingPairs[i]->getId();
+            const uint64 pairId = narrowPhaseInfoBatch.overlappingPairIds[i];
             auto it = mapPairIdToContactPairIndex->find(pairId);
             ContactPair* pairContact = nullptr;
             if (it != mapPairIdToContactPairIndex->end()) {
@@ -968,8 +1098,8 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                 // If the overlapping pair contact does not exists yet
                 if (pairContact == nullptr) {
 
-                    const Entity proxyShape1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape1();
+                    const Entity proxyShape1Entity = narrowPhaseInfoBatch.proxyShapeEntities1[i];
-                    const Entity proxyShape2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getProxyShape2();
+                    const Entity proxyShape2Entity = narrowPhaseInfoBatch.proxyShapeEntities2[i];
 
                     const Entity body1Entity = mProxyShapesComponents.getBody(proxyShape1Entity);
                     const Entity body2Entity = mProxyShapesComponents.getBody(proxyShape2Entity);
@@ -983,7 +1113,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                                                        newContactPairIndex, mMemoryManager.getPoolAllocator());
                     contactPairs->add(overlappingPairContact);
                     pairContact = &((*contactPairs)[newContactPairIndex]);
-                    mapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));
+                    mapPairIdToContactPairIndex->add(Pair<uint64, uint>(pairId, newContactPairIndex));
 
                     auto itbodyContactPairs = mapBodyToContactPairs.find(body1Entity);
                     if (itbodyContactPairs != mapBodyToContactPairs.end()) {
@@ -1075,7 +1205,11 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
             // If there are two many contact points in the manifold
             if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) {
 
-                Transform shape1LocalToWorldTransoform = mProxyShapesComponents.getLocalToWorldTransform(mOverlappingPairs[manifold.pairId]->getProxyShape1());
+                OverlappingPairs::PairLocation pairLoc = mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId];
+                Entity proxyShape1 = pairLoc.isConvexVsConvex ? mOverlappingPairs.mConvexProxyShapes1[pairLoc.pairIndex] :
+                                                                mOverlappingPairs.mConcaveProxyShapes1[pairLoc.pairIndex];
+
+                Transform shape1LocalToWorldTransoform = mProxyShapesComponents.getLocalToWorldTransform(proxyShape1);
 
                 // Reduce the number of contact points in the manifold
                 reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
@@ -1313,19 +1447,20 @@ void CollisionDetectionSystem::reportContacts(CollisionCallback& callback, List<
 // Return true if two bodies overlap (collide)
 bool CollisionDetectionSystem::testOverlap(CollisionBody* body1, CollisionBody* body2) {
 
-    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs);
 
     // Compute the broad-phase collision detection
     computeBroadPhase();
 
     // Filter the overlapping pairs to get only the ones with the selected body involved
-    OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+    List<uint64> convexPairs(mMemoryManager.getPoolAllocator());
-    filterOverlappingPairs(body1->getEntity(), body2->getEntity(), overlappingPairs);
+    List<uint64> concavePairs(mMemoryManager.getPoolAllocator());
+    filterOverlappingPairs(body1->getEntity(), body2->getEntity(), convexPairs, concavePairs);
 
-    if (overlappingPairs.size() > 0) {
+    if (convexPairs.size() > 0 || concavePairs.size() > 0) {
 
         // Compute the middle-phase collision detection
-        computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+        computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput);
 
         // Compute the narrow-phase collision detection
         return computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, nullptr);
@@ -1337,7 +1472,7 @@ bool CollisionDetectionSystem::testOverlap(CollisionBody* body1, CollisionBody*
 // Report all the bodies that overlap (collide) in the world
 void CollisionDetectionSystem::testOverlap(OverlapCallback& callback) {
 
-    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs);
 
     // Compute the broad-phase collision detection
     computeBroadPhase();
@@ -1352,19 +1487,20 @@ void CollisionDetectionSystem::testOverlap(OverlapCallback& callback) {
 // Report all the bodies that overlap (collide) with the body in parameter
 void CollisionDetectionSystem::testOverlap(CollisionBody* body, OverlapCallback& callback) {
 
-    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs);
 
     // Compute the broad-phase collision detection
     computeBroadPhase();
 
     // Filter the overlapping pairs to get only the ones with the selected body involved
-    OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+    List<uint64> convexPairs(mMemoryManager.getPoolAllocator());
-    filterOverlappingPairs(body->getEntity(), overlappingPairs);
+    List<uint64> concavePairs(mMemoryManager.getPoolAllocator());
+    filterOverlappingPairs(body->getEntity(), convexPairs, concavePairs);
 
-    if (overlappingPairs.size() > 0) {
+    if (convexPairs.size() > 0 || concavePairs.size() > 0) {
 
         // Compute the middle-phase collision detection
-        computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+        computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput);
 
         // Compute the narrow-phase collision detection
         computeNarrowPhaseOverlapSnapshot(narrowPhaseInput, &callback);
@@ -1374,19 +1510,20 @@ void CollisionDetectionSystem::testOverlap(CollisionBody* body, OverlapCallback&
 // Test collision and report contacts between two bodies.
 void CollisionDetectionSystem::testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback& callback) {
 
-    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs);
 
     // Compute the broad-phase collision detection
     computeBroadPhase();
 
     // Filter the overlapping pairs to get only the ones with the selected body involved
-    OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+    List<uint64> convexPairs(mMemoryManager.getPoolAllocator());
-    filterOverlappingPairs(body1->getEntity(), body2->getEntity(), overlappingPairs);
+    List<uint64> concavePairs(mMemoryManager.getPoolAllocator());
+    filterOverlappingPairs(body1->getEntity(), body2->getEntity(), convexPairs, concavePairs);
 
-    if (overlappingPairs.size() > 0) {
+    if (convexPairs.size() > 0 || concavePairs.size() > 0) {
 
         // Compute the middle-phase collision detection
-        computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+        computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput);
 
         // Compute the narrow-phase collision detection and report contacts
         computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback);
@@ -1396,19 +1533,20 @@ void CollisionDetectionSystem::testCollision(CollisionBody* body1, CollisionBody
 // Test collision and report all the contacts involving the body in parameter
 void CollisionDetectionSystem::testCollision(CollisionBody* body, CollisionCallback& callback) {
 
-    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs);
 
     // Compute the broad-phase collision detection
     computeBroadPhase();
 
     // Filter the overlapping pairs to get only the ones with the selected body involved
-    OverlappingPairMap overlappingPairs(mMemoryManager.getPoolAllocator());
+    List<uint64> convexPairs(mMemoryManager.getPoolAllocator());
-    filterOverlappingPairs(body->getEntity(), overlappingPairs);
+    List<uint64> concavePairs(mMemoryManager.getPoolAllocator());
+    filterOverlappingPairs(body->getEntity(), convexPairs, concavePairs);
 
-    if (overlappingPairs.size() > 0) {
+    if (convexPairs.size() > 0 || concavePairs.size() > 0) {
 
         // Compute the middle-phase collision detection
-        computeMiddlePhase(overlappingPairs, narrowPhaseInput);
+        computeMiddlePhaseCollisionSnapshot(convexPairs, concavePairs, narrowPhaseInput);
 
         // Compute the narrow-phase collision detection and report contacts
         computeNarrowPhaseCollisionSnapshot(narrowPhaseInput, callback);
@@ -1418,7 +1556,7 @@ void CollisionDetectionSystem::testCollision(CollisionBody* body, CollisionCallb
 // Test collision and report contacts between each colliding bodies in the world
 void CollisionDetectionSystem::testCollision(CollisionCallback& callback) {
 
-    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator());
+    NarrowPhaseInput narrowPhaseInput(mMemoryManager.getPoolAllocator(), mOverlappingPairs);
 
     // Compute the broad-phase collision detection
     computeBroadPhase();
@@ -1431,33 +1569,51 @@ void CollisionDetectionSystem::testCollision(CollisionCallback& callback) {
 }
 
 // Filter the overlapping pairs to keep only the pairs where a given body is involved
-void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const {
+void CollisionDetectionSystem::filterOverlappingPairs(Entity bodyEntity, List<uint64>& convexPairs, List<uint64>& concavePairs) const {
 
     // For each possible collision pair of bodies
-    for (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+    for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) {
 
-        OverlappingPair* pair = it->second;
+        if (mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes1[i]) == bodyEntity ||
+            mProxyShapesComponents.getBody(mOverlappingPairs.mConvexProxyShapes2[i]) == bodyEntity) {
 
-        if (mProxyShapesComponents.getBody(pair->getProxyShape1()) == bodyEntity ||
+            convexPairs.add(mOverlappingPairs.mConvexPairIds[i]);
-            mProxyShapesComponents.getBody(pair->getProxyShape2()) == bodyEntity) {
+        }
+    }
 
-            outFilteredPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(it->first, pair));
+    // 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]);
         }
     }
 }
 
 // Filter the overlapping pairs to keep only the pairs where two given bodies are involved
-void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const {
+void CollisionDetectionSystem::filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List<uint64>& convexPairs, List<uint64>& concavePairs) const {
+
+    // 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 (auto it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+    for (uint i=0; i < mOverlappingPairs.mConvexPairIds.size(); i++) {
 
-        OverlappingPair* pair = it->second;
+        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(pair->getProxyShape1()) == body1Entity && mProxyShapesComponents.getBody(pair->getProxyShape2()) == body2Entity) ||
+            convexPairs.add(mOverlappingPairs.mConvexPairIds[i]);
-            (mProxyShapesComponents.getBody(pair->getProxyShape1()) == body2Entity && mProxyShapesComponents.getBody(pair->getProxyShape2()) == body1Entity)) {
+        }
+    }
 
-            outFilteredPairs.add(Pair<Pair<uint, uint>, OverlappingPair*>(it->first, pair));
+    // 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]);
         }
     }
 }

src/systems/CollisionDetectionSystem.h

@@ -35,7 +35,8 @@
 #include "collision/ContactManifoldInfo.h"
 #include "collision/ContactManifold.h"
 #include "collision/ContactPair.h"
-#include "engine/OverlappingPair.h"
+#include "engine/OverlappingPairs.h"
+#include "engine/OverlappingPairs.h"
 #include "collision/narrowphase/NarrowPhaseInput.h"
 #include "collision/narrowphase/CollisionDispatch.h"
 #include "containers/Map.h"
@@ -89,7 +90,7 @@ class CollisionDetectionSystem {
         CollisionWorld* mWorld;
 
         /// Broad-phase overlapping pairs
-        OverlappingPairMap mOverlappingPairs;
+        OverlappingPairs mOverlappingPairs;
 
         /// Broad-phase system
         BroadPhaseSystem mBroadPhaseSystem;
@@ -122,18 +123,18 @@ class CollisionDetectionSystem {
         List<ContactPair>* mCurrentContactPairs;
 
         /// First map of overlapping pair id to the index of the corresponding pair contact
-        Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex1;
+        Map<uint64, uint> mMapPairIdToContactPairIndex1;
 
         /// Second map of overlapping pair id to the index of the corresponding pair contact
-        Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex2;
+        Map<uint64, uint> mMapPairIdToContactPairIndex2;
 
         /// Pointer to the map of overlappingPairId to the index of contact pair of the previous frame
         /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
-        Map<OverlappingPair::OverlappingPairId, uint>* mPreviousMapPairIdToContactPairIndex;
+        Map<uint64, uint>* mPreviousMapPairIdToContactPairIndex;
 
         /// Pointer to the map of overlappingPairId to the index of contact pair of the current frame
         /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
-        Map<OverlappingPair::OverlappingPairId, uint>* mCurrentMapPairIdToContactPairIndex;
+        Map<uint64, uint>* mCurrentMapPairIdToContactPairIndex;
 
         /// First list with the contact manifolds
         List<ContactManifold> mContactManifolds1;
@@ -175,7 +176,10 @@ class CollisionDetectionSystem {
         void computeBroadPhase();
 
         /// Compute the middle-phase collision detection
-        void computeMiddlePhase(OverlappingPairMap& overlappingPairs, NarrowPhaseInput& narrowPhaseInput);
+        void computeMiddlePhase(OverlappingPairs& overlappingPairs, NarrowPhaseInput& narrowPhaseInput);
+
+        // Compute the middle-phase collision detection
+        void computeMiddlePhaseCollisionSnapshot(List<uint64>& convexPairs, List<uint64>& concavePairs, NarrowPhaseInput& narrowPhaseInput);
 
         /// Compute the narrow-phase collision detection
         void computeNarrowPhase();
@@ -205,7 +209,7 @@ class CollisionDetectionSystem {
         bool testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseInput, bool reportContacts, bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& allocator);
 
         /// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
-        void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
+        void computeConvexVsConcaveMiddlePhase(uint64 pairId, Entity proxyShape1, Entity proxyShape2, MemoryAllocator& allocator,
                                                NarrowPhaseInput& narrowPhaseInput);
 
         /// Swap the previous and current contacts lists
@@ -214,13 +218,13 @@ class CollisionDetectionSystem {
         /// Convert the potential contact into actual contacts
         void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                       bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
-                                      Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+                                      Map<uint64, uint>* mapPairIdToContactPairIndex,
                                       List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs,
                                       Map<Entity, List<uint>>& mapBodyToContactPairs);
 
         /// Process the potential contacts after narrow-phase collision detection
         void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
-                                         Map<OverlappingPair::OverlappingPairId, uint>* mapPairIdToContactPairIndex,
+                                         Map<uint64, uint>* mapPairIdToContactPairIndex,
                                          List<ContactManifoldInfo> &potentialContactManifolds, List<ContactPair>* contactPairs,
                                          Map<Entity, List<uint>>& mapBodyToContactPairs);
 
@@ -256,10 +260,10 @@ class CollisionDetectionSystem {
         void processSmoothMeshContacts(OverlappingPair* pair);
 
         /// Filter the overlapping pairs to keep only the pairs where a given body is involved
-        void filterOverlappingPairs(Entity bodyEntity, OverlappingPairMap& outFilteredPairs) const;
+        void filterOverlappingPairs(Entity bodyEntity, List<uint64>& convexPairs, List<uint64>& concavePairs) const;
 
         /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
-        void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, OverlappingPairMap& outFilteredPairs) const;
+        void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List<uint64>& convexPairs, List<uint64>& concavePairs) const;
 
     public :
 
@@ -377,12 +381,12 @@ inline void CollisionDetectionSystem::addProxyCollisionShape(ProxyShape* proxySh
 
 // Add a pair of bodies that cannot collide with each other
 inline void CollisionDetectionSystem::addNoCollisionPair(Entity body1Entity, Entity body2Entity) {
-    mNoCollisionPairs.add(OverlappingPair::computeBodiesIndexPair(body1Entity, body2Entity));
+    mNoCollisionPairs.add(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity));
 }
 
 // Remove a pair of bodies that cannot collide with each other
 inline void CollisionDetectionSystem::removeNoCollisionPair(Entity body1Entity, Entity body2Entity) {
-    mNoCollisionPairs.remove(OverlappingPair::computeBodiesIndexPair(body1Entity, body2Entity));
+    mNoCollisionPairs.remove(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity));
 }
 
 // Ask for a collision shape to be tested again during broad-phase.
@@ -424,6 +428,7 @@ inline void CollisionDetectionSystem::setProfiler(Profiler* profiler) {
 	mProfiler = profiler;
     mBroadPhaseSystem.setProfiler(profiler);
     mCollisionDispatch.setProfiler(profiler);
+    mOverlappingPairs.setProfiler(profiler);
 }
 
 #endif

test/tests/containers/TestList.h

@@ -190,6 +190,22 @@ class TestList : public Test {
             it = list3.remove(5);
             rp3d_test((*it) == 6);
 
+            list3.clear();
+            list3.add(1);
+            list3.add(2);
+            list3.add(3);
+            list3.add(4);
+            list3.removeAtAndReplaceWithLast(3);
+            rp3d_test(list3.size() == 3);
+            rp3d_test(list3[2] == 3);
+            list3.removeAtAndReplaceWithLast(1);
+            rp3d_test(list3.size() == 2);
+            rp3d_test(list3[0] == 2);
+            rp3d_test(list3[1] == 3);
+            list3.removeAtAndReplaceWithLast(0);
+            rp3d_test(list3.size() == 1);
+            rp3d_test(list3[0] == 3);
+
             // ----- Test addRange() ----- //
 
             List<int> list4(mAllocator);