Refactor the way to create the contact manifolds and contact points

CMakeLists.txt

@@ -132,6 +132,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/collision/CollisionDetection.h"
     "src/collision/CollisionDetection.cpp"
     "src/collision/NarrowPhaseInfo.h"
+    "src/collision/NarrowPhaseInfo.cpp"
     "src/collision/ContactManifold.h"
     "src/collision/ContactManifold.cpp"
     "src/collision/ContactManifoldSet.h"
@@ -167,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES
     "src/engine/Profiler.cpp"
     "src/engine/Timer.h"
     "src/engine/Timer.cpp"
+    "src/collision/CollisionCallback.h"
+    "src/collision/CollisionCallback.cpp"
+    "src/collision/OverlapCallback.h"
     "src/mathematics/mathematics.h"
     "src/mathematics/mathematics_functions.h"
     "src/mathematics/mathematics_functions.cpp"

src/body/CollisionBody.cpp

@@ -177,7 +177,7 @@ void CollisionBody::resetContactManifoldsList() {
     // Delete the linked list of contact manifolds of that body
     ContactManifoldListElement* currentElement = mContactManifoldsList;
     while (currentElement != nullptr) {
-        ContactManifoldListElement* nextElement = currentElement->next;
+        ContactManifoldListElement* nextElement = currentElement->getNext();
 
         // Delete the current element
         currentElement->~ContactManifoldListElement();
@@ -272,8 +272,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
     // this body
     ContactManifoldListElement* currentElement = mContactManifoldsList;
     while (currentElement != nullptr) {
-        currentElement->contactManifold->mIsAlreadyInIsland = false;
+        currentElement->getContactManifold()->mIsAlreadyInIsland = false;
-        currentElement = currentElement->next;
+        currentElement = currentElement->getNext();
         nbManifolds++;
     }
 

src/collision/CollisionCallback.cpp

@@ -0,0 +1,79 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "collision/CollisionCallback.h"
+#include "engine/OverlappingPair.h"
+#include "memory/Allocator.h"
+#include "collision/ContactManifold.h"
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator) :
+    contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
+    body2(pair->getShape2()->getBody()),
+    proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
+    mMemoryAllocator(allocator) {
+
+    assert(pair != nullptr);
+
+    const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
+
+    // For each contact manifold in the set of manifolds in the pair
+    ContactManifold* contactManifold = manifoldSet.getContactManifolds();
+    while (contactManifold != nullptr) {
+
+        assert(contactManifold->getNbContactPoints() > 0);
+
+        // Add the contact manifold at the beginning of the linked
+        // list of contact manifolds of the first body
+        ContactManifoldListElement* element = new (mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)))
+                                                      ContactManifoldListElement(contactManifold,
+                                                                         contactManifoldElements);
+        contactManifoldElements = element;
+
+        contactManifold = contactManifold->getNext();
+    }
+}
+
+// Destructor
+CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
+
+    // Release memory allocator for the contact manifold list elements
+    ContactManifoldListElement* element = contactManifoldElements;
+    while (element != nullptr) {
+
+        ContactManifoldListElement* nextElement = element->getNext();
+
+        // Delete and release memory
+        element->~ContactManifoldListElement();
+        mMemoryAllocator.release(element, sizeof(ContactManifoldListElement));
+
+        element = nextElement;
+    }
+}
+

src/collision/CollisionCallback.h

@@ -0,0 +1,89 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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_COLLISION_CALLBACK_H
+#define REACTPHYSICS3D_COLLISION_CALLBACK_H
+
+// Libraries
+#include "collision/ContactManifold.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class CollisionCallback
+/**
+ * This class can be used to register a callback for collision test queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyContact() method. This method will called each time a contact
+ * point is reported.
+ */
+class CollisionCallback {
+
+    public:
+
+        // Structure CollisionCallbackInfo
+        /**
+         * This structure represents the contact information between two collision
+         * shapes of two bodies
+         */
+        struct CollisionCallbackInfo {
+
+            public:
+
+                /// Pointer to the first element of the linked-list that contains contact manifolds
+                ContactManifoldListElement* contactManifoldElements;
+
+                /// Pointer to the first body of the contact
+                CollisionBody* body1;
+
+                /// Pointer to the second body of the contact
+                CollisionBody* body2;
+
+                /// Pointer to the proxy shape of first body
+                const ProxyShape* proxyShape1;
+
+                /// Pointer to the proxy shape of second body
+                const ProxyShape* proxyShape2;
+
+                /// Memory allocator
+                Allocator& mMemoryAllocator;
+
+                // Constructor
+                CollisionCallbackInfo(OverlappingPair* pair, Allocator& allocator);
+
+                // Destructor
+                ~CollisionCallbackInfo();
+        };
+
+        /// Destructor
+        virtual ~CollisionCallback() = default;
+
+        /// This method will be called for each reported contact point
+        virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
+};
+
+}
+
+#endif

src/collision/CollisionDetection.cpp

@@ -26,10 +26,13 @@
 // Libraries
 #include "CollisionDetection.h"
 #include "engine/CollisionWorld.h"
+#include "collision/OverlapCallback.h"
 #include "body/Body.h"
 #include "collision/shapes/BoxShape.h"
 #include "body/RigidBody.h"
 #include "configuration.h"
+#include "collision/CollisionCallback.h"
+#include "collision/OverlapCallback.h"
 #include <cassert>
 #include <complex>
 #include <set>
@@ -43,7 +46,7 @@ using namespace std;
 
 // Constructor
 CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator, SingleFrameAllocator& singleFrameAllocator)
-                   : mMemoryAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
+                   : mPoolAllocator(memoryAllocator), mSingleFrameAllocator(singleFrameAllocator),
                      mWorld(world), mNarrowPhaseInfoList(nullptr), mBroadPhaseAlgorithm(*this),
                      mIsCollisionShapesAdded(false) {
 
@@ -83,7 +86,7 @@ void CollisionDetection::computeBroadPhase() {
         // Ask the broad-phase to recompute the overlapping pairs of collision
         // shapes. This call can only add new overlapping pairs in the collision
         // detection.
-        mBroadPhaseAlgorithm.computeOverlappingPairs(mMemoryAllocator);
+        mBroadPhaseAlgorithm.computeOverlappingPairs(mPoolAllocator);
     }
 }
 
@@ -101,25 +104,24 @@ void CollisionDetection::computeMiddlePhase() {
 
         OverlappingPair* pair = it->second;
 
+        // Make all the contact manifolds and contact points of the pair obselete
+        pair->makeContactsObselete();
+
         ProxyShape* shape1 = pair->getShape1();
         ProxyShape* shape2 = pair->getShape2();
 
         assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
 
-        // Check if the collision filtering allows collision between the two shapes and
+        // Check if the two shapes are still overlapping. Otherwise, we destroy the
-        // that the two shapes are still overlapping. Otherwise, we destroy the
         // overlapping pair
-        if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
+        if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
-             (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
-             !mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
 
             std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
             ++it;
 
-            // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
-
             // Destroy the overlapping pair
             itToRemove->second->~OverlappingPair();
+
             mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
             mOverlappingPairs.erase(itToRemove);
             continue;
@@ -128,6 +130,10 @@ void CollisionDetection::computeMiddlePhase() {
             ++it;
         }
 
+        // Check if the collision filtering allows collision between the two shapes
+        if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
+             (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) {
+
             CollisionBody* const body1 = shape1->getBody();
             CollisionBody* const body2 = shape2->getBody();
 
@@ -181,6 +187,7 @@ void CollisionDetection::computeMiddlePhase() {
             }
         }
     }
+}
 
 // Compute the concave vs convex middle-phase algorithm for a given pair of bodies
 void CollisionDetection::computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, Allocator& allocator,
@@ -231,7 +238,7 @@ void CollisionDetection::computeNarrowPhase() {
 
     PROFILE("CollisionDetection::computeNarrowPhase()");
 
-    const NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
+    NarrowPhaseInfo* currentNarrowPhaseInfo = mNarrowPhaseInfoList;
     while (currentNarrowPhaseInfo != nullptr) {
 
         // Select the narrow phase algorithm to use according to the two collision shapes
@@ -245,30 +252,16 @@ void CollisionDetection::computeNarrowPhase() {
             // Use the narrow-phase collision detection algorithm to check
             // if there really is a collision. If a collision occurs, the
             // notifyContact() callback method will be called.
-            ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
+            if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, true)) {
-            if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
 
-                // Reduce the number of points in the contact manifold (if necessary)
+                // Add the contact points as a potential contact manifold into the pair                
-                contactManifoldInfo.reduce(currentNarrowPhaseInfo->shape1ToWorldTransform);
+                currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold();
-
-                // If it is the first contact since the pairs are overlapping
-                if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
-
-                    // Trigger a callback event
-                    if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo);
-                }
-
-                // Add the contact manifold to the corresponding overlapping pair
-                currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo);
 
                 // Add the overlapping pair into the set of pairs in contact during narrow-phase
                 overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
                                                                       currentNarrowPhaseInfo->overlappingPair->getShape2());
                 mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
 
-                // Trigger a callback event for the new contact
-                if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
-
                 currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
             }
             else {
@@ -282,8 +275,14 @@ void CollisionDetection::computeNarrowPhase() {
         currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
     }
 
+    // Convert the potential contact into actual contacts
+    processAllPotentialContacts();
+
     // Add all the contact manifolds (between colliding bodies) to the bodies
     addAllContactManifoldsToBodies();
+
+    // Report contacts to the user
+    reportAllContacts();
 }
 
 // Allow the broadphase to notify the collision detection about an overlapping pair.
@@ -302,13 +301,9 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
     // Check if the overlapping pair already exists
     if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
 
-    // Compute the maximum number of contact manifolds for this pair
-    int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
-                                                                      shape2->getCollisionShape()->getType());
-
     // Create the overlapping pair and add it into the set of overlapping pairs
-    OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair)))
+    OverlappingPair* newPair = new (mPoolAllocator.allocate(sizeof(OverlappingPair)))
-                              OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator);
+                              OverlappingPair(shape1, shape2, mPoolAllocator, mSingleFrameAllocator);
     assert(newPair != nullptr);
 
 #ifndef NDEBUG
@@ -372,40 +367,173 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
     const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
 
     // For each contact manifold in the set of manifolds in the pair
-    for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
+    ContactManifold* contactManifold = manifoldSet.getContactManifolds();
-
+    while (contactManifold != nullptr) {
-        ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
 
         assert(contactManifold->getNbContactPoints() > 0);
 
         // Add the contact manifold at the beginning of the linked
         // list of contact manifolds of the first body
-        void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
+        ContactManifoldListElement* listElement1 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
-        ContactManifoldListElement* listElement1 = new (allocatedMemory1)
                                                       ContactManifoldListElement(contactManifold,
                                                                          body1->mContactManifoldsList);
         body1->mContactManifoldsList = listElement1;
 
         // Add the contact manifold at the beginning of the linked
         // list of the contact manifolds of the second body
-        void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement));
+        ContactManifoldListElement* listElement2 = new (mPoolAllocator.allocate(sizeof(ContactManifoldListElement)))
-        ContactManifoldListElement* listElement2 = new (allocatedMemory2)
                                                       ContactManifoldListElement(contactManifold,
                                                                          body2->mContactManifoldsList);
         body2->mContactManifoldsList = listElement2;
+
+        contactManifold = contactManifold->getNext();
     }
 }
 
-// Delete all the contact points in the currently overlapping pairs
+/// Convert the potential contact into actual contacts
-void CollisionDetection::clearContactPoints() {
+void CollisionDetection::processAllPotentialContacts() {
 
-    // For each overlapping pair
+    // For each overlapping pairs in contact during the narrow-phase
     std::map<overlappingpairid, OverlappingPair*>::iterator it;
-    for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
+    for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
-        it->second->clearContactPoints();
+
+        // Process the potential contacts of the overlapping pair
+        processPotentialContacts(it->second);
     }
 }
 
