Working on optimizations in contacts creation

include/reactphysics3d/collision/ContactManifold.h

@@ -58,7 +58,7 @@ class ContactManifold {
         // -------------------- Constants -------------------- //
 
         /// Maximum number of contact points in a reduced contact manifold
-        const int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
+        static constexpr int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
 
         // -------------------- Attributes -------------------- //
 
@@ -78,7 +78,7 @@ class ContactManifold {
         Entity colliderEntity2;
 
         /// Number of contacts in the cache
-        int8 nbContactPoints;
+        uint8 nbContactPoints;
 
         /// First friction vector of the contact manifold
         Vector3 frictionVector1;
@@ -107,16 +107,7 @@ class ContactManifold {
 
         /// Constructor
         ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
-                        uint contactPointsIndex, int8 nbContactPoints);
+                        uint contactPointsIndex, uint8 nbContactPoints);
-
-        /// Destructor
-        ~ContactManifold();
-
-        /// Copy-constructor
-        ContactManifold(const ContactManifold& contactManifold) = default;
-
-        /// Assignment operator
-        ContactManifold& operator=(const ContactManifold& contactManifold) = default;
 
         // -------------------- Friendship -------------------- //
 

include/reactphysics3d/collision/ContactManifoldInfo.h

@@ -45,8 +45,12 @@ struct ContactManifoldInfo {
 
         // -------------------- Attributes -------------------- //
 
+        /// Number of potential contact points
+        uint8 nbPotentialContactPoints;
+
         /// Indices of the contact points in the mPotentialContactPoints array
-        List<uint> potentialContactPointsIndices;
+        uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
+
 
         /// Overlapping pair id
         uint64 pairId;
@@ -54,8 +58,7 @@ struct ContactManifoldInfo {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifoldInfo(uint64 pairId, MemoryAllocator& allocator)
+        ContactManifoldInfo(uint64 pairId) : nbPotentialContactPoints(0), pairId(pairId) {
-               : potentialContactPointsIndices(allocator, 4), pairId(pairId) {
 
         }
 

include/reactphysics3d/collision/ContactPair.h

@@ -47,8 +47,11 @@ struct ContactPair {
         /// Overlapping pair Id
         uint64 pairId;
 
+        /// Number of potential contact manifolds
+        uint8 nbPotentialContactManifolds;
+
         /// Indices of the potential contact manifolds
-        List<uint> potentialContactManifoldsIndices;
+        uint potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS];
 
         /// Entity of the first body of the contact
         Entity body1Entity;
@@ -90,13 +93,21 @@ struct ContactPair {
 
         /// Constructor
         ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity,
-                    Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator)
+                    Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger)
-            : pairId(pairId), potentialContactManifoldsIndices(allocator, 8), body1Entity(body1Entity), body2Entity(body2Entity),
+            : pairId(pairId), nbPotentialContactManifolds(0), body1Entity(body1Entity), body2Entity(body2Entity),
               collider1Entity(collider1Entity), collider2Entity(collider2Entity),
               isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
               contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) {
 
         }
+
+        // Remove a potential manifold at a given index in the array
+        void removePotentialManifoldAtIndex(uint index) {
+            assert(index < nbPotentialContactManifolds);
+
+            potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1];
+            nbPotentialContactManifolds--;
+        }
 };
 
 }

include/reactphysics3d/collision/ContactPointInfo.h

@@ -76,9 +76,6 @@ struct ContactPointInfo {
             assert(contactNormal.lengthSquare() > decimal(0.8));
             assert(penDepth > decimal(0.0));
         }
-
-        /// Destructor
-        ~ContactPointInfo() = default;
 };
 
 }

include/reactphysics3d/configuration.h

@@ -103,6 +103,15 @@ constexpr decimal PI_TIMES_2 = decimal(6.28318530);
 /// without triggering a large modification of the tree each frame which can be costly
 constexpr decimal DYNAMIC_TREE_FAT_AABB_INFLATE_PERCENTAGE = decimal(0.08);
 
