Refactor contacts management

CMakeLists.txt

@@ -81,6 +81,8 @@ SET (REACTPHYSICS3D_HEADERS
     "src/body/CollisionBody.h"
     "src/body/RigidBody.h"
     "src/collision/ContactPointInfo.h"
+    "src/collision/ContactManifoldInfo.h"
+    "src/collision/ContactPair.h"
     "src/collision/broadphase/DynamicAABBTree.h"
     "src/collision/narrowphase/CollisionDispatch.h"
     "src/collision/narrowphase/GJK/VoronoiSimplex.h"

src/collision/CollisionDetection.cpp

@@ -1,4 +1,4 @@
-/********************************************************************************
+/********************************************************************************
 * ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
 * Copyright (c) 2010-2018 Daniel Chappuis                                       *
 *********************************************************************************
@@ -30,6 +30,8 @@
 #include "body/Body.h"
 #include "collision/shapes/BoxShape.h"
 #include "collision/shapes/ConcaveShape.h"
+#include "collision/ContactManifoldInfo.h"
+#include "constraint/ContactPoint.h"
 #include "body/RigidBody.h"
 #include "configuration.h"
 #include "collision/CollisionCallback.h"
@@ -55,7 +57,17 @@ CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponen
                      mOverlappingPairs(mMemoryManager.getPoolAllocator()),
                      mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents),
                      mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()),
-                     mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()) {
+                     mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getPoolAllocator()),
+                     // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds,  mMapPairIdToOverlappingPairContacts
+                     mPotentialContactManifolds(mMemoryManager.getPoolAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
+                     mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
+                     mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()),
+                     mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2),
+                     mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()),
+                     mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
+                     mContactPoints1(mMemoryManager.getPoolAllocator()),
+                     mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
+                     mCurrentContactPoints(&mContactPoints2) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -365,6 +377,9 @@ bool CollisionDetection::testNarrowPhaseCollision(NarrowPhaseInput& narrowPhaseI
 // Process the potential contacts after narrow-phase collision detection
 void CollisionDetection::processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo) {
 
+    assert(mCurrentContactPairs->size() == 0);
+    assert(mCurrentMapPairIdToContactPairIndex->size() == 0);
+
     // get the narrow-phase batches to test for collision
     NarrowPhaseInfoBatch& sphereVsSphereBatch = narrowPhaseInput.getSphereVsSphereBatch();
     NarrowPhaseInfoBatch& sphereVsCapsuleBatch = narrowPhaseInput.getSphereVsCapsuleBatch();
@@ -389,6 +404,9 @@ void CollisionDetection::computeNarrowPhase() {
 
     MemoryAllocator& allocator = mMemoryManager.getSingleFrameAllocator();
 
+    // Swap the previous and current contacts lists
+    swapPreviousAndCurrentContacts();
+
     // Test the narrow-phase collision detection on the batches to be tested
     testNarrowPhaseCollision(mNarrowPhaseInput, false, true, allocator);
 
@@ -396,7 +414,13 @@ void CollisionDetection::computeNarrowPhase() {
     processAllPotentialContacts(mNarrowPhaseInput, true);
 
     // Reduce the number of contact points in the manifolds
-    reduceContactManifolds(mOverlappingPairs);
+    reducePotentialContactManifolds(mOverlappingPairs);
+
+    // Create the actual contact manifolds and contacts (from potential contacts)
+    createContacts();
+
+    // Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+    initContactsWithPreviousOnes();
 
     // Add all the contact manifolds (between colliding bodies) to the bodies
     addAllContactManifoldsToBodies();
@@ -406,6 +430,193 @@ void CollisionDetection::computeNarrowPhase() {
 
     // Clear the list of narrow-phase infos
     mNarrowPhaseInput.clear();
+
+    // TODO : Do not forget to clear all the contact pair, contact manifolds and contact points lists
+}
+
+// Swap the previous and current contacts lists
+void CollisionDetection::swapPreviousAndCurrentContacts() {
+
+    if (mPreviousContactPairs == &mContactPairs1) {
+
+        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;
+    }
+}
+
+// Create the actual contact manifolds and contacts (from potential contacts)
+void CollisionDetection::createContacts() {
+
+    RP3D_PROFILE("CollisionDetection::createContacts()", mProfiler);
+
+    assert(mCurrentContactManifolds->size() == 0);
+    assert(mCurrentContactPoints->size() == 0);
+
+    // For each contact pair
+    for (auto it = mCurrentMapPairIdToContactPairIndex->begin(); it != mCurrentMapPairIdToContactPairIndex->end(); ++it) {
+
+        const uint contactPairIndex = it->second;
+        assert(contactPairIndex < mCurrentContactPairs->size());
+        ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
+
+        assert(contactPair.potentialContactManifoldsIndices.size() > 0);
+
+        // Start index and numnber of contact manifolds for this contact pair
+        contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
+        contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
+        contactPair.contactPointsIndex = mCurrentContactPoints->size();
+
+        // For each potential contact manifold of the pair
+        for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); 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 = potentialManifold.potentialContactPointsIndices.size();
+
+            // We create a new contact manifold
+            ContactManifold contactManifold(contactPointsIndex, nbContactPoints, mMemoryManager.getPoolAllocator(), mWorld->mConfig);
+
+            // Add the contact manifold
+            mCurrentContactManifolds->add(contactManifold);
+
+            // Increase the number of total contact point of the contact pair
+            contactPair.nbToTalContactPoints += potentialManifold.potentialContactPointsIndices.size();
+
+            assert(potentialManifold.potentialContactPointsIndices.size() > 0);
+
+            // For each contact point of the manifold
+            for (uint c=0; c < potentialManifold.potentialContactPointsIndices.size(); c++) {
+
+                ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
+
+                // Create a new contact point
+                ContactPoint contactPoint(potentialContactPoint, mWorld->mConfig);
+
+                // Add the contact point
+                mCurrentContactPoints->add(contactPoint);
+            }
+        }
+    }
+
+    // Reset the potential contacts
+    mPotentialContactPoints.clear();
+    mPotentialContactManifolds.clear();
+}
+
+// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+void CollisionDetection::initContactsWithPreviousOnes() {
+
+    // For each contact pair of the current frame
+    for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+
+        ContactPair& currentContactPair = (*mCurrentContactPairs)[i];
+
+        // Find the corresponding contact pair in the previous frame (if any)
+        auto itPrevContactPair = mPreviousMapPairIdToContactPairIndex->find(currentContactPair.pairId);
+
+        // If we have found a corresponding contact pair in the previous frame
+        if (itPrevContactPair != mPreviousMapPairIdToContactPairIndex->end()) {
+
+            const uint previousContactPairIndex = itPrevContactPair->second;
+            ContactPair& previousContactPair = (*mPreviousContactPairs)[previousContactPairIndex];
+
+            // --------------------- Contact Manifolds --------------------- //
+
+            const uint contactManifoldsIndex = currentContactPair.contactManifoldsIndex;
+            const uint nbContactManifolds = currentContactPair.nbContactManifolds;
+
+            // For each current contact manifold of the contact pair
+            for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
+
+                assert(m < mCurrentContactManifolds->size());
+                ContactManifold& currentContactManifold = (*mCurrentContactManifolds)[m];
+                assert(currentContactManifold.mNbContactPoints > 0);
+                ContactPoint& currentContactPoint = (*mCurrentContactPoints)[currentContactManifold.mContactPointsIndex];
+                const Vector3& currentContactPointNormal = currentContactPoint.getNormal();
+
+                // Find a similar contact manifold among the contact manifolds from the previous frame (for warmstarting)
+                const uint previousContactManifoldIndex = previousContactPair.contactManifoldsIndex;
+                const uint previousNbContactManifolds = previousContactPair.nbContactManifolds;
+                for (uint p=previousContactManifoldIndex; p < previousContactManifoldIndex + previousNbContactManifolds; p++) {
+
+                    ContactManifold& previousContactManifold = (*mPreviousContactManifolds)[p];
+                    assert(previousContactManifold.mNbContactPoints > 0);
+                    ContactPoint& previousContactPoint = (*mPreviousContactPoints)[previousContactManifold.mContactPointsIndex];
+
+                    // If the previous contact manifold has a similar contact normal with the current manifold
+                    if (previousContactPoint.getNormal().dot(currentContactPointNormal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
+
+                        // Transfer data from the previous contact manifold to the current one
+                        currentContactManifold.mFrictionVector1 = previousContactManifold.mFrictionVector1;
+                        currentContactManifold.mFrictionVector2 = previousContactManifold.mFrictionVector2;
+                        currentContactManifold.mFrictionImpulse1 = previousContactManifold.mFrictionImpulse1;
+                        currentContactManifold.mFrictionImpulse2 = previousContactManifold.mFrictionImpulse2;
+                        currentContactManifold.mFrictionTwistImpulse = previousContactManifold.mFrictionTwistImpulse;
+                        currentContactManifold.mRollingResistanceImpulse = previousContactManifold.mRollingResistanceImpulse;
+
+                        break;
+                    }
+                }
+            }
+
+            // --------------------- Contact Points --------------------- //
+
+            const uint contactPointsIndex = currentContactPair.contactPointsIndex;
+            const uint nbTotalContactPoints = currentContactPair.nbToTalContactPoints;
+
+            // For each current contact point of the contact pair
+            for (uint c=contactPointsIndex; c < contactPointsIndex + nbTotalContactPoints; c++) {
+
+                assert(c < mCurrentContactPoints->size());
+                ContactPoint& currentContactPoint = (*mCurrentContactPoints)[c];
+
+                // Find a similar contact point among the contact points from the previous frame (for warmstarting)
+                const uint previousContactPointsIndex = previousContactPair.contactPointsIndex;
+                const uint previousNbContactPoints = previousContactPair.nbToTalContactPoints;
+                for (uint p=previousContactPointsIndex; p < previousContactPointsIndex + previousNbContactPoints; p++) {
+
+                    ContactPoint& previousContactPoint = (*mPreviousContactPoints)[p];
+
+                    // If the previous contact point is very close to th current one
+                    const decimal distSquare = (currentContactPoint.getLocalPointOnShape1() - previousContactPoint.getLocalPointOnShape1()).lengthSquare();
+                    if (distSquare <= mWorld->mConfig.persistentContactDistanceThreshold * mWorld->mConfig.persistentContactDistanceThreshold) {
+
+                        // Transfer data from the previous contact point to the current one
+                        currentContactPoint.setPenetrationImpulse(previousContactPoint.getPenetrationImpulse());
+                        currentContactPoint.setIsRestingContact(previousContactPoint.getIsRestingContact());
+
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+    mPreviousContactPoints->clear();
+    mPreviousContactManifolds->clear();
+    mPreviousContactPairs->clear();
+    mPreviousMapPairIdToContactPairIndex->clear();
 }
 
 // Remove a body from the collision detection
@@ -506,7 +717,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
 /// Convert the potential contact into actual contacts
 void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, bool updateLastFrameInfo) {
 
-    RP3D_PROFILE("CollisionDetection::processAllPotentialContacts()", mProfiler);
+    RP3D_PROFILE("CollisionDetection::processPotentialContacts()", mProfiler);
 
     // For each narrow phase info object
     for(uint i=0; i < narrowPhaseInfoBatch.getNbObjects(); i++) {
@@ -518,21 +729,116 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
             narrowPhaseInfoBatch.lastFrameCollisionInfos[i]->isValid = true;
         }
 
+        // ----- START OLD ----- //
+
         if (narrowPhaseInfoBatch.isColliding[i]) {
 
             assert(narrowPhaseInfoBatch.contactPoints[i].size() > 0);
 
             // Transfer the contact points from the narrow phase info to the overlapping pair
+            // TOOD : COMMENT THIS
             narrowPhaseInfoBatch.overlappingPairs[i]->addPotentialContactPoints(narrowPhaseInfoBatch, i);
 
             // Remove the contacts points from the narrow phase info object.
-            narrowPhaseInfoBatch.resetContactPoints(i);
+            //narrowPhaseInfoBatch.resetContactPoints(i);
         }
+
+        // ----- END OLD ----- //
+
+        // Add the potential contacts
+        for (uint j=0; j < narrowPhaseInfoBatch.contactPoints[i].size(); j++) {
+
+            if (narrowPhaseInfoBatch.isColliding[i]) {
+
+                const ContactPointInfo& contactPoint = *(narrowPhaseInfoBatch.contactPoints[i][j]);
+
+                // Add the contact point to the list of potential contact points
+                const uint contactPointIndex = static_cast<uint>(mPotentialContactPoints.size());
+
+                // TODO : We should probably use single frame allocator here for mPotentialContactPoints
+                mPotentialContactPoints.add(contactPoint);
+
+                bool similarManifoldFound = false;
+
+                // If there are already potential contact manifolds for this overlapping pair
+                OverlappingPair::OverlappingPairId pairId = narrowPhaseInfoBatch.overlappingPairs[i]->getId();
+                auto it = mCurrentMapPairIdToContactPairIndex->find(pairId);
+                ContactPair* pairContact = nullptr;
+                if (it != mCurrentMapPairIdToContactPairIndex->end()) {
+
+                    const uint pairContactIndex = it->second;
+                    pairContact = &((*mCurrentContactPairs)[pairContactIndex]);
+
+                    assert(pairContact->potentialContactManifoldsIndices.size() > 0);
+
+                    // For each contact manifold of the overlapping pair
+                    for (uint m=0; m < pairContact->potentialContactManifoldsIndices.size(); m++) {
+                       uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
+
+                       // Get the first contact point of the current manifold
+                       assert(mPotentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0);
+                       const uint manifoldContactPointIndex = mPotentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
+                       const ContactPointInfo& manifoldContactPoint = mPotentialContactPoints[manifoldContactPointIndex];
+
+                        // If we have found a corresponding manifold for the new contact point
+                        // (a manifold with a similar contact normal direction)
+                        if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
+
+                            // Add the contact point to the manifold
+                            mPotentialContactManifolds[m].potentialContactPointsIndices.add(contactPointIndex);
+
+                           similarManifoldFound = true;
+
+                           break;
+                        }
+                    }
+                }
+
+                // If we have not found a manifold with a similar contact normal for the contact point
+                if (!similarManifoldFound) {
+
+                    // Create a new contact manifold for the overlapping pair
+                    // TODO : We should probably use single frame allocator here
+                    ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator());
+
+                    // Add the contact point to the manifold
+                    contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex);
+
+                    // If the overlapping pair contact does not exists yet
+                    if (pairContact == nullptr) {
+
+                        // TODO : We should probably use a single frame allocator here
+                        ContactPair overlappingPairContact(pairId, mMemoryManager.getPoolAllocator());
+                        const uint newContactPairIndex = mCurrentContactPairs->size();
+                        mCurrentContactPairs->add(overlappingPairContact);
+                        pairContact = &((*mCurrentContactPairs)[newContactPairIndex]);
+                        mCurrentMapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));
+                    }
+
+                    assert(pairContact != nullptr);
+
+                    // Add the potential contact manifold
+                    uint contactManifoldIndex = static_cast<uint>(mPotentialContactManifolds.size());
+                    mPotentialContactManifolds.add(contactManifoldInfo);
+
+                    // Add the contact manifold to the overlapping pair contact
+                    pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
+                }
+
+                assert(pairContact->potentialContactManifoldsIndices.size() > 0);
+            }
+        }
+
+        narrowPhaseInfoBatch.resetContactPoints(i);
     }
 }
 
 // Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds
-void CollisionDetection::reduceContactManifolds(const OverlappingPairMap& overlappingPairs) {
+void CollisionDetection::reducePotentialContactManifolds(const OverlappingPairMap& overlappingPairs) {
+
+    RP3D_PROFILE("CollisionDetection::reducePotentialContactManifolds()", mProfiler);
+
+    // ----- OLD ----- //
 
     // For each overlapping pairs in contact during the narrow-phase
     for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
@@ -546,6 +852,260 @@ void CollisionDetection::reduceContactManifolds(const OverlappingPairMap& overla
         pair->reduceContactManifolds();
     }
 
+    // ----- OLD ----- //
+
+    // Reduce the number of potential contact manifolds in a contact pair
+    for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+
+        ContactPair& contactPair = (*mCurrentContactPairs)[i];
+
+        assert(contactPair.potentialContactManifoldsIndices.size() > 0);
+
+        // While there are too many manifolds
+        while(contactPair.potentialContactManifoldsIndices.size() > mWorld->mConfig.nbMaxContactManifolds) {
+
+            // Look for a manifold with the smallest contact penetration depth.
+            decimal minDepth = DECIMAL_LARGEST;
+            int minDepthManifoldIndex = -1;
+            for (uint i=0; i < contactPair.potentialContactManifoldsIndices.size(); i++) {
+
+                ContactManifoldInfo& manifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[i]];
+
+                // Get the largest contact point penetration depth of the manifold
+                const decimal depth = computePotentialManifoldLargestContactDepth(manifold);
+
+                if (depth < minDepth) {
+                    minDepth = depth;
+                    minDepthManifoldIndex = static_cast<int>(i);
+                }
+            }
+
+            // Remove the non optimal manifold
+            assert(minDepthManifoldIndex >= 0);
+            contactPair.potentialContactManifoldsIndices.removeAt(minDepthManifoldIndex);
+        }
+    }
+
+    // Reduce the number of potential contact points in the manifolds
+    for (uint i=0; i < mCurrentContactPairs->size(); i++) {
+
+        const ContactPair& pairContact = (*mCurrentContactPairs)[i];
+
+        // For each potential contact manifold
+        for (uint i=0; i < pairContact.potentialContactManifoldsIndices.size(); i++) {
+
+            ContactManifoldInfo& manifold = mPotentialContactManifolds[pairContact.potentialContactManifoldsIndices[i]];
+
+            // If there are two many contact points in the manifold
+            if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) {
+
+                Transform shape1LocalToWorldTransoform = mOverlappingPairs[manifold.pairId]->getShape1()->getLocalToWorldTransform();
+
+                // Reduce the number of contact points in the manifold
+                reduceContactPoints(manifold, shape1LocalToWorldTransoform);
+            }
+
+            assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+        }
+    }
+}
+
+// Return the largest depth of all the contact points of a potential manifold
+decimal CollisionDetection::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold) const {
+
+    decimal largestDepth = 0.0f;
+
+    assert(manifold.potentialContactPointsIndices.size() > 0);
+
+    for (uint i=0; i < manifold.potentialContactPointsIndices.size(); i++) {
+        decimal depth = mPotentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth;
+
+        if (depth > largestDepth) {
+            largestDepth = depth;
+        }
+    }
+
+    return largestDepth;
+}
+
+// Reduce the number of contact points of a potential contact manifold
+// This is based on the technique described by Dirk Gregorius in his
+// "Contacts Creation" GDC presentation. This method will reduce the number of
+// contact points to a maximum of 4 points (but it can be less).
+void CollisionDetection::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform) {
+
+    assert(manifold.potentialContactPointsIndices.size() > 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);
+
+    int8 nbReducedPoints = 0;
+
+    uint pointsToKeepIndices[MAX_CONTACT_POINTS_IN_MANIFOLD];
+    for (int8 i=0; i<MAX_CONTACT_POINTS_IN_MANIFOLD; i++) {
+        pointsToKeepIndices[i] = 0;
+    }
+
+    //  Compute the initial contact point we need to keep.
+    // The first point we keep is always the point in a given
+    // constant direction (in order to always have same contact points
+    // between frames for better stability)
+
+    const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
+
+    // Compute the contact normal of the manifold (we use the first contact point)
+    // in the local-space of the first collision shape
+    const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mPotentialContactPoints[candidatePointsIndices[0]].normal;
+
+    // Compute a search direction
+    const Vector3 searchDirection(1, 1, 1);
+    decimal maxDotProduct = DECIMAL_SMALLEST;
+    uint elementIndexToKeep = 0;
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+        const ContactPointInfo& element = mPotentialContactPoints[candidatePointsIndices[i]];
+        decimal dotProduct = searchDirection.dot(element.localPoint1);
+        if (dotProduct > maxDotProduct) {
+            maxDotProduct = dotProduct;
+            elementIndexToKeep = i;
+            nbReducedPoints = 1;
+        }
+    }
+    pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep];
+    candidatePointsIndices.removeAt(elementIndexToKeep);
+    assert(nbReducedPoints == 1);
+
+    // Compute the second contact point we need to keep.
+    // The second point we keep is the one farthest away from the first point.
+
+    decimal maxDistance = decimal(0.0);
+    elementIndexToKeep = 0;
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+        const ContactPointInfo& element = mPotentialContactPoints[candidatePointsIndices[i]];
+        const ContactPointInfo& pointToKeep0 = mPotentialContactPoints[pointsToKeepIndices[0]];
+
+        assert(candidatePointsIndices[i] != pointsToKeepIndices[0]);
+
+        const decimal distance = (pointToKeep0.localPoint1 - element.localPoint1).lengthSquare();
+        if (distance >= maxDistance) {
+            maxDistance = distance;
+            elementIndexToKeep = i;
+            nbReducedPoints = 2;
+        }
+
+    }
+    pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep];
+    candidatePointsIndices.removeAt(elementIndexToKeep);
+    assert(nbReducedPoints == 2);
+
+    // Compute the third contact point we need to keep.
+    // The third point is the one producing the triangle with the larger area
+    // with first and second point.
+
+    // We compute the most positive or most negative triangle area (depending on winding)
+    uint thirdPointMaxAreaIndex = 0;
+    uint thirdPointMinAreaIndex = 0;
+    decimal minArea = decimal(0.0);
+    decimal maxArea = decimal(0.0);
+    bool isPreviousAreaPositive = true;
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+        const ContactPointInfo& element = mPotentialContactPoints[candidatePointsIndices[i]];
+        const ContactPointInfo& pointToKeep0 = mPotentialContactPoints[pointsToKeepIndices[0]];
+        const ContactPointInfo& pointToKeep1 = mPotentialContactPoints[pointsToKeepIndices[1]];
+
+        assert(candidatePointsIndices[i] != pointsToKeepIndices[0]);
+        assert(candidatePointsIndices[i] != pointsToKeepIndices[1]);
+
+        const Vector3 newToFirst = pointToKeep0.localPoint1 - element.localPoint1;
+        const Vector3 newToSecond = pointToKeep1.localPoint1 - element.localPoint1;
+
+        // Compute the triangle area
+        decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
+
+        if (area >= maxArea) {
+            maxArea = area;
+            thirdPointMaxAreaIndex = i;
+        }
+        if (area <= minArea) {
+            minArea = area;
+            thirdPointMinAreaIndex = i;
+        }
+    }
+    assert(minArea <= decimal(0.0));
+    assert(maxArea >= decimal(0.0));
+    if (maxArea > (-minArea)) {
+        isPreviousAreaPositive = true;
+        pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex];
+        candidatePointsIndices.removeAt(thirdPointMaxAreaIndex);
+    }
+    else {
+        isPreviousAreaPositive = false;
+        pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex];
+        candidatePointsIndices.removeAt(thirdPointMinAreaIndex);
+    }
+    nbReducedPoints = 3;
+
+    // Compute the 4th point by choosing the triangle that add the most
+    // triangle area to the previous triangle and has opposite sign area (opposite winding)
+
+    decimal largestArea = decimal(0.0); // Largest area (positive or negative)
+    elementIndexToKeep = 0;
+
+    // For each remaining candidate points
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
+
+        const ContactPointInfo& element = mPotentialContactPoints[candidatePointsIndices[i]];
+
+        assert(candidatePointsIndices[i] != pointsToKeepIndices[0]);
+        assert(candidatePointsIndices[i] != pointsToKeepIndices[1]);
+        assert(candidatePointsIndices[i] != pointsToKeepIndices[2]);
+
+        // For each edge of the triangle made by the first three points
+        for (uint i=0; i<3; i++) {
+
+            uint edgeVertex1Index = i;
+            uint edgeVertex2Index = i < 2 ? i + 1 : 0;
+
+            const ContactPointInfo& pointToKeepEdgeV1 = mPotentialContactPoints[pointsToKeepIndices[edgeVertex1Index]];
+            const ContactPointInfo& pointToKeepEdgeV2 = mPotentialContactPoints[pointsToKeepIndices[edgeVertex2Index]];
+
+            const Vector3 newToFirst = pointToKeepEdgeV1.localPoint1 - element.localPoint1;
+            const Vector3 newToSecond = pointToKeepEdgeV2.localPoint1 - element.localPoint1;
+
+            // Compute the triangle area
+            const decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
+
+            // We are looking at the triangle with maximal area (positive or negative).
+            // If the previous area is positive, we are looking at negative area now.
+            // If the previous area is negative, we are looking at the positive area now.
+            if (isPreviousAreaPositive && area <= largestArea) {
+                largestArea = area;
+                elementIndexToKeep = i;
+                nbReducedPoints = 4;
+            }
+            else if (!isPreviousAreaPositive && area >= largestArea) {
+                largestArea = area;
+                elementIndexToKeep = i;
+                nbReducedPoints = 4;
+            }
+        }
+    }
+    pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
+    candidatePointsIndices.removeAt(elementIndexToKeep);
+    assert(nbReducedPoints == 4);
+
+    // Only keep the four selected contact points in the manifold
+    manifold.potentialContactPointsIndices.clear();
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]);
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]);
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]);
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]);
 }
 
 // Report contacts for all the colliding overlapping pairs
@@ -814,7 +1374,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
     processAllPotentialContacts(narrowPhaseInput, false);
 
     // Reduce the number of contact points in the manifolds
-    reduceContactManifolds(overlappingPairs);
+    reducePotentialContactManifolds(overlappingPairs);
 
     // For each overlapping pair
     for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
@@ -909,7 +1469,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
     processAllPotentialContacts(narrowPhaseInput, false);
 
     // Reduce the number of contact points in the manifolds
-    reduceContactManifolds(overlappingPairs);
+    reducePotentialContactManifolds(overlappingPairs);
 
     // For each overlapping pair
     for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {
@@ -988,7 +1548,7 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
     processAllPotentialContacts(narrowPhaseInput, false);
 
     // Reduce the number of contact points in the manifolds
-    reduceContactManifolds(overlappingPairs);
+    reducePotentialContactManifolds(overlappingPairs);
 
     // For each overlapping pair
     for (auto it = overlappingPairs.begin(); it != overlappingPairs.end(); ++it) {

src/collision/CollisionDetection.h

@@ -30,6 +30,11 @@
 #include "body/CollisionBody.h"
 #include "systems/BroadPhaseSystem.h"
 #include "collision/shapes/CollisionShape.h"
+#include "collision/ContactPointInfo.h"
+#include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
+#include "collision/ContactManifold.h"
+#include "collision/ContactPair.h"
 #include "engine/OverlappingPair.h"
 #include "collision/narrowphase/NarrowPhaseInput.h"
 #include "collision/narrowphase/CollisionDispatch.h"
@@ -64,6 +69,11 @@ class CollisionDetection {
 
         using OverlappingPairMap = Map<Pair<uint, uint>, OverlappingPair*>;
 
+        // -------------------- Constants -------------------- //
+
+        /// Maximum number of contact points in a reduced contact manifold
+        const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
+
         // -------------------- Attributes -------------------- //
 
         /// Memory manager
@@ -96,6 +106,62 @@ class CollisionDetection {
         /// Narrow-phase collision detection input
         NarrowPhaseInput mNarrowPhaseInput;
 
+        /// List of the potential contact points
+        List<ContactPointInfo> mPotentialContactPoints;
+
+        /// List of the potential contact manifolds
+        List<ContactManifoldInfo> mPotentialContactManifolds;
+
+        /// First list of narrow-phase pair contacts
+        List<ContactPair> mContactPairs1;
+
+        /// Second list of narrow-phase pair contacts
+        List<ContactPair> mContactPairs2;
+
+        /// Pointer to the list of contact pairs of the previous frame (either mContactPairs1 or mContactPairs2)
+        List<ContactPair>* mPreviousContactPairs;
+
+        /// Pointer to the list of contact pairs of the current frame (either mContactPairs1 or mContactPairs2)
+        List<ContactPair>* mCurrentContactPairs;
+
+        /// First map of overlapping pair id to the index of the corresponding pair contact
+        Map<OverlappingPair::OverlappingPairId, uint> mMapPairIdToContactPairIndex1;
+
+        /// Second map of overlapping pair id to the index of the corresponding pair contact
+        Map<OverlappingPair::OverlappingPairId, 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;
+
+        /// 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;
+
+        /// First list with the contact manifolds
+        List<ContactManifold> mContactManifolds1;
+
+        /// Second list with the contact manifolds
+        List<ContactManifold> mContactManifolds2;
+
+        /// Pointer to the list of contact manifolds from the previous frame (either mContactManifolds1 or mContactManifolds2)
+        List<ContactManifold>* mPreviousContactManifolds;
+
+        /// Pointer to the list of contact manifolds from the current frame (either mContactManifolds1 or mContactManifolds2)
+        List<ContactManifold>* mCurrentContactManifolds;
+
+        /// Second list of contact points (contact points from either the current frame of the previous frame)
+        List<ContactPoint> mContactPoints1;
+
+        /// Second list of contact points (contact points from either the current frame of the previous frame)
+        List<ContactPoint> mContactPoints2;
+
+        /// Pointer to the contact points of the previous frame (either mContactPoints1 or mContactPoints2)
+        List<ContactPoint>* mPreviousContactPoints;
+
+        /// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2)
+        List<ContactPoint>* mCurrentContactPoints;
+
 #ifdef IS_PROFILING_ACTIVE
 
 		/// Pointer to the profiler
@@ -135,6 +201,9 @@ class CollisionDetection {
         /// Compute the middle-phase collision detection between two proxy shapes
         void computeMiddlePhaseForProxyShapes(OverlappingPair* pair, NarrowPhaseInput& outNarrowPhaseInput);
 
+        /// Swap the previous and current contacts lists
+        void swapPreviousAndCurrentContacts();
+
         /// Convert the potential contact into actual contacts
         void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                       bool updateLastFrameInfo);
@@ -142,12 +211,24 @@ class CollisionDetection {
         /// Process the potential contacts after narrow-phase collision detection
         void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo);
 
-        /// Clear the obsolete manifolds and contact points and reduce the number of contacts points of the remaining manifolds
+        /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
-        void reduceContactManifolds(const OverlappingPairMap& overlappingPairs);
+        void reducePotentialContactManifolds(const OverlappingPairMap& overlappingPairs);
+
+        /// Create the actual contact manifolds and contacts (from potential contacts)
+        void createContacts();
+
+        /// Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+        void initContactsWithPreviousOnes();
+
+        /// Reduce the number of contact points of a potential contact manifold
+        void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform);
 
         /// Report contacts for all the colliding overlapping pairs
         void reportAllContacts();
 
+        /// Return the largest depth of all the contact points of a potential manifold
+        decimal computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold) const;
+
         /// Process the potential contacts where one collion is a concave shape
         void processSmoothMeshContacts(OverlappingPair* pair);
 

src/collision/ContactManifold.cpp

@@ -26,20 +26,35 @@
 // Libraries
 #include "ContactManifold.h"
 #include "constraint/ContactPoint.h"
+#include "collision/ContactManifoldInfo.h"
 
 using namespace reactphysics3d;
 
 // Constructor
+// TODO : REMOVE THIS CONSTRUCTOR
 ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
                                  MemoryAllocator& memoryAllocator, const WorldSettings& worldSettings)
-                : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
+                : mMemoryAllocator(memoryAllocator), mContactPointsIndex(0), mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
                   mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
                   mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
-                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false),
+                  mNext(nullptr), mPrevious(nullptr), mIsObsolete(false),
                   mWorldSettings(worldSettings) {
     
 }
 
+// Constructor
+// TODO : REMOVE worldSettings from this constructor
+ContactManifold::ContactManifold(uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings)
+                :mMemoryAllocator(allocator), mContactPointsIndex(0), mWorldSettings(worldSettings) {
+
+
+    mContactPoints = nullptr;
+    mNext = nullptr;
+    mPrevious = nullptr;
+    mContactPointsIndex = contactPointsIndex;
+    mNbContactPoints = nbContactPoints;
+}
+
 // Destructor
 ContactManifold::~ContactManifold() {
 
@@ -192,6 +207,7 @@ void ContactManifold::clearObsoleteContactPoints() {
 // This is based on the technique described by Dirk Gregorius in his
 // "Contacts Creation" GDC presentation. This method will reduce the number of
 // contact points to a maximum of 4 points (but it can be less).
+// TODO : REMOVE THIS METHOD
 void ContactManifold::reduce(const Transform& shape1ToWorldTransform) {
 
     assert(mContactPoints != nullptr);

src/collision/ContactManifold.h

@@ -100,6 +100,14 @@ class ContactManifold {
 
         // -------------------- Attributes -------------------- //
 
+        // TODO : For each of the attributes, check if we need to keep it or not
+
+        /// Reference to the memory allocator
+        MemoryAllocator& mMemoryAllocator;
+
+        /// Index of the first contact point of the manifold in the list of contact points
+        uint mContactPointsIndex;
+
         /// Pointer to the first proxy shape of the contact
         ProxyShape* mShape1;
 
@@ -133,9 +141,6 @@ class ContactManifold {
         /// True if the contact manifold has already been added into an island
         bool mIsAlreadyInIsland;
 
-        /// Reference to the memory allocator
-        MemoryAllocator& mMemoryAllocator;
-
         /// Pointer to the next contact manifold in the linked-list
         ContactManifold* mNext;
 
@@ -224,14 +229,17 @@ class ContactManifold {
         ContactManifold(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator,
                         const WorldSettings& worldSettings);
 
+        /// Constructor
+        ContactManifold(uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings);
+
         /// Destructor
         ~ContactManifold();
 
-        /// Deleted copy-constructor
+        /// Copy-constructor
-        ContactManifold(const ContactManifold& contactManifold) = delete;
+        ContactManifold(const ContactManifold& contactManifold) = default;
 
-        /// Deleted assignment operator
+        /// Assignment operator
-        ContactManifold& operator=(const ContactManifold& contactManifold) = delete;
+        ContactManifold& operator=(const ContactManifold& contactManifold) = default;
 
         /// Return a pointer to the first proxy shape of the contact
         ProxyShape* getShape1() const;
@@ -264,6 +272,7 @@ class ContactManifold {
         friend class CollisionBody;
         friend class ContactManifoldSet;
         friend class ContactSolver;
+        friend class CollisionDetection;
 };
 
 // Return a pointer to the first proxy shape of the contact

src/collision/ContactManifoldInfo.h

@@ -0,0 +1,67 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
+#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+#include "engine/OverlappingPair.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactManifoldInfo
+/**
+ * This structure contains informations about a collision contact
+ * manifold computed during the narrow-phase collision detection.
+ */
+struct ContactManifoldInfo {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Indices of the contact points in the mPotentialContactPoints array
+        List<uint> potentialContactPointsIndices;
+
+        /// Overlapping pair id
+        OverlappingPair::OverlappingPairId pairId;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifoldInfo(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
+               : potentialContactPointsIndices(allocator), pairId(pairId) {
+
+        }
+
+
+};
+
+}
+
+#endif

src/collision/ContactManifoldSet.cpp

@@ -110,8 +110,12 @@ int ContactManifoldSet::getTotalNbContactPoints() const {
 }
 
 // Return the maximum number of contact manifolds allowed between to collision shapes
+// TODO : Remove this method
 int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
 
+    return mWorldSettings.nbMaxContactManifolds;
+
+    /*
     // If both shapes are convex
     if (shape1->isConvex() && shape2->isConvex()) {
         return mWorldSettings.nbMaxContactManifoldsConvexShape;
@@ -120,6 +124,7 @@ int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape
     else {
         return mWorldSettings.nbMaxContactManifoldsConcaveShape;
     }
+    */
 }
 
 // Remove a contact manifold that is the least optimal (smaller penetration depth)