+// Process the potential contact manifold of a pair to create actual contact manifold
+void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
+
+    // Reduce the number of contact points of the manifold
+    pair->reducePotentialContactManifolds();
+
+    // If there is a concave mesh shape in the pair
+    if (pair->hasConcaveShape()) {
+
+        processSmoothMeshContacts(pair);
+    }
+    else {   // If both collision shapes are convex
+
+        // Add all the potential contact manifolds as actual contact manifolds to the pair
+        ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
+        while (potentialManifold != nullptr) {
+
+             pair->addContactManifold(potentialManifold);
+
+             potentialManifold = potentialManifold->mNext;
+        }
+    }
+
+    // Clear the obselete contact manifolds and contact points
+    pair->clearObseleteManifoldsAndContactPoints();
+
+    // Reset the potential contacts of the pair
+    pair->clearPotentialContactManifolds();
+}
+
+// Report contacts for all the colliding overlapping pairs
+void CollisionDetection::reportAllContacts() {
+
+    // For each overlapping pairs in contact during the narrow-phase
+    std::map<overlappingpairid, OverlappingPair*>::iterator it;
+    for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
+
+        // If there is a user callback
+        if (mWorld->mEventListener != nullptr) {
+
+            CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
+
+            // Trigger a callback event to report the new contact to the user
+             mWorld->mEventListener->newContact(collisionInfo);
+        }
+    }
+}
+
+// Process the potential contacts where one collion is a concave shape.
+// This method processes the concave triangle mesh collision using the smooth mesh collision algorithm described
+// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
+// issue with some internal edges.
+void CollisionDetection::processSmoothMeshContacts(OverlappingPair* pair) {
+
+//    // Set with the triangle vertices already processed to void further contacts with same triangle
+//    std::unordered_multimap<int, Vector3> processTriangleVertices;
+
+//    std::vector<SmoothMeshContactInfo*> smoothContactPoints;
+
+//    // If the collision shape 1 is the triangle
+//    bool isFirstShapeTriangle = pair->getShape1()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE;
+//    assert(isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() == CollisionShapeType::TRIANGLE);
+//    assert(!isFirstShapeTriangle || pair->getShape2()->getCollisionShape()->getType() != CollisionShapeType::TRIANGLE);
+
+//    const TriangleShape* triangleShape = nullptr;
+//    if (isFirstShapeTriangle) {
+//        triangleShape = static_cast<const TriangleShape*>(pair->getShape1()->getCollisionShape());
+//    }
+//    else {
+//        triangleShape = static_cast<const TriangleShape*>(pair->getShape2()->getCollisionShape());
+//    }
+//    assert(triangleShape != nullptr);
+
+//    // Get the temporary memory allocator
+//    Allocator& allocator = pair->getTemporaryAllocator();
+
+//    // For each potential contact manifold of the pair
+//    ContactManifoldInfo* potentialManifold = pair->getPotentialContactManifolds();
+//    while (potentialManifold != nullptr) {
+
+//        // For each contact point of the potential manifold
+//        ContactPointInfo* contactPointInfo = potentialManifold->getFirstContactPointInfo();
+//        while (contactPointInfo != nullptr) {
+
+//            // Compute the barycentric coordinates of the point in the triangle
+//            decimal u, v, w;
+//            computeBarycentricCoordinatesInTriangle(triangleShape->getVertexPosition(0),
+//                                                    triangleShape->getVertexPosition(1),
+//                                                    triangleShape->getVertexPosition(2),
+//                                                    isFirstShapeTriangle ? contactPointInfo->localPoint1 : contactPointInfo->localPoint2,
+//                                                    u, v, w);
+//            int nbZeros = 0;
+//            bool isUZero = approxEqual(u, 0, 0.0001);
+//            bool isVZero = approxEqual(v, 0, 0.0001);
+//            bool isWZero = approxEqual(w, 0, 0.0001);
+//            if (isUZero) nbZeros++;
+//            if (isVZero) nbZeros++;
+//            if (isWZero) nbZeros++;
+
+//            // If the triangle contact point is on a triangle vertex of a triangle edge
+//            if (nbZeros == 1 || nbZeros == 2) {
+
+
+//                // Create a smooth mesh contact info
+//                SmoothMeshContactInfo* smoothContactInfo = new (allocator.allocate(sizeof(SmoothMeshContactInfo)))
+//                                                           SmoothMeshContactInfo(potentialManifold, contactInfo, isFirstShapeTriangle,
+//                                                                                 triangleShape->getVertexPosition(0),
+//                                                                                 triangleShape->getVertexPosition(1),
+//                                                                                 triangleShape->getVertexPosition(2));
+
+//                smoothContactPoints.push_back(smoothContactInfo);
+
+//                // Remove the contact point info from the manifold. If the contact point will be kept in the future, we
+//                // will put the contact point back in the manifold.
+//                ...
+//            }
+
+//            // Note that we do not remove the contact points that are not on the vertices or edges of the triangle
+//            // from the contact manifold because we know we will keep to contact points. We only remove the vertices
+//            // and edges contact points for the moment. If those points will be kept in the future, we will have to
+//            // put them back again in the contact manifold
+//        }
+
+//        potentialManifold = potentialManifold->mNext;
+//    }
+
+//    // Sort the list of narrow-phase contacts according to their penetration depth
+//    std::sort(smoothContactPoints.begin(), smoothContactPoints.end(), ContactsDepthCompare());
+
+//    ...
+}
+
 // Compute the middle-phase collision detection between two proxy shapes
 NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(OverlappingPair* pair) {
 
@@ -424,7 +552,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
 
         // No middle-phase is necessary, simply create a narrow phase info
         // for the narrow-phase collision detection
-        narrowPhaseInfo = new (mMemoryAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
+        narrowPhaseInfo = new (mPoolAllocator.allocate(sizeof(NarrowPhaseInfo))) NarrowPhaseInfo(pair, shape1->getCollisionShape(),
                                        shape2->getCollisionShape(), shape1->getLocalToWorldTransform(),
                                        shape2->getLocalToWorldTransform(), shape1->getCachedCollisionData(),
                                        shape2->getCachedCollisionData());
@@ -436,7 +564,7 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin
 
         // Run the middle-phase collision detection algorithm to find the triangles of the concave
         // shape we need to use during the narrow-phase collision detection
-        computeConvexVsConcaveMiddlePhase(pair, mMemoryAllocator, &narrowPhaseInfo);
+        computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo);
     }
 
     return narrowPhaseInfo;
@@ -450,7 +578,7 @@ void CollisionDetection::testAABBOverlap(const AABB& aabb, OverlapCallback* over
     std::unordered_set<bodyindex> reportedBodies;
 
     // Ask the broad-phase to get all the overlapping shapes
-    LinkedList<int> overlappingNodes(mMemoryAllocator);
+    LinkedList<int> overlappingNodes(mPoolAllocator);
     mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(aabb, overlappingNodes);
 
     // For each overlaping proxy shape
@@ -504,7 +632,7 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
                 const CollisionShapeType shape2Type = body2ProxyShape->getCollisionShape()->getType();
 
                 // Create a temporary overlapping pair
-                OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
+                OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
 
                 // Compute the middle-phase collision detection between the two shapes
                 NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -526,16 +654,18 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
                             // Use the narrow-phase collision detection algorithm to check
                             // if there really is a collision. If a collision occurs, the
                             // notifyContact() callback method will be called.
-                            ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+                            isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
-                            isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
                         }
                     }
 
                     NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
                     narrowPhaseInfo = narrowPhaseInfo->next;
 
+                    // Call the destructor
+                    currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
                     // Release the allocated memory
-                    mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                    mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
                 }
 
                 // Return if we have found a narrow-phase collision
@@ -570,7 +700,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
         const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
 
         // Ask the broad-phase to get all the overlapping shapes
-        LinkedList<int> overlappingNodes(mMemoryAllocator);
+        LinkedList<int> overlappingNodes(mPoolAllocator);
         mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
 
         const bodyindex bodyId = body->getID();
@@ -595,7 +725,7 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
                     const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
 
                     // Create a temporary overlapping pair
-                    OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
+                    OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
 
                     // Compute the middle-phase collision detection between the two shapes
                     NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -617,16 +747,18 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
                                 // Use the narrow-phase collision detection algorithm to check
                                 // if there really is a collision. If a collision occurs, the
                                 // notifyContact() callback method will be called.
-                                ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+                                isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, false);
-                                isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
                             }
                         }
 
                         NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
                         narrowPhaseInfo = narrowPhaseInfo->next;
 
+                        // Call the destructor
+                        currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
                         // Release the allocated memory
-                        mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                        mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
                     }
 
                     // Return if we have found a narrow-phase collision
@@ -673,7 +805,7 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
             if (aabb1.testCollision(aabb2)) {
 
                 // Create a temporary overlapping pair
-                OverlappingPair pair(body1ProxyShape, body2ProxyShape, 0, mMemoryAllocator);
+                OverlappingPair pair(body1ProxyShape, body2ProxyShape, mPoolAllocator, mPoolAllocator);
 
                 // Compute the middle-phase collision detection between the two shapes
                 NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -693,26 +825,29 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
                         // Use the narrow-phase collision detection algorithm to check
                         // if there really is a collision. If a collision occurs, the
                         // notifyContact() callback method will be called.
-                        ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+                        if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
-                        if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
 
-                            // Reduce the number of points in the contact manifold (if necessary)
+                            // Add the contact points as a potential contact manifold into the pair
-                            contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+                            narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
-
-                            CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
-                                                                                   body1ProxyShape, body2ProxyShape);
-
-                            // Report the contact to the user
-                            collisionCallback->notifyContact(collisionInfo);
                         }
                     }
 
                     NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
                     narrowPhaseInfo = narrowPhaseInfo->next;
 
+                    // Call the destructor
+                    currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
                     // Release the allocated memory
-                    mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                    mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
                 }
+
+                // Process the potential contacts
+                processPotentialContacts(&pair);
+
+                // Report the contacts to the user
+                CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
+                collisionCallback->notifyContact(collisionInfo);
             }
 
             // Go to the next proxy shape
@@ -737,7 +872,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
         const AABB& shapeAABB = mBroadPhaseAlgorithm.getFatAABB(bodyProxyShape->mBroadPhaseID);
 
         // Ask the broad-phase to get all the overlapping shapes
-        LinkedList<int> overlappingNodes(mMemoryAllocator);
+        LinkedList<int> overlappingNodes(mPoolAllocator);
         mBroadPhaseAlgorithm.reportAllShapesOverlappingWithAABB(shapeAABB, overlappingNodes);
 
         const bodyindex bodyId = body->getID();
@@ -760,7 +895,7 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
                     const CollisionShapeType shape2Type = proxyShape->getCollisionShape()->getType();
 
                     // Create a temporary overlapping pair
-                    OverlappingPair pair(bodyProxyShape, proxyShape, 0, mMemoryAllocator);
+                    OverlappingPair pair(bodyProxyShape, proxyShape, mPoolAllocator, mPoolAllocator);
 
                     // Compute the middle-phase collision detection between the two shapes
                     NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
@@ -777,17 +912,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
                             // Use the narrow-phase collision detection algorithm to check
                             // if there really is a collision. If a collision occurs, the
                             // notifyContact() callback method will be called.
-                            ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+                            if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
-                            if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
 
-                                // Reduce the number of points in the contact manifold (if necessary)
+                                // Add the contact points as a potential contact manifold into the pair
-                                contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+                                narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
 
-                                CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
+                                // Report the contacts to the user
-                                                                                       proxyShape->getBody(), bodyProxyShape,
+                                CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
-                                                                                       proxyShape);
-
-                                // Report the contact to the user
                                 callback->notifyContact(collisionInfo);
                             }
                         }
@@ -795,9 +926,15 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
                         NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
                         narrowPhaseInfo = narrowPhaseInfo->next;
 
+                        // Call the destructor
+                        currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
                         // Release the allocated memory
-                        mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                        mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
                     }
+
+                    // Process the potential contacts
+                    processPotentialContacts(&pair);
                 }
             }
 
@@ -822,10 +959,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
     map<overlappingpairid, OverlappingPair*>::iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
 
-        OverlappingPair* pair = it->second;
+        OverlappingPair* originalPair = it->second;
 
-        ProxyShape* shape1 = pair->getShape1();
+        // Create a new overlapping pair so that we do not work on the original one
-        ProxyShape* shape2 = pair->getShape2();
+        OverlappingPair pair(originalPair->getShape1(), originalPair->getShape2(), mPoolAllocator,
+                             mPoolAllocator);
+
+        ProxyShape* shape1 = pair.getShape1();
+        ProxyShape* shape2 = pair.getShape2();
 
         // Check if the collision filtering allows collision between the two shapes and
         // that the two shapes are still overlapping.
@@ -834,14 +975,14 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
              mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
 
             // Compute the middle-phase collision detection between the two shapes
-            NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(pair);
+            NarrowPhaseInfo* narrowPhaseInfo = computeMiddlePhaseForProxyShapes(&pair);
-
-            const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
-            const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
 
             // For each narrow-phase info object
             while (narrowPhaseInfo != nullptr) {
 
+                const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
+                const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
+
                 // Select the narrow phase algorithm to use according to the two collision shapes
                 NarrowPhaseAlgorithm* narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(shape1Type, shape2Type);
 
@@ -851,29 +992,29 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
                     // Use the narrow-phase collision detection algorithm to check
                     // if there really is a collision. If a collision occurs, the
                     // notifyContact() callback method will be called.
-                    ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
+                    if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, true)) {
-                    if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
 
-                        // Reduce the number of points in the contact manifold (if necessary)
+                        // Add the contact points as a potential contact manifold into the pair
-                        contactManifoldInfo.reduce(narrowPhaseInfo->shape1ToWorldTransform);
+                        narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
 
-                        CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
+                        // Report the contacts to the user
-                                                                               shape2->getBody(), shape1, shape2);
+                        CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
-
-                        // Report the contact to the user
                         callback->notifyContact(collisionInfo);
                     }
-
-                    // The previous frame collision info is now valid
-                    narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
                 }
 
                 NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;
                 narrowPhaseInfo = narrowPhaseInfo->next;
 
+                // Call the destructor
+                currentNarrowPhaseInfo->~NarrowPhaseInfo();
+
                 // Release the allocated memory
-                mMemoryAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
+                mPoolAllocator.release(currentNarrowPhaseInfo, sizeof(NarrowPhaseInfo));
             }
+
+            // Process the potential contacts
+            processPotentialContacts(&pair);
         }
     }
 }