+/// Maximum number of contact manifolds in an overlapping pair
+constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3;
+
+/// Maximum number of potential contact manifolds in an overlapping pair
+constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS;
+
+/// Maximum number of contact points in potential contact manifold
+constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 12;
+
 /// Current version of ReactPhysics3D
 const std::string RP3D_VERSION = std::string("0.8.0");
 

include/reactphysics3d/containers/List.h

@@ -294,6 +294,21 @@ class List {
             mSize++;
         }
 
+        /// Add an element into the list by constructing it directly into the list (in order to avoid a copy)
+        template<typename...Ts>
+        void emplace(Ts&&... args) {
+
+            // If we need to allocate more memory
+            if (mSize == mCapacity) {
+                reserve(mCapacity == 0 ? 1 : mCapacity * 2);
+            }
+
+            // Construct the element directly at its location in the array
+            new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(std::forward<Ts>(args)...);
+
+            mSize++;
+        }
+
         /// Add a given numbers of elements at the end of the list but do not init them
         void addWithoutInit(uint nbElements) {
 

include/reactphysics3d/engine/PhysicsWorld.h

@@ -116,9 +116,6 @@ class PhysicsWorld {
             /// might enter sleeping mode
             decimal defaultSleepAngularVelocity;
 
-            /// Maximum number of contact manifolds in an overlapping pair
-            uint nbMaxContactManifolds;
-
             /// This is used to test if two contact manifold are similar (same contact normal) in order to
             /// merge them. If the cosine of the angle between the normals of the two manifold are larger
             /// than the value bellow, the manifold are considered to be similar.
@@ -139,7 +136,6 @@ class PhysicsWorld {
                 defaultTimeBeforeSleep = 1.0f;
                 defaultSleepLinearVelocity = decimal(0.02);
                 defaultSleepAngularVelocity = decimal(3.0) * (PI_RP3D / decimal(180.0));
-                nbMaxContactManifolds = 3;
                 cosAngleSimilarContactManifold = decimal(0.95);
             }
 
@@ -163,7 +159,6 @@ class PhysicsWorld {
                 ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl;
                 ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl;
                 ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl;
-                ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl;
                 ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl;
 
                 return ss.str();

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -128,19 +128,9 @@ class CollisionDetectionSystem {
         /// List of lost contact pairs (contact pairs in contact in previous frame but not in the current one)
         List<ContactPair> mLostContactPairs;
 
-        /// First map of overlapping pair id to the index of the corresponding pair contact
-        Map<uint64, uint> mMapPairIdToContactPairIndex1;
-
-        /// Second map of overlapping pair id to the index of the corresponding pair contact
-        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<uint64, 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<uint64, uint>* mCurrentMapPairIdToContactPairIndex;
 
         /// First list with the contact manifolds
         List<ContactManifold> mContactManifolds1;
@@ -223,12 +213,11 @@ class CollisionDetectionSystem {
         /// Convert the potential contact into actual contacts
         void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                       bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
-                                      Map<uint64, uint>* mapPairIdToContactPairIndex,
+                                      List<ContactManifoldInfo>& potentialContactManifolds,
-                                      List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs);
+                                      Map<uint64, uint>& mapPairIdToContactPairIndex, List<ContactPair>* contactPairs);
 
         /// Process the potential contacts after narrow-phase collision detection
         void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
-                                         Map<uint64, uint>* mapPairIdToContactPairIndex,
                                          List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs);
 
         /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
@@ -238,6 +227,9 @@ class CollisionDetectionSystem {
         /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
         void createContacts();
 
+        /// Compute the map from contact pairs ids to contact pair for the next frame
+        void computeMapPreviousContactPairs();
+
         /// Compute the lost contact pairs (contact pairs in contact in the previous frame but not in the current one)
         void computeLostContactPairs();
 
@@ -277,6 +269,9 @@ class CollisionDetectionSystem {
         /// Filter the overlapping pairs to keep only the pairs where two given bodies are involved
         void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, List<uint64>& convexPairs, List<uint64>& concavePairs) const;
 
+        /// Remove an element in an array (and replace it by the last one in the array)
+        void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const;
+
     public :
 
         // -------------------- Methods -------------------- //

src/collision/ContactManifold.cpp

@@ -32,14 +32,9 @@ using namespace reactphysics3d;
 
 // Constructor
 ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity colliderEntity1, Entity colliderEntity2,
-                                 uint contactPointsIndex, int8 nbContactPoints)
+                                 uint contactPointsIndex, uint8 nbContactPoints)
                 :contactPointsIndex(contactPointsIndex), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
                  colliderEntity1(colliderEntity1), colliderEntity2(colliderEntity2), nbContactPoints(nbContactPoints), frictionImpulse1(0), frictionImpulse2(0),
                  frictionTwistImpulse(0), isAlreadyInIsland(false) {
 
 }
-
-// Destructor
-ContactManifold::~ContactManifold() {
-
-}

src/engine/PhysicsWorld.cpp

@@ -824,11 +824,11 @@ void PhysicsWorld::createIslands() {
                 if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex)
                     && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) {
 
-                    assert(pair.potentialContactManifoldsIndices.size() > 0);
+                    assert(pair.nbPotentialContactManifolds > 0);
-                    nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
+                    nbTotalManifolds += pair.nbPotentialContactManifolds;
 
                     // Add the contact manifold into the island
-                    mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
+                    mIslands.nbContactManifolds[islandIndex] += pair.nbPotentialContactManifolds;
                     pair.isAlreadyInIsland = true;
 
                     // Check if the other body has already been added to the island

src/systems/CollisionDetectionSystem.cpp

@@ -62,13 +62,11 @@ CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, Collider
                      mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator(), mOverlappingPairs), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()),
                      mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
                      mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