src/collision/ContactManifoldSet.h

@@ -33,7 +33,6 @@ namespace reactphysics3d {
 
 // Declarations
 class ContactManifold;
-class ContactManifoldInfo;
 class ProxyShape;
 class MemoryAllocator;
 struct WorldSettings;

src/collision/ContactPair.h

@@ -0,0 +1,77 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_OVERLAPPING_PAIR_CONTACT_H
+#define REACTPHYSICS3D_OVERLAPPING_PAIR_CONTACT_H
+
+// Libraries
+#include "mathematics/mathematics.h"
+#include "configuration.h"
+#include "engine/OverlappingPair.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Structure ContactPair
+/**
+ * This structure represents a pair of shapes that are in contact during narrow-phase.
+ */
+struct ContactPair {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Overlapping pair Id
+        OverlappingPair::OverlappingPairId pairId;
+
+        /// Indices of the potential contact manifolds
+        List<uint> potentialContactManifoldsIndices;
+
+        /// Index of the first contact manifold in the array
+        uint contactManifoldsIndex;
+
+        /// Number of contact manifolds
+        int8 nbContactManifolds;
+
+        /// Index of the first contact point in the array of contact points
+        uint contactPointsIndex;
+
+        /// Total number of contact points in all the manifolds of the contact pair
+        uint nbToTalContactPoints;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactPair(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
+            : pairId(pairId), potentialContactManifoldsIndices(allocator), contactManifoldsIndex(0), nbContactManifolds(0),
+              contactPointsIndex(0), nbToTalContactPoints(0) {
+
+        }
+};
+
+}
+
+#endif

src/collision/ContactPointInfo.h

@@ -65,9 +65,11 @@ struct ContactPointInfo {
         /// Contact point of body 2 in local space of body 2
         Vector3 localPoint2;
 
+        // TODO : Remove this field
         /// Pointer to the next contact point info
         ContactPointInfo* next;
 
+        // TODO : Remove this field
         /// True if the contact point has already been inserted into a manifold
         bool isUsed;
 

src/configuration.h

@@ -154,13 +154,8 @@ struct WorldSettings {
     /// might enter sleeping mode
     decimal defaultSleepAngularVelocity = decimal(3.0) * (PI / decimal(180.0));
 
-    /// Maximum number of contact manifolds in an overlapping pair that involves two
+    /// Maximum number of contact manifolds in an overlapping pair
-    /// convex collision shapes.
+    uint nbMaxContactManifolds = 3;
-    int nbMaxContactManifoldsConvexShape = 1;
-
-    /// Maximum number of contact manifolds in an overlapping pair that involves at
-    /// least one concave collision shape.
-    int nbMaxContactManifoldsConcaveShape = 3;
 
     /// 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
@@ -184,8 +179,7 @@ struct WorldSettings {
         ss << "defaultTimeBeforeSleep=" << defaultTimeBeforeSleep << std::endl;
         ss << "defaultSleepLinearVelocity=" << defaultSleepLinearVelocity << std::endl;
         ss << "defaultSleepAngularVelocity=" << defaultSleepAngularVelocity << std::endl;
-        ss << "nbMaxContactManifoldsConvexShape=" << nbMaxContactManifoldsConvexShape << std::endl;
+        ss << "nbMaxContactManifolds=" << nbMaxContactManifolds << std::endl;
-        ss << "nbMaxContactManifoldsConcaveShape=" << nbMaxContactManifoldsConcaveShape << std::endl;
         ss << "cosAngleSimilarContactManifold=" << cosAngleSimilarContactManifold << std::endl;
 
         return ss.str();

src/constraint/ContactPoint.cpp

@@ -45,6 +45,21 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const WorldSetti
     mIsObsolete = false;
 }
 
+// Constructor
+ContactPoint::ContactPoint(const ContactPointInfo& contactInfo, const WorldSettings& worldSettings)
+             : mNormal(contactInfo.normal),
+               mPenetrationDepth(contactInfo.penetrationDepth),
+               mLocalPointOnShape1(contactInfo.localPoint1),
+               mLocalPointOnShape2(contactInfo.localPoint2),
+               mIsRestingContact(false), mIsObsolete(false),
+               mWorldSettings(worldSettings) {
+
+    assert(mPenetrationDepth > decimal(0.0));
+    assert(mNormal.lengthSquare() > decimal(0.8));
+
+    mIsObsolete = false;
+}
+
 // Update the contact point with a new one that is similar (very close)
 /// The idea is to keep the cache impulse (for warm starting the contact solver)
 void ContactPoint::update(const ContactPointInfo* contactInfo) {

src/constraint/ContactPoint.h

@@ -112,17 +112,20 @@ class ContactPoint {
         /// Constructor
         ContactPoint(const ContactPointInfo* contactInfo, const WorldSettings& worldSettings);
 
+        /// Constructor
+        ContactPoint(const ContactPointInfo& contactInfo, const WorldSettings& worldSettings);
+
         /// Destructor
         ~ContactPoint() = default;
 
-        /// Deleted copy-constructor
+        /// Copy-constructor
-        ContactPoint(const ContactPoint& contact) = delete;
+        ContactPoint(const ContactPoint& contact) = default;
 
-        /// Deleted assignment operator
+        /// Assignment operator
-        ContactPoint& operator=(const ContactPoint& contact) = delete;
+        ContactPoint& operator=(const ContactPoint& contact) = default;
 
         /// Return the normal vector of the contact
-        Vector3 getNormal() const;
+        const Vector3& getNormal() const;
 
         /// Return the contact point on the first proxy shape in the local-space of the proxy shape
         const Vector3& getLocalPointOnShape1() const;
@@ -152,13 +155,14 @@ class ContactPoint {
         friend class ContactManifold;
         friend class ContactManifoldSet;
         friend class ContactSolver;
+        friend class CollisionDetection;
 };
 
 // Return the normal vector of the contact
 /**
  * @return The contact normal
  */
-inline Vector3 ContactPoint::getNormal() const {
+inline const Vector3& ContactPoint::getNormal() const {
     return mNormal;
 }
 

src/engine/OverlappingPair.h

@@ -146,6 +146,9 @@ class OverlappingPair {
         /// Deleted assignment operator
         OverlappingPair& operator=(const OverlappingPair& pair) = delete;
 
+        /// Return the Id of the pair
+        OverlappingPairId getId() const;
+
         /// Return the pointer to first proxy collision shape
         ProxyShape* getShape1() const;
 
@@ -202,6 +205,11 @@ class OverlappingPair {
         friend class DynamicsWorld;
 };
 
+// Return the Id of the pair
+inline OverlappingPair::OverlappingPairId OverlappingPair::getId() const {
+    return mPairID;
+}
+
 // Return the pointer to first body
 inline ProxyShape* OverlappingPair::getShape1() const {
     return mContactManifoldSet.getShape1();

src/memory/DefaultAllocator.h

@@ -50,6 +50,10 @@ class DefaultAllocator : public MemoryAllocator {
         /// Allocate memory of a given size (in bytes) and return a pointer to the
         /// allocated memory.
         virtual void* allocate(size_t size) override {
+
+            // TODO : Make sure to reduce the calls to default allocator is not called within a frame. For instance by a call
+            //        to a pool allocator with size too large
+
             return malloc(size);
         }