src/collision/CollisionDetection.h

@@ -72,7 +72,7 @@ class CollisionDetection {
         NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
 
         /// Reference to the memory allocator
-        PoolAllocator& mMemoryAllocator;
+        PoolAllocator& mPoolAllocator;
 
         /// Reference to the single frame memory allocator
         SingleFrameAllocator& mSingleFrameAllocator;
@@ -118,9 +118,6 @@ class CollisionDetection {
         /// involed in the corresponding contact.
         void addContactManifoldToBody(OverlappingPair* pair);
 
-        /// Delete all the contact points in the currently overlapping pairs
-        void clearContactPoints();
-
         /// Fill-in the collision detection matrix
         void fillInCollisionMatrix();
 
@@ -138,6 +135,18 @@ class CollisionDetection {
         /// Compute the middle-phase collision detection between two proxy shapes
         NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair);
 
+        /// Convert the potential contact into actual contacts
+        void processAllPotentialContacts();
+
+        /// Process the potential contact manifold of a pair to create actual contact manifold
+        void processPotentialContacts(OverlappingPair* pair);
+
+        /// Report contacts for all the colliding overlapping pairs
+        void reportAllContacts();
+
+        /// Process the potential contacts where one collion is a concave shape
+        void processSmoothMeshContacts(OverlappingPair* pair);
+   
     public :
 
         // -------------------- Methods -------------------- //

src/collision/ContactManifold.cpp

@@ -30,15 +30,14 @@
 using namespace reactphysics3d;
 
 // Constructor
-ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
+ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, Allocator& memoryAllocator)
-                                 PoolAllocator& memoryAllocator, short normalDirectionId)
+                : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
-                : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
                   mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
                   mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
-                  mMemoryAllocator(memoryAllocator) {
+                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObselete(false) {
     
     // For each contact point info in the manifold
-    const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo();
+    const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
     while(pointInfo != nullptr) {
 
         // Create the new contact point
@@ -46,7 +45,8 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS
                 ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
 
         // Add the new contact point into the manifold
-        mContactPoints[mNbContactPoints] = contact;
+        contact->setNext(mContactPoints);
+        mContactPoints = contact;
         mNbContactPoints++;
 
         pointInfo = pointInfo->next;
@@ -58,49 +58,69 @@ ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyS
 
 // Destructor
 ContactManifold::~ContactManifold() {
-    clear();
-}
 
-// Clear the contact manifold
+    // Delete all the contact points
-void ContactManifold::clear() {
+    ContactPoint* contactPoint = mContactPoints;
-    for (uint i=0; i<mNbContactPoints; i++) {
+    while(contactPoint != nullptr) {
 
-        // Call the destructor explicitly and tell the memory allocator that
+        ContactPoint* nextContactPoint = contactPoint->getNext();
-		// the corresponding memory block is now free
+
-        mContactPoints[i]->~ContactPoint();
+        // TODO : Delete this
-        mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
+        bool test = mMemoryAllocator.isReleaseNeeded();
+
+        // Delete the contact point
+        contactPoint->~ContactPoint();
+        mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
+
+        contactPoint = nextContactPoint;
     }
-    mNbContactPoints = 0;
 }
 
 // Add a contact point
 void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
 
-    assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD);
-
     // Create the new contact point
     ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
             ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
 
     // Add the new contact point into the manifold
-    mContactPoints[mNbContactPoints] = contactPoint;
+    contactPoint->setNext(mContactPoints);
-    mNbContactPoints++;
+    mContactPoints = contactPoint;
 
+    mNbContactPoints++;
 }
 
-// Remove a contact point
+// Clear the obselete contact points
-void ContactManifold::removeContactPoint(int index) {
+void ContactManifold::clearObseleteContactPoints() {
 
-    assert(mNbContactPoints > 0);
+    ContactPoint* contactPoint = mContactPoints;
-    assert(index >= 0 && index < mNbContactPoints);
+    ContactPoint* previousContactPoint = nullptr;
+    while (contactPoint != nullptr) {
 
-    // Delete the contact
+        ContactPoint* nextContactPoint =  contactPoint->getNext();
-    mContactPoints[index]->~ContactPoint();
-    mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
 
-    for (int i=index; (i+1) < mNbContactPoints; i++) {
+        if (contactPoint->getIsObselete()) {
-        mContactPoints[i] = mContactPoints[i+1];
+
+            // Delete the contact point
+            contactPoint->~ContactPoint();
+            mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
+
+            if (previousContactPoint != nullptr) {
+                previousContactPoint->setNext(nextContactPoint);
+            }
+            else {
+                mContactPoints = nextContactPoint;
             }
 
             mNbContactPoints--;
         }
+        else {
+            previousContactPoint = contactPoint;
+        }
+
+        contactPoint = nextContactPoint;
+    }
+
+    assert(mNbContactPoints >= 0);
+    assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+}

src/collision/ContactManifold.h

@@ -46,40 +46,43 @@ class ContactManifold;
  */
 struct ContactManifoldListElement {
 
-    public:
+    private:
 
         // -------------------- Attributes -------------------- //
 
-        /// Pointer to the actual contact manifold
+        /// Pointer to a contact manifold with contact points
-        ContactManifold* contactManifold;
+        ContactManifold* mContactManifold;
 
         /// Next element of the list
-        ContactManifoldListElement* next;
+        ContactManifoldListElement* mNext;
+
+   public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifoldListElement(ContactManifold* initContactManifold,
+        ContactManifoldListElement(ContactManifold* contactManifold,
-                                   ContactManifoldListElement* initNext)
+                                   ContactManifoldListElement* next)
-                                  :contactManifold(initContactManifold), next(initNext) {
+                                  :mContactManifold(contactManifold), mNext(next) {
 
         }
+
+        /// Return the contact manifold
+        ContactManifold* getContactManifold() {
+            return mContactManifold;
+        }
+
+        /// Return the next element in the linked-list
+        ContactManifoldListElement* getNext() {
+            return mNext;
+        }
 };
 
 // Class ContactManifold
 /**
  * This class represents the set of contact points between two bodies.
  * The contact manifold is implemented in a way to cache the contact
- * points among the frames for better stability following the
+ * points among the frames for better stability.
- * "Contact Generation" presentation of Erwin Coumans at GDC 2010
- * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
- * Some code of this class is based on the implementation of the
- * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
- * The contacts between two bodies are added one after the other in the cache.
- * When the cache is full, we have to remove one point. The idea is to keep
- * the point with the deepest penetration depth and also to keep the
- * points producing the larger area (for a more stable contact manifold).
- * The new added point is always kept.
  */
 class ContactManifold {
 
@@ -94,10 +97,10 @@ class ContactManifold {
         ProxyShape* mShape2;
 
         /// Contact points in the manifold
-        ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
+        ContactPoint* mContactPoints;
 
         /// Normal direction Id (Unique Id representing the normal direction)
-        short int mNormalDirectionId;
+        short int mContactNormalId;
 
         /// Number of contacts in the cache
         int8 mNbContactPoints;
@@ -124,20 +127,86 @@ class ContactManifold {
         bool mIsAlreadyInIsland;
 
         /// Reference to the memory allocator
-        PoolAllocator& mMemoryAllocator;
+        Allocator& mMemoryAllocator;
+
+        /// Pointer to the next contact manifold in the linked-list
+        ContactManifold* mNext;
+
+        /// Pointer to the previous contact manifold in linked-list
+        ContactManifold* mPrevious;
+
+        /// True if the contact manifold is obselete
+        bool mIsObselete;
 
         // -------------------- Methods -------------------- //
 
         /// Return true if the contact manifold has already been added into an island
         bool isAlreadyInIsland() const;
 
+        /// Set the pointer to the next element in the linked-list
+        void setNext(ContactManifold* nextManifold);
+
+        /// Return true if the manifold is obselete
+        bool getIsObselete() const;
+
+        /// Set to true to make the manifold obselete
+        void setIsObselete(bool isObselete, bool setContactPoints);
+
+        /// Clear the obselete contact points
+        void clearObseleteContactPoints();
+
+        /// Return the contact normal direction Id of the manifold
+        short getContactNormalId() const;
+
+        /// Return the largest depth of all the contact points
+        decimal getLargestContactDepth() const;
+
+        /// set the first friction vector at the center of the contact manifold
+        void setFrictionVector1(const Vector3& mFrictionVector1);
+
+        /// set the second friction vector at the center of the contact manifold
+        void setFrictionVector2(const Vector3& mFrictionVector2);
+
+        /// Set the first friction accumulated impulse
+        void setFrictionImpulse1(decimal frictionImpulse1);
+
+        /// Set the second friction accumulated impulse
+        void setFrictionImpulse2(decimal frictionImpulse2);
+
+        /// Add a contact point
+        void addContactPoint(const ContactPointInfo* contactPointInfo);
+
+        /// Set the friction twist accumulated impulse
+        void setFrictionTwistImpulse(decimal frictionTwistImpulse);
+
+        /// Set the accumulated rolling resistance impulse
+        void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
+
+        /// Set the pointer to the previous element in the linked-list
+        void setPrevious(ContactManifold* previousManifold);
+
+        /// Return the first friction vector at the center of the contact manifold
+        const Vector3& getFrictionVector1() const;
+
+        /// Return the second friction vector at the center of the contact manifold
+        const Vector3& getFrictionVector2() const;
+
+        /// Return the first friction accumulated impulse
+        decimal getFrictionImpulse1() const;
+
+        /// Return the second friction accumulated impulse
+        decimal getFrictionImpulse2() const;
+
+        /// Return the friction twist accumulated impulse
+        decimal getFrictionTwistImpulse() const;
+        
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
+        ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
-                        PoolAllocator& memoryAllocator, short int normalDirectionId);
+                        Allocator& memoryAllocator);
 
         /// Destructor
         ~ContactManifold();
@@ -160,68 +229,25 @@ class ContactManifold {
         /// Return a pointer to the second body of the contact manifold
         CollisionBody* getBody2() const;
 
-        /// Return the normal direction Id
-        short int getNormalDirectionId() const;
-
-        /// Clear the contact manifold
-        void clear();
-
         /// Return the number of contact points in the manifold
         int8 getNbContactPoints() const;
 
-        /// Return the first friction vector at the center of the contact manifold
+        /// Return a pointer to the first contact point of the manifold
-        const Vector3& getFrictionVector1() const;
+        ContactPoint* getContactPoints() const;
 
-        /// set the first friction vector at the center of the contact manifold
+        /// Return a pointer to the previous element in the linked-list
-        void setFrictionVector1(const Vector3& mFrictionVector1);
+        ContactManifold* getPrevious() const;
 
-        /// Return the second friction vector at the center of the contact manifold
+        /// Return a pointer to the next element in the linked-list
-        const Vector3& getFrictionVector2() const;
+        ContactManifold* getNext() const;
-
-        /// set the second friction vector at the center of the contact manifold
-        void setFrictionVector2(const Vector3& mFrictionVector2);
-
-        /// Return the first friction accumulated impulse
-        decimal getFrictionImpulse1() const;
-
-        /// Set the first friction accumulated impulse
-        void setFrictionImpulse1(decimal frictionImpulse1);
-
-        /// Return the second friction accumulated impulse
-        decimal getFrictionImpulse2() const;
-
-        /// Set the second friction accumulated impulse
-        void setFrictionImpulse2(decimal frictionImpulse2);
-
-        /// Return the friction twist accumulated impulse
-        decimal getFrictionTwistImpulse() const;
-
-        /// Set the friction twist accumulated impulse
-        void setFrictionTwistImpulse(decimal frictionTwistImpulse);
-
-        /// Set the accumulated rolling resistance impulse
-        void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
-
-        /// Return a contact point of the manifold
-        ContactPoint* getContactPoint(uint index) const;
-
-        /// Return the normalized averaged normal vector
-        Vector3 getAverageContactNormal() const;
-
-        /// Return the largest depth of all the contact points
-        decimal getLargestContactDepth() const;
-
-        /// Add a contact point
-        void addContactPoint(const ContactPointInfo* contactPointInfo);
-
-        /// Remove a contact point
-        void removeContactPoint(int index);
 
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
         friend class Island;
         friend class CollisionBody;
+        friend class ContactManifoldSet;
+        friend class ContactSolver;
 };
 
 // Return a pointer to the first proxy shape of the contact
@@ -244,11 +270,6 @@ inline CollisionBody* ContactManifold::getBody2() const {
     return mShape2->getBody();
 }
 
-// Return the normal direction Id
-inline short int ContactManifold::getNormalDirectionId() const {
-    return mNormalDirectionId;
-}
-
 // Return the number of contact points in the manifold
 inline int8 ContactManifold::getNbContactPoints() const {
     return mNbContactPoints;
@@ -309,10 +330,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
     mRollingResistanceImpulse = rollingResistanceImpulse;
 }
 
-// Return a contact point of the manifold
+// Return a pointer to the first contact point of the manifold
-inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
+inline ContactPoint* ContactManifold::getContactPoints() const {
-    assert(index < mNbContactPoints);
+    return mContactPoints;
-    return mContactPoints[index];
 }
 
 // Return true if the contact manifold has already been added into an island
@@ -320,31 +340,69 @@ inline bool ContactManifold::isAlreadyInIsland() const {
     return mIsAlreadyInIsland;
 }
 
-// Return the normalized averaged normal vector
-inline Vector3 ContactManifold::getAverageContactNormal() const {
-    Vector3 averageNormal;
-
-    for (uint i=0; i<mNbContactPoints; i++) {
-        averageNormal += mContactPoints[i]->getNormal();
-    }
-
-    return averageNormal.getUnit();
-}
-
 // Return the largest depth of all the contact points
 inline decimal ContactManifold::getLargestContactDepth() const {
     decimal largestDepth = 0.0f;
 
-    for (uint i=0; i<mNbContactPoints; i++) {
+    assert(mNbContactPoints > 0);
-        decimal depth = mContactPoints[i]->getPenetrationDepth();
+
+    ContactPoint* contactPoint = mContactPoints;
+    while(contactPoint != nullptr){
+        decimal depth = contactPoint->getPenetrationDepth();
         if (depth > largestDepth) {
             largestDepth = depth;
         }
+
+        contactPoint = contactPoint->getNext();
     }
 
     return largestDepth;
 }
 
+// Return a pointer to the previous element in the linked-list
+inline ContactManifold* ContactManifold::getPrevious() const {
+    return mPrevious;
+}
+
+// Set the pointer to the previous element in the linked-list
+inline void ContactManifold::setPrevious(ContactManifold* previousManifold) {
+    mPrevious = previousManifold;
+}
+
+// Return a pointer to the next element in the linked-list
+inline ContactManifold* ContactManifold::getNext() const {
+    return mNext;
+}
+
+// Set the pointer to the next element in the linked-list
+inline void ContactManifold::setNext(ContactManifold* nextManifold) {
+    mNext = nextManifold;
+}
+
+// Return true if the manifold is obselete
+inline bool ContactManifold::getIsObselete() const {
+    return mIsObselete;
+}
+
+// Set to true to make the manifold obselete
+inline void ContactManifold::setIsObselete(bool isObselete, bool setContactPoints) {
+    mIsObselete = isObselete;
+
+    if (setContactPoints) {
+        ContactPoint* contactPoint = mContactPoints;
+        while (contactPoint != nullptr) {
+            contactPoint->setIsObselete(isObselete);
+
+            contactPoint = contactPoint->getNext();
+        }
+    }
+}
+
+// Return the contact normal direction Id of the manifold
+inline short ContactManifold::getContactNormalId() const {
+    return mContactNormalId;
+}
+
 }
 #endif
 

src/collision/ContactManifoldInfo.cpp

@@ -30,7 +30,8 @@ using namespace reactphysics3d;
 
 // Constructor
 ContactManifoldInfo::ContactManifoldInfo(Allocator& allocator)
-     : mContactPointsList(nullptr), mAllocator(allocator), mNbContactPoints(0) {}
+     : mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator),
+       mContactNormalId(-1) {}
 
 // Destructor
 ContactManifoldInfo::~ContactManifoldInfo() {
@@ -40,14 +41,13 @@ ContactManifoldInfo::~ContactManifoldInfo() {
 }
 
 // Add a new contact point into the manifold
-void ContactManifoldInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
+void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId) {
-                     const Vector3& localPt1, const Vector3& localPt2) {
 
-    assert(penDepth > decimal(0.0));
+    assert(contactPointInfo->penetrationDepth > decimal(0.0));
+    assert(contactNormalId >= 0);
+    assert(mContactNormalId == -1 || contactNormalId == mContactNormalId);
 
-    // Create the contact point info
+    mContactNormalId = contactNormalId;
-    ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
-            ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
 
     // Add it into the linked list of contact points
     contactPointInfo->next = mContactPointsList;
@@ -73,15 +73,31 @@ void ContactManifoldInfo::reset() {
     mNbContactPoints = 0;
 }
 
-// Reduce the number of points in the contact manifold
+// Return the largest penetration depth among its contact points
+decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
+
+    ContactPointInfo* contactPoint = mContactPointsList;
+    assert(contactPoint != nullptr);
+    decimal maxDepth = decimal(0.0);
+    while (contactPoint != nullptr) {
+
+        if (contactPoint->penetrationDepth > maxDepth) {
+            maxDepth = contactPoint->penetrationDepth;
+        }
+
+        contactPoint = contactPoint->next;
+    }
+
+    return maxDepth;
+}
+
+// Reduce the number of contact points of the currently computed manifold
 // This is based on the technique described by Dirk Gregorius in his
 // "Contacts Creation" GDC presentation
 void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
 
     assert(mContactPointsList != nullptr);
 
-    // TODO : Implement this (do not forget to deallocate removed points)
-
     // The following algorithm only works to reduce to 4 contact points
     assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
 
@@ -221,10 +237,10 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
                     largestArea = area;
                     pointsToKeep[3] = element;
                 }
+            }
 
             element = element->next;
         }
-        }
         assert(pointsToKeep[3] != nullptr);
 
         // Delete the contact points we do not want to keep from the linked list
@@ -250,6 +266,9 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
             }
             element = element->next;
 