-                     mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()),
+                     mLostContactPairs(mMemoryManager.getSingleFrameAllocator()), mPreviousMapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator()),
-                     mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()), mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1),
+                     mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()),
-                     mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2), mContactManifolds1(mMemoryManager.getPoolAllocator()),
+                     mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
-                     mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
+                     mContactPoints1(mMemoryManager.getPoolAllocator()), mContactPoints2(mMemoryManager.getPoolAllocator()),
-                     mContactPoints1(mMemoryManager.getPoolAllocator()),
+                     mPreviousContactPoints(&mContactPoints1), mCurrentContactPoints(&mContactPoints2) {
-                     mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
-                     mCurrentContactPoints(&mContactPoints2) {
 
 #ifdef IS_RP3D_PROFILING_ENABLED
 
@@ -159,7 +157,7 @@ void CollisionDetectionSystem::addLostContactPair(uint64 overlappingPairIndex) {
 
     // Create a lost contact pair
     ContactPair lostContactPair(mOverlappingPairs.mPairIds[overlappingPairIndex], body1Entity, body2Entity, collider1Entity, collider2Entity, mLostContactPairs.size(),
-                                true, isTrigger, mMemoryManager.getHeapAllocator());
+                                true, isTrigger);
     mLostContactPairs.add(lostContactPair);
 }
 
@@ -480,13 +478,14 @@ 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<uint64, uint>* mapPairIdToContactPairIndex,
                                                      List<ContactManifoldInfo>& potentialContactManifolds,
                                                      List<ContactPair>* contactPairs) {
 
     assert(contactPairs->size() == 0);
     assert(mapPairIdToContactPairIndex->size() == 0);
 
+    Map<uint64, uint> mapPairIdToContactPairIndex(mMemoryManager.getHeapAllocator(), mPreviousMapPairIdToContactPairIndex.size());
+
     // get the narrow-phase batches to test for collision
     NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch();
     NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch();
@@ -496,18 +495,13 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar
     NarrowPhaseInfoBatch& convexPolyhedronVsConvexPolyhedronBatch = narrowPhaseInput.getConvexPolyhedronVsConvexPolyhedronBatch();
 
     // Process the potential contacts
-    processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+    processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
-                             potentialContactManifolds, contactPairs);
+    processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
-    processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+    processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
-                             potentialContactManifolds, contactPairs);
+    processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
-    processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+    processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
-                             potentialContactManifolds, contactPairs);
+    processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints,
-    processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
+                             potentialContactManifolds, mapPairIdToContactPairIndex, contactPairs);
-                            potentialContactManifolds, contactPairs);
-    processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs);
-    processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs);
 }
 
 // Compute the narrow-phase collision detection