+            // Call the destructor
+            elementToDelete->~ContactPointInfo();
+
             // Delete the current element
             mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
         }
@@ -258,21 +277,4 @@ void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
     }
 }
 
-/// Return the largest penetration depth among the contact points
-decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
-
-    decimal maxDepth = decimal(0.0);
-    ContactPointInfo* element = mContactPointsList;
-    while(element != nullptr) {
-
-        if (element->penetrationDepth > maxDepth) {
-            maxDepth = element->penetrationDepth;
-        }
-
-        element = element->next;
-    }
-
-    return maxDepth;
-}
-
 

src/collision/ContactManifoldInfo.h

@@ -34,7 +34,7 @@
 namespace reactphysics3d {
 
 // Constants
-const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4;   // Maximum number of contacts in the manifold
+const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4;   // Maximum number of contacts in the manifold
 
 // Class ContactManifoldInfo
 /**
@@ -50,12 +50,18 @@ class ContactManifoldInfo {
         /// Linked list with all the contact points
         ContactPointInfo* mContactPointsList;
 
-        /// Memory allocator used to allocate contact points
-        Allocator& mAllocator;
-
         /// Number of contact points in the manifold
         uint mNbContactPoints;
 
+        /// Next element in the linked-list of contact manifold info
+        ContactManifoldInfo* mNext;
+
+        /// Reference the the memory allocator where the contact point infos have been allocated
+        Allocator& mAllocator;
+
+        /// Contact normal direction Id (Identify the contact normal direction of points in manifold)
+        short mContactNormalId;
+
     public:
 
         // -------------------- Methods -------------------- //
@@ -66,15 +72,14 @@ class ContactManifoldInfo {
         /// Destructor
         ~ContactManifoldInfo();
 
-        /// Deleted copy-constructor
+        /// Deleted Copy-constructor
         ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
 
         /// Deleted assignment operator
         ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
 
         /// Add a new contact point into the manifold
-        void addContactPoint(const Vector3& contactNormal, decimal penDepth,
+        void addContactPoint(ContactPointInfo* contactPointInfo, short contactNormalId);
-                             const Vector3& localPt1, const Vector3& localPt2);
 
         /// Remove all the contact points
         void reset();
@@ -82,11 +87,21 @@ class ContactManifoldInfo {
         /// Get the first contact point info of the linked list of contact points
         ContactPointInfo* getFirstContactPointInfo() const;
 
-        /// Reduce the number of points in the contact manifold
+        /// Return the largest penetration depth among its contact points
+        decimal getLargestPenetrationDepth() const;
+
+        /// Return the pointer to the next manifold info in the linked-list
+        ContactManifoldInfo* getNext();
+
+        /// Return the contact normal Id
+        short getContactNormalId() const;
+
+        /// Reduce the number of contact points of the currently computed manifold
         void reduce(const Transform& shape1ToWorldTransform);
 
-        /// Return the largest penetration depth among the contact points
+        // Friendship
-        decimal getLargestPenetrationDepth() const;
+        friend class OverlappingPair;
+        friend class CollisionDetection;
 };
 
 // Get the first contact point info of the linked list of contact points
@@ -94,6 +109,16 @@ inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
     return mContactPointsList;
 }
 
+// Return the pointer to the next manifold info in the linked-list
+inline ContactManifoldInfo* ContactManifoldInfo::getNext() {
+    return mNext;
+}
+
+// Return the contact normal Id
+inline short ContactManifoldInfo::getContactNormalId() const {
+    return mContactNormalId;
+}
+
 }
 #endif
 

src/collision/ContactManifoldSet.cpp

@@ -30,10 +30,12 @@ using namespace reactphysics3d;
 
 // Constructor
 ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
-                                       PoolAllocator& memoryAllocator, int nbMaxManifolds)
+                                       Allocator& memoryAllocator)
-                   : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
+                   : mNbManifolds(0), mShape1(shape1),
-                     mShape2(shape2), mMemoryAllocator(memoryAllocator) {
+                     mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) {
 
+    // Compute the maximum number of manifolds allowed between the two shapes
+    mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
 }
 
 // Destructor
@@ -43,167 +45,131 @@ ContactManifoldSet::~ContactManifoldSet() {
     clear();
 }
 
-void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
+void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
 
-    assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr);
+    assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr);
 
-    // If there is no contact manifold yet
+    // Try to find an existing contact manifold with similar contact normal
-    if (mNbManifolds == 0) {
+    ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo->getContactNormalId());
-
-        // If the maximum number of manifold is 1
-        if (mNbMaxManifolds == 1) {
-            createManifold(contactManifoldInfo, 0);
-        }
-        else {
-
-            // Compute an Id corresponding to the normal direction (using a cubemap)
-            short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
-
-            createManifold(contactManifoldInfo, normalDirectionId);
-        }
-    }
-    else {   // If there is already at least one contact manifold in the set
-
-        // If the maximum number of manifold is 1
-        if (mNbMaxManifolds == 1) {
-
-            // Replace the old manifold with the new one
-            updateManifoldWithNewOne(0, contactManifoldInfo);
-        }
-        else {
-
-            // Compute an Id corresponding to the normal direction (using a cubemap)
-            short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
-
-            // Select the manifold with the most similar normal (if exists)
-            int similarManifoldIndex = 0;
-            if (mNbMaxManifolds > 1) {
-                similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
-            }
 
     // If a similar manifold has been found
-            if (similarManifoldIndex != -1) {
+    if (similarManifold != nullptr) {
 
-                // Replace the old manifold with the new one
+        // Update the old manifold with the new one
-                updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo);
+        updateManifoldWithNewOne(similarManifold, contactManifoldInfo);
     }
     else {
 
-                // If we have not reach the maximum number of manifolds
+        // If there are too much contact manifolds in the set
-                if (mNbManifolds < mNbMaxManifolds) {
+        if (mNbManifolds >= mNbMaxManifolds) {
 
-                    // Create the new contact manifold
+            // We need to remove a manifold from the set.
-                    createManifold(contactManifoldInfo, normalDirectionId);
+            // We seach for the one with the smallest maximum penetration depth among its contact points
-                }
+            ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth());
-                else {
 
-                    decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth();
+            // If the manifold with the minimum penetration depth is an existing one
+            if (minDepthManifold != nullptr) {
 
-                    // We have reached the maximum number of manifold, we do not
+                // Remove the manifold
-                    // want to keep the manifold with the smallest penetration detph
+                removeManifold(minDepthManifold);
-                    int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth);
 
-                    // If the manifold with the smallest penetration depth is not the new one,
+                // Create a new contact manifold
-                    // we have to keep the new manifold and remove the one with the smallest depth
+                createManifold(contactManifoldInfo);
-                    if (smallestPenDepthManifoldIndex >= 0) {
-                        removeManifold(smallestPenDepthManifoldIndex);
-                        createManifold(contactManifoldInfo, normalDirectionId);
-                    }
-                }
             }
         }
+
+        // Create a new contact manifold
+        createManifold(contactManifoldInfo);
     }
 }
 
 // Update a previous similar manifold with a new one
-void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) {
+void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
 
-   // For each contact point of the previous manifold
+    assert(oldManifold != nullptr);
-   for (int i=0; i<mManifolds[oldManifoldIndex]->getNbContactPoints(); ) {
+    assert(newManifold != nullptr);
 
-       ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i);
+   // For each contact point of the new manifold
+   ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
+   assert(contactPointInfo != nullptr);
+   while (contactPointInfo != nullptr) {
 
-       // For each contact point in the new manifold
+       // For each contact point in the old manifold
-       ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo();
+       bool isSimilarPointFound = false;
-       bool needToRemovePoint = true;
+       ContactPoint* oldContactPoint = oldManifold->getContactPoints();
-       while (newPoint != nullptr) {
+       while (oldContactPoint != nullptr) {
+
+           assert(oldContactPoint != nullptr);
 
             // If the new contact point is similar (very close) to the old contact point
-            if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) {
+            if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
 
                 // Replace (update) the old contact point with the new one
-                contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(),
+                oldContactPoint->update(contactPointInfo, mShape1->getLocalToWorldTransform(),  mShape2->getLocalToWorldTransform());
-                                               mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform());
+                isSimilarPointFound = true;
-                needToRemovePoint = false;
-                newPoint->isUsed = true;
                 break;
             }
 
-            newPoint = newPoint->next;
+            oldContactPoint = oldContactPoint->getNext();
        }
 
-       // If no new contact point is similar to the old one
+       // If we have not found a similar contact point
-       if (needToRemovePoint) {
+       if (!isSimilarPointFound) {
 
-           // Remove the old contact point
+           // Add the contact point to the manifold
-           mManifolds[oldManifoldIndex]->removeContactPoint(i);
+           oldManifold->addContactPoint(contactPointInfo);
-       }
-       else {
-           i++;
-       }
        }
 
-   // For each point of the new manifold that have not been used yet (to update
+       contactPointInfo = contactPointInfo->next;
-   // an existing point in the previous manifold), add it into the previous manifold
-   const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo();
-   while (newPointInfo != nullptr) {
-
-        if (!newPointInfo->isUsed) {
-            mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo);
    }
 
-        newPointInfo = newPointInfo->next;
+   // The old manifold is no longer obselete
-   }
+   oldManifold->setIsObselete(false, false);
 }
 
-// Return the manifold with the smallest contact penetration depth
+// Return the manifold with the smallest contact penetration depth among its points
-int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
+ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
 
-    // The contact point will be in a new contact manifold, we now have too much
-    // manifolds condidates. We need to remove one. We choose to remove the manifold
-    // with the smallest contact depth among their points
-    int smallestDepthManifoldIndex = -1;
-    decimal minDepth = initDepth;
     assert(mNbManifolds == mNbMaxManifolds);
-    for (int i=0; i<mNbManifolds; i++) {
+
-        decimal depth = mManifolds[i]->getLargestContactDepth();
+    ContactManifold* minDepthManifold = nullptr;
+    decimal minDepth = initDepth;
+    ContactManifold* manifold = mManifolds;
+    while (manifold != nullptr) {
+        decimal depth = manifold->getLargestContactDepth();
         if (depth < minDepth) {
             minDepth = depth;
-            smallestDepthManifoldIndex = i;
+            minDepthManifold = manifold;
-        }
         }
 
-    return smallestDepthManifoldIndex;
+        manifold = manifold->getNext();
     }
 
-// Return the index of the contact manifold with a similar average normal.
+    return minDepthManifold;
-// If no manifold has close enough average normal, it returns -1
+}
-int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
+
+// Return the contact manifold with a similar average normal.
+// If no manifold has close enough average normal, it returns nullptr
+ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
+
+    ContactManifold* manifold = mManifolds;
 
     // Return the Id of the manifold with the same normal direction id (if exists)
-    for (int i=0; i<mNbManifolds; i++) {
+    while (manifold != nullptr) {
-        if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
+        if (normalDirectionId == manifold->getContactNormalId()) {
-            return i;
+            return manifold;
-        }
         }
 
-    return -1;
+        manifold = manifold->getNext();
+    }
+
+    return nullptr;
 }
 
 // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
 // Each face of the cube is divided into 4x4 buckets. This method maps the
 // normal vector into of the of the bucket and returns a unique Id for the bucket
-short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
+short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) {
 
     assert(normal.lengthSquare() > MACHINE_EPSILON);
 
@@ -240,36 +206,100 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
 // Clear the contact manifold set
 void ContactManifoldSet::clear() {
 
-    // Destroy all the contact manifolds
+    ContactManifold* manifold = mManifolds;
-    for (int i=mNbManifolds-1; i>=0; i--) {
+    while(manifold != nullptr) {
-        removeManifold(i);
+
+        ContactManifold* nextManifold = manifold->getNext();
+
+        // Delete the contact manifold
+        manifold->~ContactManifold();
+        mMemoryAllocator.release(manifold, sizeof(ContactManifold));
+
+        manifold = nextManifold;
+
+        mNbManifolds--;
     }
 
     assert(mNbManifolds == 0);
 }
 
 // Create a new contact manifold and add it to the set
-void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) {
+void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
     assert(mNbManifolds < mNbMaxManifolds);
 
-    mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
+    ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
-                                    ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId);
+                                    ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
+    manifold->setPrevious(nullptr);
+    manifold->setNext(mManifolds);
+    mManifolds = manifold;
+
     mNbManifolds++;
 }
 
 // Remove a contact manifold from the set
-void ContactManifoldSet::removeManifold(int index) {
+void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
 
     assert(mNbManifolds > 0);
-    assert(index >= 0 && index < mNbManifolds);
 
-    // Delete the new contact
+    ContactManifold* previous = manifold->getPrevious();
-    mManifolds[index]->~ContactManifold();
+    ContactManifold* next = manifold->getNext();
-    mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
 
-    for (int i=index; (i+1) < mNbManifolds; i++) {
+    if (previous != nullptr) {
-        mManifolds[i] = mManifolds[i+1];
+        previous->setNext(manifold->getNext());
     }
+    if (next != nullptr) {
+        next->setPrevious(manifold->getPrevious());
+    }
+
+    // Delete the contact manifold
+    manifold->~ContactManifold();
+    mMemoryAllocator.release(manifold, sizeof(ContactManifold));
 
     mNbManifolds--;
 }
+
+// Make all the contact manifolds and contact points obselete
+void ContactManifoldSet::makeContactsObselete() {
+
+    ContactManifold* manifold = mManifolds;
+    while (manifold != nullptr) {
+
+        manifold->setIsObselete(true, true);
+
+        manifold = manifold->getNext();
+    }
+}
+
+// Clear the obselete contact manifolds and contact points
+void ContactManifoldSet::clearObseleteManifoldsAndContactPoints() {
+
+    ContactManifold* manifold = mManifolds;
+    ContactManifold* previousManifold = nullptr;
+    while (manifold != nullptr) {
+        ContactManifold* nextManifold = manifold->getNext();
+
+        if (manifold->getIsObselete()) {
+
+            if (previousManifold != nullptr) {
+                previousManifold->setNext(nextManifold);
+
+                if (nextManifold != nullptr) {
+                    nextManifold->setPrevious(previousManifold);
+                }
+            }
+            else {
+                mManifolds = nextManifold;
+            }
+
+            // Delete the contact manifold
+            removeManifold(manifold);
+
+        }
+        else {
+            manifold->clearObseleteContactPoints();
+            previousManifold = manifold;
+        }
+
+        manifold = nextManifold;
+    }
+}

src/collision/ContactManifoldSet.h

@@ -60,33 +60,34 @@ class ContactManifoldSet {
         /// Pointer to the second proxy shape of the contact
         ProxyShape* mShape2;
 
-        /// Reference to the memory allocator
+        /// Reference to the memory allocator for the contact manifolds
-        PoolAllocator& mMemoryAllocator;
+        Allocator& mMemoryAllocator;
 
         /// Contact manifolds of the set
-        ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
+        ContactManifold* mManifolds;
 
         // -------------------- Methods -------------------- //
 
         /// Create a new contact manifold and add it to the set
-        void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId);
+        void createManifold(const ContactManifoldInfo* manifoldInfo);
 
-        /// Remove a contact manifold from the set
+        // Return the contact manifold with a similar average normal.
-        void removeManifold(int index);
+        ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
-
-        // Return the index of the contact manifold with a similar average normal.
-        int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
-
-        // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
-        // Each face of the cube is divided into 4x4 buckets. This method maps the
-        // normal vector into of the of the bucket and returns a unique Id for the bucket
-        short int computeCubemapNormalId(const Vector3& normal) const;
 
         /// Return the manifold with the smallest contact penetration depth
-        int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
+        ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
 
         /// Update a previous similar manifold with a new one
-        void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold);
+        void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
+
+        /// Return the maximum number of contact manifolds allowed between to collision shapes
+        int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2);
+
+        /// Clear the contact manifold set
+        void clear();
+
+        /// Delete a contact manifold
+        void removeManifold(ContactManifold* manifold);
 
     public:
 
@@ -94,13 +95,13 @@ class ContactManifoldSet {
 
         /// Constructor
         ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
-                           PoolAllocator& memoryAllocator, int nbMaxManifolds);
+                           Allocator& memoryAllocator);
 
         /// Destructor
         ~ContactManifoldSet();
 
         /// Add a contact manifold in the set
-        void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
+        void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
 
         /// Return the first proxy shape
         ProxyShape* getShape1() const;
@@ -108,17 +109,25 @@ class ContactManifoldSet {
         /// Return the second proxy shape
         ProxyShape* getShape2() const;
 
-        /// Clear the contact manifold set
-        void clear();
-
         /// Return the number of manifolds in the set
         int getNbContactManifolds() const;
 
-        /// Return a given contact manifold
+        /// Return a pointer to the first element of the linked-list of contact manifolds
-        ContactManifold* getContactManifold(int index) const;
+        ContactManifold* getContactManifolds() const;
+
+        /// Make all the contact manifolds and contact points obselete
+        void makeContactsObselete();
 
         /// Return the total number of contact points in the set of manifolds
         int getTotalNbContactPoints() const;
+
+        /// Clear the obselete contact manifolds and contact points
+        void clearObseleteManifoldsAndContactPoints();
+
+        // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
+        // Each face of the cube is divided into 4x4 buckets. This method maps the
+        // normal vector into of the of the bucket and returns a unique Id for the bucket
+        static short int computeCubemapNormalId(const Vector3& normal);
 };
 
 // Return the first proxy shape
@@ -136,21 +145,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const {
     return mNbManifolds;
 }
 
-// Return a given contact manifold
+// Return a pointer to the first element of the linked-list of contact manifolds
-inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const {
+inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
-    assert(index >= 0 && index < mNbManifolds);
+    return mManifolds;
-    return mManifolds[index];
 }
 
 // Return the total number of contact points in the set of manifolds
 inline int ContactManifoldSet::getTotalNbContactPoints() const {
     int nbPoints = 0;
-    for (int i=0; i<mNbManifolds; i++) {
+
-        nbPoints += mManifolds[i]->getNbContactPoints();
+    ContactManifold* manifold = mManifolds;
+    while (manifold != nullptr) {
+        nbPoints += manifold->getNbContactPoints();
+
+        manifold = manifold->getNext();
     }
+
     return nbPoints;
 }
 
+// Return the maximum number of contact manifolds allowed between to collision shapes
+inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
+
+    // If both shapes are convex
+    if (shape1->isConvex() && shape2->isConvex()) {
+        return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
+
+    }   // If there is at least one concave shape
+    else {
+        return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
+    }
+}
+
 }
 
 #endif

src/collision/NarrowPhaseInfo.cpp

@@ -0,0 +1,97 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include <iostream>
+#include "NarrowPhaseInfo.h"
+#include "ContactPointInfo.h"
+#include "engine/OverlappingPair.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
+                const CollisionShape* shape2, const Transform& shape1Transform,
+                const Transform& shape2Transform, void** cachedData1, void** cachedData2)
+      : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
+        shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
+        contactPoints(nullptr), cachedCollisionData1(cachedData1),
+        cachedCollisionData2(cachedData2), next(nullptr) {
+
+}
+
+// Destructor
+NarrowPhaseInfo::~NarrowPhaseInfo() {
+
+    assert(contactPoints == nullptr);
+}
+
+// Add a new contact point
+void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
+                     const Vector3& localPt1, const Vector3& localPt2) {
+
+    assert(penDepth > decimal(0.0));
+
+    // Get the memory allocator
+    Allocator& allocator = overlappingPair->getTemporaryAllocator();
+
+    // Create the contact point info
+    ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
+            ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
+
+    // Add it into the linked list of contact points
+    contactPointInfo->next = contactPoints;
+    contactPoints = contactPointInfo;
+}
+
+/// Take all the generated contact points and create a new potential
+/// contact manifold into the overlapping pair
+void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() {
+    overlappingPair->addPotentialContactPoints(this);
+}
+
+// Reset the remaining contact points
+void NarrowPhaseInfo::resetContactPoints() {
+
+    // Get the memory allocator
+    Allocator& allocator = overlappingPair->getTemporaryAllocator();
+
+    // For each remaining contact point info
+    ContactPointInfo* element = contactPoints;
+    while(element != nullptr) {
+
+        ContactPointInfo* elementToDelete = element;
+
+        element = element->next;
+
+        // Call the destructor
+        elementToDelete->~ContactPointInfo();
+
+        // Delete the current element
+        allocator.release(elementToDelete, sizeof(ContactPointInfo));
+    }
+
+    contactPoints = nullptr;
+}

src/collision/NarrowPhaseInfo.h

@@ -28,6 +28,7 @@
 
 // Libraries
 #include "shapes/CollisionShape.h"
+#include "collision/ContactManifoldInfo.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -58,6 +59,9 @@ struct NarrowPhaseInfo {
         /// Transform that maps from collision shape 2 local-space to world-space
         Transform shape2ToWorldTransform;
 
+        /// Linked-list of contact points created during the narrow-phase
+        ContactPointInfo* contactPoints;
+
         /// Cached collision data of the proxy shape
         // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2
         void** cachedCollisionData1;
@@ -72,12 +76,20 @@ struct NarrowPhaseInfo {
         /// Constructor
         NarrowPhaseInfo(OverlappingPair* pair, const CollisionShape* shape1,
                         const CollisionShape* shape2, const Transform& shape1Transform,
-                        const Transform& shape2Transform, void** cachedData1, void** cachedData2)
+                        const Transform& shape2Transform, void** cachedData1, void** cachedData2);
-              : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
-                shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
-                cachedCollisionData1(cachedData1), cachedCollisionData2(cachedData2), next(nullptr) {
 
-        }
+        /// Destructor
+        ~NarrowPhaseInfo();
+
+        /// Add a new contact point
+        void addContactPoint(const Vector3& contactNormal, decimal penDepth,
+                             const Vector3& localPt1, const Vector3& localPt2);
+
+        /// Create a new potential contact manifold into the overlapping pair using current contact points
+        void addContactPointsAsPotentialContactManifold();
+
+        /// Reset the remaining contact points
+        void resetContactPoints();
 };
 
 }

src/collision/OverlapCallback.h

@@ -0,0 +1,57 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2016 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_OVERLAP_CALLBACK_H
+#define REACTPHYSICS3D_OVERLAP_CALLBACK_H
+
+// Libraries
+#include "body/CollisionBody.h"
+
+/// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class OverlapCallback
+/**
+ * This class can be used to register a callback for collision overlap queries.
+ * You should implement your own class inherited from this one and implement
+ * the notifyOverlap() method. This method will called each time a contact
+ * point is reported.
+ */
+class OverlapCallback {
+
+    public:
+
+        /// Destructor
+        virtual ~OverlapCallback() {
+
+        }
+
+        /// This method will be called for each reported overlapping bodies
+        virtual void notifyOverlap(CollisionBody* collisionBody)=0;
+};
+
+}
+
+#endif

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp

@@ -33,7 +33,7 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between two capsules
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
-bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
     
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
@@ -116,8 +116,8 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
 				decimal penetrationDepth = sumRadius - segmentsDistance;
 
 				// Create the contact info object
-				contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
+                narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
-				contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
+                narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
 
 				return true;
 			}
@@ -148,7 +148,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
 		decimal penetrationDepth = sumRadius - closestPointsDistance;
 
 		// Create the contact info object
-        contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
+        narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
 
 		return true;
 	}

src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h

@@ -61,8 +61,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 		CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between two capsules
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
-                                   ContactManifoldInfo& contactManifoldInfo) override;
 };
 
 }

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp

@@ -37,13 +37,12 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between a capsule and a polyhedron
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
-bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
-                                                       ContactManifoldInfo& contactManifoldInfo) {
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
     SATAlgorithm satAlgorithm;
-    GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+    GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
 
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
@@ -61,7 +60,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
 		// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
 
         // Get the contact point created by GJK
-        ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo();
+        ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
         assert(contactPoint != nullptr);
 
         bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
@@ -96,7 +95,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
 				&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
 
                 // Remove the previous contact point computed by GJK
-                contactManifoldInfo.reset();
+                narrowPhaseInfo->resetContactPoints();
 
                 const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
                 const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
@@ -116,7 +115,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
                 satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
                                                           polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
                                                           capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
-                                                          contactManifoldInfo, isCapsuleShape1);
+                                                          narrowPhaseInfo, isCapsuleShape1);
 
                 break;
             }