@@ -527,7 +521,7 @@ void CollisionDetectionSystem::computeNarrowPhase() {
     testNarrowPhaseCollision(mNarrowPhaseInput, true, allocator);
 
     // Process all the potential contacts after narrow-phase collision
-    processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex,
+    processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints,
                                 mPotentialContactManifolds, mCurrentContactPairs);
 
     // Reduce the number of contact points in the manifolds
@@ -539,9 +533,21 @@ void CollisionDetectionSystem::computeNarrowPhase() {
     // Create the actual narrow-phase contacts
     createContacts();
 
+    // Compute the map from contact pairs ids to contact pair for the next frame
+    computeMapPreviousContactPairs();
+
     mNarrowPhaseInput.clear();
 }
 
+/// Compute the map from contact pairs ids to contact pair for the next frame
+void CollisionDetectionSystem::computeMapPreviousContactPairs() {
+
+    mPreviousMapPairIdToContactPairIndex.clear();
+    for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+        mPreviousMapPairIdToContactPairIndex.add(Pair<uint64, uint>((*mCurrentContactPairs)[i].pairId, i));
+    }
+}
+
 // Compute the narrow-phase collision detection for the testOverlap() methods.
 /// This method returns true if contacts are found.
 bool CollisionDetectionSystem::computeNarrowPhaseOverlapSnapshot(NarrowPhaseInput& narrowPhaseInput, OverlapCallback* callback) {
@@ -629,8 +635,7 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf
                 const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
 
                 // Create a new contact pair
-                ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity,
+                ContactPair contactPair(narrowPhaseInfoBatch.overlappingPairIds[i], body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false);
-                                        contactPairs.size(), isTrigger, false, mMemoryManager.getHeapAllocator());
                 contactPairs.add(contactPair);
 
                 setOverlapContactPairId.add(narrowPhaseInfoBatch.overlappingPairIds[i]);
@@ -657,14 +662,13 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn
 
         List<ContactPointInfo> potentialContactPoints(allocator);
         List<ContactManifoldInfo> potentialContactManifolds(allocator);
-        Map<uint64, uint> mapPairIdToContactPairIndex(allocator);
         List<ContactPair> contactPairs(allocator);
         List<ContactPair> lostContactPairs(allocator);                  // Not used during collision snapshots
         List<ContactManifold> contactManifolds(allocator);
         List<ContactPoint> contactPoints(allocator);
 
         // Process all the potential contacts after narrow-phase collision
-        processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, &contactPairs);
+        processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, potentialContactManifolds, &contactPairs);
 
         // Reduce the number of contact points in the manifolds
         reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints);
@@ -687,24 +691,20 @@ void CollisionDetectionSystem::swapPreviousAndCurrentContacts() {
         mPreviousContactPairs = &mContactPairs2;
         mPreviousContactManifolds = &mContactManifolds2;
         mPreviousContactPoints = &mContactPoints2;
-        mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2;
 
         mCurrentContactPairs = &mContactPairs1;
         mCurrentContactManifolds = &mContactManifolds1;
         mCurrentContactPoints = &mContactPoints1;
-        mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1;
     }
     else {
 
         mPreviousContactPairs = &mContactPairs1;
         mPreviousContactManifolds = &mContactManifolds1;
         mPreviousContactPoints = &mContactPoints1;
-        mPreviousMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex1;
 
         mCurrentContactPairs = &mContactPairs2;
         mCurrentContactManifolds = &mContactManifolds2;
         mCurrentContactPoints = &mContactPoints2;
-        mCurrentMapPairIdToContactPairIndex = &mMapPairIdToContactPairIndex2;
     }
 }
 