@@ -133,7 +132,7 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
     if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
 
         // Run the SAT algorithm to find the separating axis and compute contact point
-        bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+        bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
 
         narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
         narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;

src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h

@@ -61,8 +61,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a capsule and a polyhedron
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
-                                   ContactManifoldInfo& contactManifoldInfo) override;
 };
 
 }

src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp

@@ -141,114 +141,114 @@ void ConcaveVsConvexAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
 // Process the concave triangle mesh collision using the smooth mesh collision algorithm described
 // by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
 // issue with some internal edges.
-void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
+//void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
-                                                          std::vector<SmoothMeshContactInfo> contactPoints,
+//                                                          std::vector<SmoothMeshContactInfo> contactPoints,
-                                                          NarrowPhaseCallback* narrowPhaseCallback) {
+//                                                          NarrowPhaseCallback* narrowPhaseCallback) {
 
-    // Set with the triangle vertices already processed to void further contacts with same triangle
+//    // Set with the triangle vertices already processed to void further contacts with same triangle
-    std::unordered_multimap<int, Vector3> processTriangleVertices;
+//    std::unordered_multimap<int, Vector3> processTriangleVertices;
 
-    // Sort the list of narrow-phase contacts according to their penetration depth
+//    // Sort the list of narrow-phase contacts according to their penetration depth
-    std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
+//    std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
 
-    // For each contact point (from smaller penetration depth to larger)
+//    // For each contact point (from smaller penetration depth to larger)
-    std::vector<SmoothMeshContactInfo>::const_iterator it;
+//    std::vector<SmoothMeshContactInfo>::const_iterator it;
-    for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
+//    for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
 
-        const SmoothMeshContactInfo info = *it;
+//        const SmoothMeshContactInfo info = *it;
-        const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+//        const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
 
-        // Compute the barycentric coordinates of the point in the triangle
+//        // Compute the barycentric coordinates of the point in the triangle
-        decimal u, v, w;
+//        decimal u, v, w;
-        computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
+//        computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
-                                                info.triangleVertices[1],
+//                                                info.triangleVertices[1],
-                                                info.triangleVertices[2],
+//                                                info.triangleVertices[2],
-                                                contactPoint, u, v, w);
+//                                                contactPoint, u, v, w);
-        int nbZeros = 0;
+//        int nbZeros = 0;
-        bool isUZero = approxEqual(u, 0, 0.0001);
+//        bool isUZero = approxEqual(u, 0, 0.0001);
-        bool isVZero = approxEqual(v, 0, 0.0001);
+//        bool isVZero = approxEqual(v, 0, 0.0001);
-        bool isWZero = approxEqual(w, 0, 0.0001);
+//        bool isWZero = approxEqual(w, 0, 0.0001);
-        if (isUZero) nbZeros++;
+//        if (isUZero) nbZeros++;
-        if (isVZero) nbZeros++;
+//        if (isVZero) nbZeros++;
-        if (isWZero) nbZeros++;
+//        if (isWZero) nbZeros++;
 
-        // If it is a vertex contact
+//        // If it is a vertex contact
-        if (nbZeros == 2) {
+//        if (nbZeros == 2) {
 
-            Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
+//            Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
 
-            // Check that this triangle vertex has not been processed yet
+//            // Check that this triangle vertex has not been processed yet
-            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
+//            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
 
-                // Keep the contact as it is and report it
+//                // Keep the contact as it is and report it
-                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+//                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
-            }
+//            }
-        }
+//        }
-        else if (nbZeros == 1) {  // If it is an edge contact
+//        else if (nbZeros == 1) {  // If it is an edge contact
 
-            Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
+//            Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
-            Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
+//            Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
 
-            // Check that this triangle edge has not been processed yet
+//            // Check that this triangle edge has not been processed yet
-            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
+//            if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
-                !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
+//                !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
 
-                // Keep the contact as it is and report it
+//                // Keep the contact as it is and report it
-                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
+//                narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
-            }
+//            }
 
-        }
+//        }
-        else {  // If it is a face contact
+//        else {  // If it is a face contact
 
-            ContactPointInfo newContactInfo(info.contactInfo);
+//            ContactPointInfo newContactInfo(info.contactInfo);
 
-            ProxyShape* firstShape;
+//            ProxyShape* firstShape;
-            ProxyShape* secondShape;
+//            ProxyShape* secondShape;
-            if (info.isFirstShapeTriangle) {
+//            if (info.isFirstShapeTriangle) {
-                firstShape = overlappingPair->getShape1();
+//                firstShape = overlappingPair->getShape1();
-                secondShape = overlappingPair->getShape2();
+//                secondShape = overlappingPair->getShape2();
-            }
+//            }
-            else {
+//            else {
-                firstShape = overlappingPair->getShape2();
+//                firstShape = overlappingPair->getShape2();
-                secondShape = overlappingPair->getShape1();
+//                secondShape = overlappingPair->getShape1();
-            }
+//            }
 
-            // We use the triangle normal as the contact normal
+//            // We use the triangle normal as the contact normal
-            Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
+//            Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
-            Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
+//            Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
-            Vector3 localNormal = a.cross(b);
+//            Vector3 localNormal = a.cross(b);
-            newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
+//            newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
-            Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
+//            Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
-            Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
+//            Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
-            newContactInfo.normal.normalize();
+//            newContactInfo.normal.normalize();
-            if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
+//            if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
-                newContactInfo.normal = -newContactInfo.normal;
+//                newContactInfo.normal = -newContactInfo.normal;
-            }
+//            }
 
-            // We recompute the contact point on the second body with the new normal as described in
+//            // We recompute the contact point on the second body with the new normal as described in
-            // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
+//            // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
-            // Dirk Gregorius) to avoid adding torque
+//            // Dirk Gregorius) to avoid adding torque
-            Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
+//            Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
-            if (info.isFirstShapeTriangle) {
+//            if (info.isFirstShapeTriangle) {
-                Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
+//                Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
-                newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
+//                newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
-            }
+//            }
-            else {
+//            else {
-                Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
+//                Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
-                newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
+//                newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
-            }
+//            }
 
-            // Report the contact
+//            // Report the contact
-            narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
+//            narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
-        }
+//        }
 
-        // Add the three vertices of the triangle to the set of processed
+//        // Add the three vertices of the triangle to the set of processed
-        // triangle vertices
+//        // triangle vertices
-        addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
+//        addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
-        addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
+//        addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
-        addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
+//        addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
-    }
+//    }
-}
+//}
 
 // Return true if the vertex is in the set of already processed vertices
 bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {

src/collision/narrowphase/ConcaveVsConvexAlgorithm.h

@@ -83,25 +83,35 @@ class MiddlePhaseTriangleCallback : public TriangleCallback {
 
 // Class SmoothMeshContactInfo
 /**
- * This class is used to store data about a contact with a triangle for the smooth
+ * Contains data for of potential smooth contact during the smooth mesh
- * mesh algorithm.
+ * contacts computation.
  */
-class SmoothMeshContactInfo {
+struct SmoothMeshContactInfo {
 
     public:
 
-        ContactPointInfo contactInfo;
+        ContactManifoldInfo* contactManifoldInfo;
+        ContactPointInfo* contactInfo;
         bool isFirstShapeTriangle;
         Vector3 triangleVertices[3];
+        bool isUVWZero[3];
 
         /// Constructor
-        SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
+        SmoothMeshContactInfo(ContactManifoldInfo* manifoldInfo, ContactPointInfo* contactPointInfo,
-                              const Vector3& trianglePoint2, const Vector3& trianglePoint3)
+                              bool firstShapeTriangle,
-            : contactInfo(contact) {
+                              const Vector3& trianglePoint1, const Vector3& trianglePoint2,
+                              const Vector3& trianglePoint3, bool isUZero, bool isVZero, bool isWZero)
+            : contactManifoldInfo(manifoldInfo), contactInfo(contactPointInfo) {
+
             isFirstShapeTriangle = firstShapeTriangle;
+
             triangleVertices[0] = trianglePoint1;
             triangleVertices[1] = trianglePoint2;
             triangleVertices[2] = trianglePoint3;
+
+            isUVWZero[0] = isUZero;
+            isUVWZero[1] = isVZero;
+            isUVWZero[2] = isWZero;
         }
 
 };
@@ -109,7 +119,7 @@ class SmoothMeshContactInfo {
 struct ContactsDepthCompare {
     bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
     {
-        return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
+        return contact1.contactInfo->penetrationDepth < contact2.contactInfo->penetrationDepth;
     }
 };
 

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp

@@ -34,12 +34,11 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between two convex polyhedra
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
-bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
-                                                                ContactManifoldInfo& contactManifoldInfo) {
 
     // Run the SAT algorithm to find the separating axis and compute contact point
     SATAlgorithm satAlgorithm;
-    bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+    bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
 
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
     narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;

src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h

@@ -61,8 +61,7 @@ class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm
         ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between two convex polyhedra
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
-                                   ContactManifoldInfo& contactManifoldInfo) override;
 };
 
 }

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -46,8 +46,7 @@ using namespace reactphysics3d;
 /// algorithm on the enlarged object to obtain a simplex polytope that contains the
 /// origin, they we give that simplex polytope to the EPA algorithm which will compute
 /// the correct penetration depth and contact points between the enlarged objects.
-GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
-                                                    ContactManifoldInfo& contactManifoldInfo) {
 
     PROFILE("GJKAlgorithm::testCollision()");
     
@@ -209,7 +208,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
         }
 
         // Add a new contact point
-        contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB);
+        narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
 
         return GJKResult::COLLIDE_IN_MARGIN;
     }

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -87,16 +87,13 @@ class GJKAlgorithm {
         GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
 
         /// Compute a contact info if the two bounding volumes collide.
-        GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts);
-                                ContactManifoldInfo& contactManifoldInfo);
 
         /// Use the GJK Algorithm to find if a point is inside a convex collision shape
         bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
 
         /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
         bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
-
-
 };
 
 }

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -83,8 +83,10 @@ class NarrowPhaseAlgorithm {
         /// Deleted assignment operator
         NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
 
+        // TODO : Use the following reportContacts variable in all narrow-phase algorithms
+
         /// Compute a contact info if the two bounding volume collide
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0;
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
 };
 
 }

src/collision/narrowphase/SAT/SATAlgorithm.cpp

@@ -44,7 +44,7 @@ using namespace reactphysics3d;
 const decimal SATAlgorithm::SAME_SEPARATING_AXIS_BIAS = decimal(0.001);
 
 // Test collision between a sphere and a convex mesh
-bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
 
     PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()");
 
@@ -145,7 +145,7 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
     const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
 
     // Create the contact info object
-    contactManifoldInfo.addContactPoint(isSphereShape1 ? normalWorld : -normalWorld, minPenetrationDepth,
+    narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
                                         isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
                                         isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
 
@@ -171,7 +171,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint faceInd
 }
 
 // Test collision between a capsule and a convex mesh
-bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
+bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
 
     PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()");
 
@@ -387,7 +387,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
         computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
                                                   polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
                                                   capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
-                                                  contactManifoldInfo, isCapsuleShape1);
+                                                  narrowPhaseInfo, isCapsuleShape1);
 
          lastFrameInfo.satIsAxisFacePolyhedron1 = true;
          lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
@@ -406,7 +406,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo*
         const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
 
         // Create the contact point
-        contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+        narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
                                             isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
                                             isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
 
@@ -477,7 +477,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
                                                              decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
                                                              const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
                                                              const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
-                                                             ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const {
+                                                             NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
 
     HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
     uint firstEdgeIndex = face.edgeIndex;
@@ -523,7 +523,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
 			const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
 
 			// Create the contact point
-			contactManifoldInfo.addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
+            narrowPhaseInfo->addContactPoint(isCapsuleShape1 ? -normalWorld : normalWorld, penetrationDepth,
 				isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
 				isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
 		}
@@ -543,8 +543,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
 }
 
 // Test collision between two convex polyhedrons
-bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
+bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo,
-                                                                   ContactManifoldInfo& contactManifoldInfo) const {
+                                                                   bool reportContacts) const {
 
     PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
 
@@ -869,7 +869,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
 				Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
 
                 // Create a new contact point
-                contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+                narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
                                                     isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
                                                     isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
             }
@@ -893,7 +893,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
         const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
 
         // Create the contact point
-        contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
+        narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
                                             closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
 
         lastFrameInfo.satIsAxisFacePolyhedron1 = false;

src/collision/narrowphase/SAT/SATAlgorithm.h

@@ -113,24 +113,24 @@ class SATAlgorithm {
         SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
 
         /// Test collision between a sphere and a convex mesh
-        bool testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+        bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
 
         /// Test collision between a capsule and a convex mesh
-        bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+        bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
 
         /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
         void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
                                                        decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
                                                        const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
                                                        const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
-                                                       ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const;
+                                                       NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const;
 
         // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
         bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
                                           const Vector3& edgeAdjacentFace2Normal) const;
 
         /// Test collision between two convex meshes
-        bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
+        bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
 };
 
 }

src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp

@@ -34,7 +34,7 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between a sphere and a capsule
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
-bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
+bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
 
     bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
 
@@ -86,7 +86,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
         }
         
         // Create the contact info object
-        contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
+        narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
                                             isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
                                             isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
 

src/collision/narrowphase/SphereVsCapsuleAlgorithm.h

@@ -61,8 +61,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
 		SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a sphere and a capsule
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
-                                   ContactManifoldInfo& contactManifoldInfo) override;
 };
 
 }

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp

@@ -34,12 +34,11 @@ using namespace reactphysics3d;
 // Compute the narrow-phase collision detection between a sphere and a convex polyhedron
 // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
 // by Dirk Gregorius.
-bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
-                                                      ContactManifoldInfo& contactManifoldInfo) {
 
     // First, we run the GJK algorithm
     GJKAlgorithm gjkAlgorithm;
-    GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
+    GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
 
 	assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
 		narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@@ -61,7 +60,7 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
 
         // Run the SAT algorithm to find the separating axis and compute contact point
         SATAlgorithm satAlgorithm;
-        bool isColliding =  satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
+        bool isColliding =  satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
 
         narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
         narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;

src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h

@@ -61,8 +61,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
         SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
 
         /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
-                                   ContactManifoldInfo& contactManifoldInfo) override;
 };
 
 }

src/collision/narrowphase/SphereVsSphereAlgorithm.cpp

@@ -30,8 +30,7 @@
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;  
 
-bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
-                                            ContactManifoldInfo& contactManifoldInfo) {
     
     assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
     assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
@@ -62,7 +61,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
         decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
         
         // Create the contact info object
-        contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
+        narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
                                             intersectionOnBody1, intersectionOnBody2);
 
         return true;

src/collision/narrowphase/SphereVsSphereAlgorithm.h

@@ -61,8 +61,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
         SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
 
         /// Compute a contact info if the two bounding volume collide
-        virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
+        virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) override;
-                                   ContactManifoldInfo& contactManifoldInfo) override;
 };
 
 }

src/collision/shapes/BoxShape.h

@@ -237,6 +237,8 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
         case 4: return Vector3(0, -1, 0);
         case 5: return Vector3(0, 1, 0);
     }
+
+    assert(false);
 }
 
 // Return the centroid of the box

src/collision/shapes/CollisionShape.h

@@ -118,10 +118,6 @@ class CollisionShape {
         /// Return true if the collision shape type is a convex shape
         static bool isConvex(CollisionShapeType shapeType);
 
-        /// Return the maximum number of contact manifolds in an overlapping pair given two shape types
-        static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
-                                                CollisionShapeType shapeType2);
-
         // -------------------- Friendship -------------------- //
 
         friend class ProxyShape;
@@ -151,19 +147,6 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
     mScaling = scaling;
 }
 
-// Return the maximum number of contact manifolds allowed in an overlapping
-// pair wit the given two collision shape types
-inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
-                                                        CollisionShapeType shapeType2) {
-    // If both shapes are convex
-    if (isConvex(shapeType1) && isConvex(shapeType2)) {
-        return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
-    }   // If there is at least one concave shape
-    else {
-        return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
-    }
-}
-
 }
 
 #endif

src/constraint/ContactPoint.cpp

@@ -37,19 +37,20 @@ ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform&
                mPenetrationDepth(contactInfo->penetrationDepth),
                mLocalPointOnBody1(contactInfo->localPoint1),
                mLocalPointOnBody2(contactInfo->localPoint2),
-               mIsRestingContact(false) {
+               mIsRestingContact(false), mIsObselete(false), mNext(nullptr) {
 
     assert(mPenetrationDepth > decimal(0.0));
 
     // Compute the world position of the contact points
     mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
     mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+
+    mIsObselete = 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, const Transform& body1Transform,
+void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform, const Transform& body2Transform) {
-                          const Transform& body2Transform) {
 
     assert(isSimilarWithContactPoint(contactInfo));
     assert(contactInfo->penetrationDepth > decimal(0.0));
@@ -62,4 +63,6 @@ void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform&
     // Compute the world position of the contact points
     mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
     mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
+
+    mIsObselete = false;
 }

src/constraint/ContactPoint.h

@@ -71,6 +71,37 @@ class ContactPoint {
         /// Cached penetration impulse
         decimal mPenetrationImpulse;
 
+        /// True if the contact point is obselete
+        bool mIsObselete;
+
+        /// Pointer to the next contact point in the linked-list
+        ContactPoint* mNext;
+
+        // -------------------- Methods -------------------- //
+
+        /// Update the contact point with a new one that is similar (very close)
+        void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
+                    const Transform& body2Transform);
+
+        /// Return true if the contact point is similar (close enougth) to another given contact point
+        bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
+
+        /// Set the cached penetration impulse
+        void setPenetrationImpulse(decimal impulse);
+
+
+        /// Set the mIsRestingContact variable
+        void setIsRestingContact(bool isRestingContact);
+
+        /// Set to true to make the contact point obselete
+        void setIsObselete(bool isObselete);
+
+        /// Set the next contact point in the linked list
+        void setNext(ContactPoint* next);
+
+        /// Return true if the contact point is obselete
+        bool getIsObselete() const;
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -88,10 +119,6 @@ class ContactPoint {
         /// Deleted assignment operator
         ContactPoint& operator=(const ContactPoint& contact) = delete;
 
-        /// Update the contact point with a new one that is similar (very close)
-        void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
-                    const Transform& body2Transform);
-
         /// Return the normal vector of the contact
         Vector3 getNormal() const;
 
@@ -110,23 +137,22 @@ class ContactPoint {
         /// Return the cached penetration impulse
         decimal getPenetrationImpulse() const;
 
-        /// Return true if the contact point is similar (close enougth) to another given contact point
-        bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
-
-        /// Set the cached penetration impulse
-        void setPenetrationImpulse(decimal impulse);
-
         /// Return true if the contact is a resting contact
         bool getIsRestingContact() const;
 
-        /// Set the mIsRestingContact variable
+        /// Return the next contact point in the linked list
-        void setIsRestingContact(bool isRestingContact);
+        ContactPoint* getNext() const;
 
         /// Return the penetration depth
         decimal getPenetrationDepth() const;
 
         /// Return the number of bytes used by the contact point
         size_t getSizeInBytes() const;
+
+        // Friendship
+        friend class ContactManifold;
+        friend class ContactManifoldSet;
+        friend class ContactSolver;
 };
 
 // Return the normal vector of the contact
@@ -180,6 +206,26 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
     mIsRestingContact = isRestingContact;
 }
 
+// Return true if the contact point is obselete
+inline bool ContactPoint::getIsObselete() const {
+    return mIsObselete;
+}
+
+// Set to true to make the contact point obselete
+inline void ContactPoint::setIsObselete(bool isObselete) {
+    mIsObselete = isObselete;
+}
+
+// Return the next contact point in the linked list
+inline ContactPoint* ContactPoint::getNext() const {
+   return mNext;
+}
+
+// Set the next contact point in the linked list
+inline void ContactPoint::setNext(ContactPoint* next) {
+    mNext = next;
+}
+
 // Return the penetration depth of the contact
 inline decimal ContactPoint::getPenetrationDepth() const {
     return mPenetrationDepth;

src/engine/CollisionWorld.h

@@ -233,64 +233,6 @@ inline void CollisionWorld::testOverlap(CollisionBody* body, OverlapCallback* ov
     mCollisionDetection.testOverlap(body, overlapCallback, categoryMaskBits);
 }
 
-// Class CollisionCallback
-/**
- * This class can be used to register a callback for collision test queries.
- * You should implement your own class inherited from this one and implement
- * the notifyContact() method. This method will called each time a contact
- * point is reported.
- */
-class CollisionCallback {
-
-    public:
-
-        struct CollisionCallbackInfo {
-
-            public:
-                const ContactManifoldInfo& contactManifold;
-                CollisionBody* body1;
-                CollisionBody* body2;
-                const ProxyShape* proxyShape1;
-                const ProxyShape* proxyShape2;
-
-                // Constructor
-                CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2,
-                                      const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
-                    contactManifold(manifold), body1(b1), body2(b2),
-                    proxyShape1(proxShape1), proxyShape2(proxShape2) {
-
-                }
-        };
-
-        /// Destructor
-        virtual ~CollisionCallback() {
-
-        }
-
-        /// This method will be called for each reported contact point
-        virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
-};
-
-// Class OverlapCallback
-/**
- * This class can be used to register a callback for collision overlap queries.
- * You should implement your own class inherited from this one and implement
- * the notifyOverlap() method. This method will called each time a contact
- * point is reported.
- */
-class OverlapCallback {
-
-    public:
-
-        /// Destructor
-        virtual ~OverlapCallback() {
-
-        }
-
-        /// This method will be called for each reported overlapping bodies
-        virtual void notifyOverlap(CollisionBody* collisionBody)=0;
-};
-
 }
 
  #endif

src/engine/ContactSolver.cpp