@@ -722,7 +722,7 @@ void CollisionDetectionSystem::createContacts() {
         ContactPair& contactPair = (*mCurrentContactPairs)[p];
 
         contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
-        contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
+        contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
         contactPair.contactPointsIndex = mCurrentContactPoints->size();
 
         // Add the associated contact pair to both bodies of the pair (used to create islands later)
@@ -734,13 +734,13 @@ void CollisionDetectionSystem::createContacts() {
         }
 
         // For each potential contact manifold of the pair
-        for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
+        for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) {
 
             ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
 
             // Start index and number of contact points for this manifold
             const uint contactPointsIndex = mCurrentContactPoints->size();
-            const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
+            const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints);
             contactPair.nbToTalContactPoints += nbContactPoints;
 
             // We create a new contact manifold
@@ -750,10 +750,10 @@ void CollisionDetectionSystem::createContacts() {
             // Add the contact manifold
             mCurrentContactManifolds->add(contactManifold);
 
-            assert(potentialManifold.potentialContactPointsIndices.size() > 0);
+            assert(potentialManifold.nbPotentialContactPoints > 0);
 
             // For each contact point of the manifold
-            for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
+            for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) {
 
                 ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
 
@@ -775,7 +775,6 @@ void CollisionDetectionSystem::createContacts() {
     mPreviousContactPoints->clear();
     mPreviousContactManifolds->clear();
     mPreviousContactPairs->clear();
-    mPreviousMapPairIdToContactPairIndex->clear();
 
     // Reset the potential contacts
     mPotentialContactPoints.clear(true);
@@ -817,20 +816,20 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
     for (uint p=0; p < contactPairs.size(); p++) {
 
         ContactPair& contactPair = contactPairs[p];
-        assert(contactPair.potentialContactManifoldsIndices.size() > 0);
+        assert(contactPair.nbPotentialContactManifolds > 0);
 
         contactPair.contactManifoldsIndex = contactManifolds.size();
-        contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
+        contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
         contactPair.contactPointsIndex = contactPoints.size();
 
         // For each potential contact manifold of the pair
-        for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
+        for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) {
 
             ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
 
             // Start index and number of contact points for this manifold
             const uint contactPointsIndex = contactPoints.size();
-            const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
+            const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints;
             contactPair.nbToTalContactPoints += nbContactPoints;
 
             // We create a new contact manifold
@@ -840,10 +839,10 @@ void CollisionDetectionSystem::createSnapshotContacts(List<ContactPair>& contact
             // Add the contact manifold
             contactManifolds.add(contactManifold);
 
-            assert(potentialManifold.potentialContactPointsIndices.size() > 0);
+            assert(potentialManifold.nbPotentialContactPoints > 0);
 
             // For each contact point of the manifold
-            for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
+            for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) {
 
                 ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
 
@@ -866,10 +865,10 @@ void CollisionDetectionSystem::initContactsWithPreviousOnes() {
         ContactPair& currentContactPair = (*mCurrentContactPairs)[i];
 
         // Find the corresponding contact pair in the previous frame (if any)
-        auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex->find(currentContactPair.pairId);
+        auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex.find(currentContactPair.pairId);
 
         // If we have found a corresponding contact pair in the previous frame
-        if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex->end()) {
+        if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex.end()) {
 
             const uint previousContactPairIndex = itPrevContactPair->second;
             ContactPair& previousContactPair = (*mPreviousContactPairs)[previousContactPairIndex];
@@ -988,8 +987,8 @@ void CollisionDetectionSystem::raycast(RaycastCallback* raycastCallback,
 // Convert the potential contact into actual contacts
 void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo,
                                                         List<ContactPointInfo>& potentialContactPoints,
-                                                        Map<uint64, uint>* mapPairIdToContactPairIndex,
                                                         List<ContactManifoldInfo>& potentialContactManifolds,
+                                                        Map<uint64, uint>& mapPairIdToContactPairIndex,
                                                         List<ContactPair>* contactPairs) {
 
     RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler);
@@ -1012,13 +1011,6 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
 
             mOverlappingPairs.mCollidingInCurrentFrame[pairIndex] = true;
 
-            // If there is not already a contact pair for this overlapping pair
-            auto it = mapPairIdToContactPairIndex->find(pairId);
-            ContactPair* pairContact = nullptr;
-            if (it == mapPairIdToContactPairIndex->end()) {
-
-                // Create a new ContactPair
-
             const Entity collider1Entity = narrowPhaseInfoBatch.colliderEntities1[i];
             const Entity collider2Entity = narrowPhaseInfoBatch.colliderEntities2[i];
 
@@ -1028,17 +1020,70 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
             const Entity body1Entity = mCollidersComponents.mBodiesEntities[collider1Index];
             const Entity body2Entity = mCollidersComponents.mBodiesEntities[collider2Index];
 
+            // If we have a convex vs convex collision (if we consider the base collision shapes of the colliders)
+            if (mCollidersComponents.mCollisionShapes[collider1Index]->isConvex() &&
+                mCollidersComponents.mCollisionShapes[collider2Index]->isConvex()) {
+
+                // Create a new ContactPair
+
                 const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
 
                 assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity));
 
                 const uint newContactPairIndex = contactPairs->size();
-                ContactPair overlappingPairContact(pairId, body1Entity, body2Entity,
+
-                                                   collider1Entity, collider2Entity,
+                contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
-                                                   newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger, mMemoryManager.getHeapAllocator());
+                                      newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger);
-                contactPairs->add(overlappingPairContact);
+
+                ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]);
+
+                // Create a new potential contact manifold for the overlapping pair
+                uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
+                potentialContactManifolds.emplace(pairId);
+                ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
+
+                const uint contactPointIndexStart = static_cast<uint>(potentialContactPoints.size());
+
+                // Add the potential contacts
+                for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
+
+                    if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
+
+                        // Add the contact point to the manifold
+                        contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j;
+                        contactManifoldInfo.nbPotentialContactPoints++;
+
+                        // Add the contact point to the list of potential contact points
+                        const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
+
+                        potentialContactPoints.add(contactPoint);
+                    }
+                }
+
+                // Add the contact manifold to the overlapping pair contact
+                assert(overlappingPairContact.pairId == contactManifoldInfo.pairId);
+                pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex;
+                pairContact->nbPotentialContactManifolds = 1;
+            }
+            else {
+
+                // If there is not already a contact pair for this overlapping pair
+                auto it = mapPairIdToContactPairIndex.find(pairId);
+                ContactPair* pairContact = nullptr;
+                if (it == mapPairIdToContactPairIndex.end()) {
+
+                    // Create a new ContactPair
+
+                    const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
+
+                    assert(!mWorld->mCollisionBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mCollisionBodyComponents.getIsEntityDisabled(body2Entity));
+
+                    const uint newContactPairIndex = contactPairs->size();
+                    contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
+                                                       newContactPairIndex, mOverlappingPairs.getCollidingInPreviousFrame(pairId), isTrigger);
                     pairContact = &((*contactPairs)[newContactPairIndex]);
-                mapPairIdToContactPairIndex->add(Pair<uint64, uint>(pairId, newContactPairIndex));
+                    mapPairIdToContactPairIndex.add(Pair<uint64, uint>(pairId, newContactPairIndex));
+
                 }
                 else { // If a ContactPair already exists for this overlapping pair, we use this one
 
@@ -1063,12 +1108,14 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                     bool similarManifoldFound = false;
 
                     // For each contact manifold of the overlapping pair
-                for (uint m=0; m < pairContact->potentialContactManifoldsIndices.size(); m++) {
+                    for (uint m=0; m < pairContact->nbPotentialContactManifolds; m++) {
 
                        uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
 
+                       if (potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
+
                            // Get the first contact point of the current manifold
-                   assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0);
+                           assert(potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints > 0);
                            const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
                            const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex];
 
@@ -1077,35 +1124,38 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                             if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
 
                                 // Add the contact point to the manifold
-                        potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex);
+                                potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex;
+                                potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++;
 
                                 similarManifoldFound = true;
 
                                 break;
                             }
                        }
+                    }
 
                     // If we have not found a manifold with a similar contact normal for the contact point
-                if (!similarManifoldFound) {
+                    if (!similarManifoldFound && pairContact->nbPotentialContactManifolds < NB_MAX_POTENTIAL_CONTACT_MANIFOLDS) {
 
-                    // Create a new contact manifold for the overlapping pair
+                        // Create a new potential contact manifold for the overlapping pair
-                    ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator());
+                        uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
+                        potentialContactManifolds.emplace(pairId);
+                        ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
 
                         // Add the contact point to the manifold
-                    contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex);
+                        contactManifoldInfo.potentialContactPointsIndices[0] = contactPointIndex;
+                        contactManifoldInfo.nbPotentialContactPoints = 1;
 
                         assert(pairContact != nullptr);
 
-                    // Add the potential contact manifold
-                    uint contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
-                    potentialContactManifolds.add(contactManifoldInfo);
-
                         // Add the contact manifold to the overlapping pair contact
                         assert(pairContact->pairId == contactManifoldInfo.pairId);
-                    pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
+                        pairContact->potentialContactManifoldsIndices[pairContact->nbPotentialContactManifolds] = contactManifoldIndex;
+                        pairContact->nbPotentialContactManifolds++;
                     }
 
-                assert(pairContact->potentialContactManifoldsIndices.size() > 0);
+                    assert(pairContact->nbPotentialContactManifolds > 0);
+                }
             }
 
             narrowPhaseInfoBatch.resetContactPoints(i);