@@ -148,10 +148,8 @@ void ContactSolver::initializeForIsland(Island* island) {
         const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
 
         // For each  contact point of the contact manifold
-        for (uint c=0; c<externalManifold->getNbContactPoints(); c++) {
+        ContactPoint* externalContact = externalManifold->getContactPoints();
-
+        while (externalContact != nullptr) {
-            // Get a contact point
-            ContactPoint* externalContact = externalManifold->getContactPoint(c);
 
             // Get the contact point on the two bodies
             Vector3 p1 = externalContact->getWorldPointOnBody1();
@@ -200,6 +198,8 @@ void ContactSolver::initializeForIsland(Island* island) {
             mContactConstraints[mNbContactManifolds].normal += mContactPoints[mNbContactPoints].normal;
 
             mNbContactPoints++;
+
+            externalContact = externalContact->getNext();
         }
 
         mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);

src/engine/DynamicsWorld.cpp

@@ -683,9 +683,9 @@ void DynamicsWorld::computeIslands() {
             // For each contact manifold in which the current body is involded
             ContactManifoldListElement* contactElement;
             for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
-                 contactElement = contactElement->next) {
+                 contactElement = contactElement->getNext()) {
 
-                ContactManifold* contactManifold = contactElement->contactManifold;
+                ContactManifold* contactManifold = contactElement->getContactManifold();
 
                 assert(contactManifold->getNbContactPoints() > 0);
 
@@ -842,12 +842,13 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
 
         // For each contact manifold of the pair
         const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
-        for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
+        ContactManifold* manifold = manifoldSet.getContactManifolds();
-
+        while (manifold != nullptr) {
-            ContactManifold* manifold = manifoldSet.getContactManifold(i);
 
             // Get the contact manifold
             contactManifolds.push_back(manifold);
+
+            manifold = manifold->getNext();
         }
     }
 

src/engine/EventListener.h

@@ -27,7 +27,7 @@
 #define REACTPHYSICS3D_EVENT_LISTENER_H
 
 // Libraries
-#include "collision/ContactManifoldInfo.h"
+#include "collision/CollisionCallback.h"
 
 namespace reactphysics3d {
 
@@ -49,17 +49,11 @@ class EventListener {
         /// Destructor
         virtual ~EventListener() = default;
 
-        /// Called when a new contact point is found between two bodies that were separated before
-        /**
-         * @param contact Information about the contact
-         */
-        virtual void beginContact(const ContactManifoldInfo& contactManifold) {}
-
         /// Called when a new contact point is found between two bodies
         /**
          * @param contact Information about the contact
          */
-        virtual void newContact(const ContactManifoldInfo& contactManifold) {}
+        virtual void newContact(const CollisionCallback::CollisionCallbackInfo& collisionInfo) {}
 
         /// Called at the beginning of an internal tick of the simulation step.
         /// Each time the DynamicsWorld::update() method is called, the physics

src/engine/OverlappingPair.cpp

@@ -26,12 +26,108 @@
 // Libraries
 #include <cassert>
 #include "OverlappingPair.h"
+#include "collision/ContactManifoldInfo.h"
 
 using namespace reactphysics3d;
 
 // Constructor
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
-                                 int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
+                                 Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator)
-                : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) {
+                : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr),
+                  mTempMemoryAllocator(temporaryMemoryAllocator) {
     
 }                               
+
+// Create a new potential contact manifold using contact-points from narrow-phase
+void OverlappingPair::addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo) {
+
+    assert(narrowPhaseInfo->contactPoints != nullptr);
+
+    // For each potential contact point to add
+    ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
+    while (contactPoint != nullptr) {
+
+        ContactPointInfo* nextContactPoint = contactPoint->next;
+
+        // Compute the contact normal id
+        short contactNormalId = ContactManifoldSet::computeCubemapNormalId(contactPoint->normal);
+
+        // Look if the contact point correspond to an existing potential manifold
+        // (if the contact point normal is similar to the normal of an existing manifold)
+        ContactManifoldInfo* manifold = mPotentialContactManifolds;
+        bool similarManifoldFound = false;
+        while(manifold != nullptr) {
+
+            // If we have found a corresponding manifold for the new contact point
+            // (a manifold with a similar contact normal direction)
+            if (manifold->getContactNormalId() == contactNormalId) {
+
+                // Add the contact point to the manifold
+                manifold->addContactPoint(contactPoint, contactNormalId);
+
+               similarManifoldFound = true;
+
+               break;
+            }
+
+            manifold = manifold->getNext();
+        }
+
+        // If we have not found an existing manifold with a similar contact normal
+        if (!similarManifoldFound) {
+
+            // Create a new potential contact manifold
+            ContactManifoldInfo* manifoldInfo = new (mTempMemoryAllocator.allocate(sizeof(ContactManifoldInfo)))
+                                            ContactManifoldInfo(mTempMemoryAllocator);
+
+            // Add the manifold into the linked-list of potential contact manifolds
+            manifoldInfo->mNext = mPotentialContactManifolds;
+            mPotentialContactManifolds = manifoldInfo;
+
+            // Add the contact point to the manifold
+            manifoldInfo->addContactPoint(contactPoint, contactNormalId);
+        }
+
+        contactPoint = nextContactPoint;
+    }
+
+    // All the contact point info of the narrow-phase info have been moved
+    // into the potential contacts of the overlapping pair
+    narrowPhaseInfo->contactPoints = nullptr;
+}
+
+// Clear all the potential contact manifolds
+void OverlappingPair::clearPotentialContactManifolds() {
+
+    // Do we need to release memory
+    if (mTempMemoryAllocator.isReleaseNeeded()) {
+
+        ContactManifoldInfo* element = mPotentialContactManifolds;
+        while(element != nullptr) {
+
+            // Remove the proxy collision shape
+            ContactManifoldInfo* elementToRemove = element;
+            element = element->getNext();
+
+            // Delete the element
+            elementToRemove->~ContactManifoldInfo();
+            mTempMemoryAllocator.release(elementToRemove, sizeof(ContactManifoldInfo));
+        }
+    }
+
+    mPotentialContactManifolds = nullptr;
+}
+
+// Reduce the number of contact points of all the potential contact manifolds
+void OverlappingPair::reducePotentialContactManifolds() {
+
+    // For each potential contact manifold
+    ContactManifoldInfo* manifold = mPotentialContactManifolds;
+    while (manifold != nullptr) {
+
+        // Reduce the number of contact points of the manifold
+        manifold->reduce(mContactManifoldSet.getShape1()->getLocalToWorldTransform());
+
+        manifold = manifold->getNext();
+    }
+}

src/engine/OverlappingPair.h

@@ -102,13 +102,19 @@ class OverlappingPair {
         /// Collision information about the last frame (for temporal coherence)
         LastFrameCollisionInfo mLastFrameCollisionInfo;
 
+        /// Linked-list of potential contact manifold
+        ContactManifoldInfo* mPotentialContactManifolds;
+
+        /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo
+        Allocator& mTempMemoryAllocator;
+
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
         OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
-                        int nbMaxContactManifolds, PoolAllocator& memoryAllocator);
+                        Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator);
 
         /// Destructor
         ~OverlappingPair() = default;
@@ -125,9 +131,6 @@ class OverlappingPair {
         /// Return the pointer to second body
         ProxyShape* getShape2() const;
 
-        /// Add a contact manifold
-        void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
-
         /// Return the last frame collision info
         LastFrameCollisionInfo& getLastFrameCollisionInfo();
 
@@ -137,8 +140,32 @@ class OverlappingPair {
         /// Return the a reference to the contact manifold set
         const ContactManifoldSet& getContactManifoldSet();
 
-        /// Clear the contact points of the contact manifold
+        /// Clear all the potential contact manifolds
-        void clearContactPoints();
+        void clearPotentialContactManifolds();
+
+        /// Add potential contact-points from narrow-phase into potential contact manifolds
+        void addPotentialContactPoints(NarrowPhaseInfo* narrowPhaseInfo);
+
+        /// Add a contact to the contact manifold
+        void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
+
+        /// Return a reference to the temporary memory allocator
+        Allocator& getTemporaryAllocator();
+
+        /// Return true if one of the shapes of the pair is a concave shape
+        bool hasConcaveShape() const;
+
+        /// Return a pointer to the first potential contact manifold in the linked-list
+        ContactManifoldInfo* getPotentialContactManifolds();
+
+        /// Reduce the number of contact points of all the potential contact manifolds
+        void reducePotentialContactManifolds();
+
+        /// Make the contact manifolds and contact points obselete
+        void makeContactsObselete();
+
+        /// Clear the obselete contact manifold and contact points
+        void clearObseleteManifoldsAndContactPoints();
 
         /// Return the pair of bodies index
         static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
@@ -162,7 +189,7 @@ inline ProxyShape* OverlappingPair::getShape2() const {
 }                
 
 // Add a contact to the contact manifold
-inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
+inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
     mContactManifoldSet.addContactManifold(contactManifoldInfo);
 }
 
@@ -181,6 +208,12 @@ inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
     return mContactManifoldSet;
 }
 
+// Make the contact manifolds and contact points obselete
+inline void OverlappingPair::makeContactsObselete() {
+
+    mContactManifoldSet.makeContactsObselete();
+}
+
 // Return the pair of bodies index
 inline overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) {
     assert(shape1->mBroadPhaseID >= 0 && shape2->mBroadPhaseID >= 0);
@@ -205,9 +238,25 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
     return indexPair;
 }
 
-// Clear the contact points of the contact manifold
+// Return a reference to the temporary memory allocator
-inline void OverlappingPair::clearContactPoints() {
+inline Allocator& OverlappingPair::getTemporaryAllocator() {
-   mContactManifoldSet.clear();
+    return mTempMemoryAllocator;
+}
+
+// Return true if one of the shapes of the pair is a concave shape
+inline bool OverlappingPair::hasConcaveShape() const {
+    return !getShape1()->getCollisionShape()->isConvex() ||
+           !getShape2()->getCollisionShape()->isConvex();
+}
+
+// Return a pointer to the first potential contact manifold in the linked-list
+inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
+    return mPotentialContactManifolds;
+}
+
+// Clear the obselete contact manifold and contact points
+inline void OverlappingPair::clearObseleteManifoldsAndContactPoints() {
+    mContactManifoldSet.clearObseleteManifoldsAndContactPoints();
 }
 
 }

src/memory/Allocator.h

@@ -49,6 +49,9 @@ class Allocator {
 
         /// Release previously allocated memory.
         virtual void release(void* pointer, size_t size)=0;
+
+        /// Return true if memory needs to be release with this allocator
+        virtual bool isReleaseNeeded() const=0;
 };
 
 }

src/memory/PoolAllocator.h

@@ -138,8 +138,16 @@ class PoolAllocator : public Allocator {
         /// Release previously allocated memory.
         virtual void release(void* pointer, size_t size) override;
 
+        /// Return true if memory needs to be release with this allocator
+        virtual bool isReleaseNeeded() const override;
+
 };
 
+// Return true if memory needs to be release with this allocator
+inline bool PoolAllocator::isReleaseNeeded() const {
+    return true;
+}
+
 }
 
 #endif

src/memory/SingleFrameAllocator.h

@@ -72,10 +72,15 @@ class SingleFrameAllocator : public Allocator {
         /// Reset the marker of the current allocated memory
         virtual void reset();
 
-
+        /// Return true if memory needs to be release with this allocator
-
+        virtual bool isReleaseNeeded() const override;
 };
 
+// Return true if memory needs to be release with this allocator
+inline bool SingleFrameAllocator::isReleaseNeeded() const {
+    return false;
+}
+
 }
 
 #endif

src/reactphysics3d.h

@@ -58,6 +58,8 @@
 #include "collision/PolyhedronMesh.h"
 #include "collision/TriangleVertexArray.h"
 #include "collision/PolygonVertexArray.h"
+#include "collision/CollisionCallback.h"
+#include "collision/OverlapCallback.h"
 #include "constraint/BallAndSocketJoint.h"
 #include "constraint/SliderJoint.h"
 #include "constraint/HingeJoint.h"

testbed/scenes/collisiondetection/CollisionDetectionScene.h

@@ -79,26 +79,34 @@ class ContactManager : public rp3d::CollisionCallback {
         /// This method will be called for each reported contact point
         virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override {
 
+            // For each contact manifold
+            rp3d::ContactManifoldListElement* manifoldElement = collisionCallbackInfo.contactManifoldElements;
+            while (manifoldElement != nullptr) {
+
+                // Get the contact manifold
+                rp3d::ContactManifold* contactManifold = manifoldElement->getContactManifold();
+
                 // For each contact point
-            rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo();
+                rp3d::ContactPoint* contactPoint = contactManifold->getContactPoints();
-            while (contactPointInfo != nullptr) {
+                while (contactPoint != nullptr) {
 
                     // Contact normal
-				rp3d::Vector3 normal = contactPointInfo->normal;
+                    rp3d::Vector3 normal = contactPoint->getNormal();
                     openglframework::Vector3 contactNormal(normal.x, normal.y, normal.z);
 
-                rp3d::Vector3 point1 = contactPointInfo->localPoint1;
+                    rp3d::Vector3 point1 = contactPoint->getLocalPointOnBody1();
                     point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1;
 
                     openglframework::Vector3 position1(point1.x, point1.y, point1.z);
                     mContactPoints.push_back(ContactPoint(position1, contactNormal, openglframework::Color::red()));
 
-                rp3d::Vector3 point2 = contactPointInfo->localPoint2;
+                    rp3d::Vector3 point2 = contactPoint->getLocalPointOnBody2();
                     point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2;
                     openglframework::Vector3 position2(point2.x, point2.y, point2.z);
                     mContactPoints.push_back(ContactPoint(position2, contactNormal, openglframework::Color::blue()));
 
-                contactPointInfo = contactPointInfo->next;
+                    contactPoint = contactPoint->getNext();
+                }
             }
         }
 

testbed/src/SceneDemo.cpp

@@ -342,14 +342,16 @@ std::vector<ContactPoint> SceneDemo::computeContactPointsOfWorld(const rp3d::Dyn
         const rp3d::ContactManifold* manifold = *it;
 
         // For each contact point of the manifold
-        for (uint i=0; i<manifold->getNbContactPoints(); i++) {
+        rp3d::ContactPoint* contactPoint = manifold->getContactPoints();
+        while (contactPoint != nullptr) {
 
-            rp3d::ContactPoint* contactPoint = manifold->getContactPoint(i);
             rp3d::Vector3 point = contactPoint->getWorldPointOnBody1();
 			rp3d::Vector3 normalWorld = contactPoint->getNormal();
 			openglframework::Vector3 normal = openglframework::Vector3(normalWorld.x, normalWorld.y, normalWorld.z);
             ContactPoint contact(openglframework::Vector3(point.x, point.y, point.z), normal, openglframework::Color::red());
             contactPoints.push_back(contact);
+
+            contactPoint = contactPoint->getNext();
         }
 
     }