@@ -1126,12 +1176,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
         ContactPair& contactPair = (*contactPairs)[i];
 
         // While there are too many manifolds in the contact pair
-        while(contactPair.potentialContactManifoldsIndices.size() > mWorld->mConfig.nbMaxContactManifolds) {
+        while(contactPair.nbPotentialContactManifolds > NB_MAX_CONTACT_MANIFOLDS) {
 
             // Look for a manifold with the smallest contact penetration depth.
             decimal minDepth = DECIMAL_LARGEST;
             int minDepthManifoldIndex = -1;
-            for (uint j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) {
+            for (uint j=0; j < contactPair.nbPotentialContactManifolds; j++) {
 
                 ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]];
 
@@ -1146,7 +1196,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
 
             // Remove the non optimal manifold
             assert(minDepthManifoldIndex >= 0);
-            contactPair.potentialContactManifoldsIndices.removeAt(minDepthManifoldIndex);
+            contactPair.removePotentialManifoldAtIndex(minDepthManifoldIndex);
         }
     }
 
@@ -1156,12 +1206,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
         const ContactPair& pairContact = (*contactPairs)[i];
 
         // For each potential contact manifold
-        for (uint j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) {
+        for (uint j=0; j < pairContact.nbPotentialContactManifolds; j++) {
 
             ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
 
             // If there are two many contact points in the manifold
-            if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) {
+            if (manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
 
                 Entity collider1 = mOverlappingPairs.mColliders1[mOverlappingPairs.mMapPairIdToPairIndex[manifold.pairId]];
 
@@ -1171,7 +1221,7 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(List<ContactPair>
                 reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
             }
 
-            assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+            assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
         }
     }
 }
@@ -1182,9 +1232,9 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co
 
     decimal largestDepth = 0.0f;
 
-    assert(manifold.potentialContactPointsIndices.size() > 0);
+    assert(manifold.nbPotentialContactPoints > 0);
 
-    for (uint i=0; i < manifold.potentialContactPointsIndices.size(); i++) {
+    for (uint i=0; i < manifold.nbPotentialContactPoints; i++) {
         decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth;
 
         if (depth > largestDepth) {
@@ -1202,14 +1252,18 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co
 void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
                                              const List<ContactPointInfo>& potentialContactPoints) const {
 
-    assert(manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD);
+    assert(manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
 
     // The following algorithm only works to reduce to a maximum of 4 contact points
     assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
 
     // List of the candidate contact points indices in the manifold. Every time that we have found a
     // point we want to keep, we will remove it from this list
-    List<uint> candidatePointsIndices(manifold.potentialContactPointsIndices);
+    uint candidatePointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
+    uint8 nbCandidatePoints = manifold.nbPotentialContactPoints;
+    for (uint8 i=0 ; i < manifold.nbPotentialContactPoints; i++) {
+        candidatePointsIndices[i] = manifold.potentialContactPointsIndices[i];
+    }
 
     int8 nbReducedPoints = 0;
 
@@ -1233,7 +1287,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     const Vector3 searchDirection(1, 1, 1);
     decimal maxDotProduct = DECIMAL_SMALLEST;
     uint elementIndexToKeep = 0;
-    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+    for (uint i=0; i < nbCandidatePoints; i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
         decimal dotProduct = searchDirection.dot(element.localPoint1);
@@ -1244,7 +1298,8 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
         }
     }
     pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep];
-    candidatePointsIndices.removeAt(elementIndexToKeep);
+    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
+    //candidatePointsIndices.removeAt(elementIndexToKeep);
     assert(nbReducedPoints == 1);
 
     // Compute the second contact point we need to keep.
@@ -1252,7 +1307,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
 
     decimal maxDistance = decimal(0.0);
     elementIndexToKeep = 0;
-    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+    for (uint i=0; i < nbCandidatePoints; i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
         const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@@ -1268,7 +1323,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
 
     }
     pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep];
-    candidatePointsIndices.removeAt(elementIndexToKeep);
+    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
     assert(nbReducedPoints == 2);
 
     // Compute the third contact point we need to keep.
@@ -1281,7 +1336,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     decimal minArea = decimal(0.0);
     decimal maxArea = decimal(0.0);
     bool isPreviousAreaPositive = true;
-    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+    for (uint i=0; i < nbCandidatePoints; i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
         const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@@ -1310,12 +1365,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     if (maxArea > (-minArea)) {
         isPreviousAreaPositive = true;
         pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex];
-        candidatePointsIndices.removeAt(thirdPointMaxAreaIndex);
+        removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints);
     }
     else {
         isPreviousAreaPositive = false;
         pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex];
-        candidatePointsIndices.removeAt(thirdPointMinAreaIndex);
+        removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints);
     }
     nbReducedPoints = 3;
 
@@ -1328,7 +1383,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     decimal area;
 
     // For each remaining candidate points
-    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+    for (uint i=0; i < nbCandidatePoints; i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
 
@@ -1365,14 +1420,22 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
         }
     }
     pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
-    candidatePointsIndices.removeAt(elementIndexToKeep);
+    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
 
     // Only keep the four selected contact points in the manifold
-    manifold.potentialContactPointsIndices.clear();
+    manifold.potentialContactPointsIndices[0] = pointsToKeepIndices[0];
-    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]);
+    manifold.potentialContactPointsIndices[1] = pointsToKeepIndices[1];
-    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]);
+    manifold.potentialContactPointsIndices[2] = pointsToKeepIndices[2];
-    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]);
+    manifold.potentialContactPointsIndices[3] = pointsToKeepIndices[3];
-    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]);
+    manifold.nbPotentialContactPoints = 4;
+}
+
+// Remove an element in an array (and replace it by the last one in the array)
+void CollisionDetectionSystem::removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const {
+    assert(index < arraySize);
+    assert(arraySize > 0);
+    array[index] = array[arraySize - 1];
+    arraySize--;
 }
 
 // Report contacts and triggers