Add ContactManifoldSet to allow several ContactManifolds per overlapping pair

src/collision/CollisionDetection.cpp

@@ -119,26 +119,32 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
             continue;
         }
 
-        // For each contact manifold of the overlapping pair
+        // For each contact manifold set of the overlapping pair
-        ContactManifold* manifold = pair->getContactManifold();
+        ContactManifoldSet* manifoldSet = pair->getContactManifoldSet();
-        for (uint i=0; i<manifold->getNbContactPoints(); i++) {
+        for (uint j=0; j<manifoldSet->getNbContactManifolds(); j++) {
 
-            ContactPoint* contactPoint = manifold->getContactPoint(i);
+            ContactManifold* manifold = manifoldSet->getContactManifold(j);
 
-            // Create the contact info object for the contact
+            // For each contact manifold of the manifold set
-            ContactPointInfo* contactInfo = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
+            for (uint i=0; i<manifold->getNbContactPoints(); i++) {
-                                         ContactPointInfo(manifold->getShape1(), manifold->getShape2(),
-                                                          contactPoint->getNormal(),
-                                                          contactPoint->getPenetrationDepth(),
-                                                          contactPoint->getLocalPointOnBody1(),
-                                                          contactPoint->getLocalPointOnBody2());
 
-            // Notify the collision callback about this new contact
+                ContactPoint* contactPoint = manifold->getContactPoint(i);
-            if (callback != NULL) callback->notifyContact(*contactInfo);
 
-            // Delete and remove the contact info from the memory allocator
+                // Create the contact info object for the contact
-            contactInfo->~ContactPointInfo();
+                ContactPointInfo* contactInfo = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
-            mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
+                                             ContactPointInfo(manifold->getShape1(), manifold->getShape2(),
+                                                              contactPoint->getNormal(),
+                                                              contactPoint->getPenetrationDepth(),
+                                                              contactPoint->getLocalPointOnBody1(),
+                                                              contactPoint->getLocalPointOnBody2());
+
+                // Notify the collision callback about this new contact
+                if (callback != NULL) callback->notifyContact(*contactInfo);
+
+                // Delete and remove the contact info from the memory allocator
+                contactInfo->~ContactPointInfo();
+                mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
+            }
         }
     }
 }
@@ -162,6 +168,9 @@ void CollisionDetection::computeBroadPhase() {
 void CollisionDetection::computeNarrowPhase() {
 
     PROFILE("CollisionDetection::computeNarrowPhase()");
+
+    // Clear the set of overlapping pairs in narrow-phase contact
+    mContactOverlappingPairs.clear();
     
     // For each possible collision pair of bodies
     map<overlappingpairid, OverlappingPair*>::iterator it;
@@ -252,6 +261,9 @@ void CollisionDetection::computeNarrowPhase() {
             mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
         }
     }
+
+    // Add all the contact manifolds (between colliding bodies) to the bodies
+    addAllContactManifoldsToBodies();
 }
 
 // Compute the narrow-phase collision detection
@@ -259,6 +271,8 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
                                                          const std::set<uint>& shapes1,
                                                          const std::set<uint>& shapes2) {
 
+    mContactOverlappingPairs.clear();
+
     // For each possible collision pair of bodies
     map<overlappingpairid, OverlappingPair*>::iterator it;
     for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
@@ -361,6 +375,9 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
             mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
         }
     }
+
+    // Add all the contact manifolds (between colliding bodies) to the bodies
+    addAllContactManifoldsToBodies();
 }
 
 // Allow the broadphase to notify the collision detection about an overlapping pair.
@@ -382,9 +399,13 @@ 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->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
-                              OverlappingPair(shape1, shape2, mWorld->mMemoryAllocator);
+                              OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
     assert(newPair != NULL);
     std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
             mOverlappingPairs.insert(make_pair(pairID, newPair));
@@ -434,39 +455,55 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
     // Add the contact to the contact manifold set of the corresponding overlapping pair
     overlappingPair->addContact(contact);
 
-    // Add the contact manifold into the list of contact manifolds
+    // Add the overlapping pair into the set of pairs in contact during narrow-phase
-    // of the two bodies involved in the contact
+    overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
+                                                          overlappingPair->getShape2());
+    mContactOverlappingPairs[pairId] = overlappingPair;
+}
 
-    // TODO : It seems that we add the same manifold multiple times to the same body
+void CollisionDetection::addAllContactManifoldsToBodies() {
-    // in case we call createContact() multiple time for different contact points from the
+
-    // same manifold. Check that it is not the case.
+    // For each overlapping pairs in contact during the narrow-phase
-    addContactManifoldToBody(overlappingPair->getContactManifold(),
+    std::map<overlappingpairid, OverlappingPair*>::iterator it;
-                             overlappingPair->getShape1()->getBody(),
+    for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
-                             overlappingPair->getShape2()->getBody());
+
+        // Add all the contact manifolds of the pair into the list of contact manifolds
+        // of the two bodies involved in the contact
+        addContactManifoldToBody(it->second);
+    }
 }
 
 // Add a contact manifold to the linked list of contact manifolds of the two bodies involved
 // in the corresponding contact
-void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifold,
+void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
-                                                  CollisionBody* body1, CollisionBody* body2) {
 
-    assert(contactManifold != NULL);
+    assert(pair != NULL);
 
-    // Add the contact manifold at the beginning of the linked
+    CollisionBody* body1 = pair->getShape1()->getBody();
-    // list of contact manifolds of the first body
+    CollisionBody* body2 = pair->getShape2()->getBody();
-    void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+    ContactManifoldSet* manifoldSet = pair->getContactManifoldSet();
-    ContactManifoldListElement* listElement1 = new (allocatedMemory1)
-                                                  ContactManifoldListElement(contactManifold,
-                                                                     body1->mContactManifoldsList);
-    body1->mContactManifoldsList = listElement1;
 
-    // Add the contact manifold at the beginning of the linked
+    // For each contact manifold in the set of manifolds in the pair
-    // list of the contact manifolds of the second body
+    for (int i=0; i<manifoldSet->getNbContactManifolds(); i++) {
-    void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+
-    ContactManifoldListElement* listElement2 = new (allocatedMemory2)
+        ContactManifold* contactManifold = manifoldSet->getContactManifold(i);
-                                                  ContactManifoldListElement(contactManifold,
+
-                                                                     body2->mContactManifoldsList);
+        // Add the contact manifold at the beginning of the linked
-    body2->mContactManifoldsList = listElement2;
+        // list of contact manifolds of the first body
+        void* allocatedMemory1 = mWorld->mMemoryAllocator.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->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+        ContactManifoldListElement* listElement2 = new (allocatedMemory2)
+                                                      ContactManifoldListElement(contactManifold,
+                                                                         body2->mContactManifoldsList);
+        body2->mContactManifoldsList = listElement2;
+    }
 }
 
 // Delete all the contact points in the currently overlapping pairs

src/collision/CollisionDetection.h

@@ -78,6 +78,9 @@ class CollisionDetection {
         /// Broad-phase overlapping pairs
         std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
 
+        /// Overlapping pairs in contact (during the current Narrow-phase collision detection)
+        std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
+
         /// Broad-phase algorithm
         BroadPhaseAlgorithm mBroadPhaseAlgorithm;
 
@@ -111,14 +114,16 @@ class CollisionDetection {
 
         /// Add a contact manifold to the linked list of contact manifolds of the two bodies
         /// involed in the corresponding contact.
-        void addContactManifoldToBody(ContactManifold* contactManifold,
+        void addContactManifoldToBody(OverlappingPair* pair);
-                                      CollisionBody *body1, CollisionBody *body2);
 
         /// Delete all the contact points in the currently overlapping pairs
         void clearContactPoints();
 
         /// Fill-in the collision detection matrix
         void fillInCollisionMatrix();
+
+        /// Add all the contact manifold of colliding pairs to their bodies
+        void addAllContactManifoldsToBodies();
    
     public :
 

src/collision/ContactManifold.cpp

@@ -31,9 +31,10 @@ using namespace reactphysics3d;
 
 // Constructor
 ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
-                                 MemoryAllocator& memoryAllocator)
+                                 MemoryAllocator& memoryAllocator, short normalDirectionId)
-                : mShape1(shape1), mShape2(shape2), mNbContactPoints(0), mFrictionImpulse1(0.0),
+                : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
-                  mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
+                  mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
+                  mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
                   mMemoryAllocator(memoryAllocator) {
     
 }

src/collision/ContactManifold.h

@@ -98,6 +98,9 @@ class ContactManifold {
         /// Contact points in the manifold
         ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
 
+        /// Normal direction Id (Unique Id representing the normal direction)
+        short int mNormalDirectionId;
+
         /// Number of contacts in the cache
         uint mNbContactPoints;
 
@@ -147,9 +150,6 @@ class ContactManifold {
 
         /// Return true if the contact manifold has already been added into an island
         bool isAlreadyInIsland() const;
-
-        /// Return the normalized averaged normal vector
-        Vector3 getAverageContactNormal() const;
         
     public:
 
@@ -157,7 +157,7 @@ class ContactManifold {
 
         /// Constructor
         ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
-                        MemoryAllocator& memoryAllocator);
+                        MemoryAllocator& memoryAllocator, short int normalDirectionId);
 
         /// Destructor
         ~ContactManifold();
@@ -174,6 +174,9 @@ 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;
+
         /// Add a contact point to the manifold
         void addContactPoint(ContactPoint* contact);
 
@@ -219,6 +222,12 @@ class ContactManifold {
         /// 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;
+
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
@@ -246,6 +255,11 @@ 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 uint ContactManifold::getNbContactPoints() const {
     return mNbContactPoints;
@@ -323,6 +337,20 @@ inline Vector3 ContactManifold::getAverageContactNormal() const {
     return averageNormal.getUnit();
 }
 
+// Return the largest depth of all the contact points
+inline decimal ContactManifold::getLargestContactDepth() const {
+    decimal largestDepth = 0.0f;
+
+    for (int i=0; i<mNbContactPoints; i++) {
+        decimal depth = mContactPoints[i]->getPenetrationDepth();
+        if (depth > largestDepth) {
+            largestDepth = depth;
+        }
+    }
+
+    return largestDepth;
+}
+
 }
 #endif
 

src/collision/ContactManifoldSet.cpp

@@ -39,38 +39,127 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
 // Destructor
 ContactManifoldSet::~ContactManifoldSet() {
 
+    // Clear all the contact manifolds
+    clear();
 }
 
 // Add a contact point to the manifold set
 void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
 
+    // Compute an Id corresponding to the normal direction (using a cubemap)
+    short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
+
     // If there is no contact manifold yet
     if (mNbManifolds == 0) {
 
-        createManifold();
+        createManifold(normalDirectionId);
         mManifolds[0]->addContactPoint(contact);
 
         return;
     }
 
+    // Select the manifold with the most similar normal (if exists)
+    int similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
 
+    // If a similar manifold has been found
+    if (similarManifoldIndex != -1) {
+
+        // Add the contact point to that similar manifold
+        mManifolds[similarManifoldIndex]->addContactPoint(contact);
+
+        return;
+    }
+
+    // If the maximum number of manifold has not been reached yet
+    if (mNbManifolds < mNbMaxManifolds) {
+
+        // Create a new manifold for the contact point
+        createManifold(normalDirectionId);
+        mManifolds[mNbManifolds-1]->addContactPoint(contact);
+
+        return;
+    }
+
+    // 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 keep the manifolds
+    // with the largest contact depth among their points
+    int smallestDepthIndex = -1;
+    decimal minDepth = contact->getPenetrationDepth();
+    assert(mNbManifolds == mNbMaxManifolds);
+    for (int i=0; i<mNbManifolds; i++) {
+        decimal depth = mManifolds[i]->getLargestContactDepth();
+        if (depth < minDepth) {
+            minDepth = depth;
+            smallestDepthIndex = i;
+        }
+    }
+
+    // If we do not want to keep to new manifold (not created yet) with the
+    // new contact point
+    if (smallestDepthIndex == -1) {
+        return;
+    }
+
+    assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
+
+    // Here we need to replace an existing manifold with a new one (that contains
+    // the new contact point)
+    removeManifold(smallestDepthIndex);
+    createManifold(normalDirectionId);
+    mManifolds[mNbManifolds-1]->addContactPoint(contact);
+
+    return;
 }
 
 // Return the index of the contact manifold with a similar average normal.
 // If no manifold has close enough average normal, it returns -1
-int ContactManifoldSet::selectManifoldWithSimilarNormal(const Vector3& normal) {
+int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
 
-    decimal maxDotProduct;
+    // Return the Id of the manifold with the same normal direction id (if exists)
-    int indexManifold = -1;
     for (int i=0; i<mNbManifolds; i++) {
-        decimal dotProduct = normal.dot(mManifolds[i]->getAverageContactNormal());
+        if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
-        if (dotProduct > maxDotProduct) {
+            return i;
-            maxDotProduct = dotProduct;
-            indexManifold = i;
         }
     }
 
-    return indexManifold;
+    return -1;
+}
+
+// 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 {
+
+    assert(normal.lengthSquare() > MACHINE_EPSILON);
+
+    int faceNo;
+    decimal u, v;
+    decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
+    Vector3 normalScaled = normal / max;
+
+    if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
+        faceNo = normalScaled.x > 0 ? 0 : 1;
+        u = normalScaled.y;
+        v = normalScaled.z;
+    }
+    else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
+        faceNo = normalScaled.y > 0 ? 2 : 3;
+        u = normalScaled.x;
+        v = normalScaled.z;
+    }
+    else {
+        faceNo = normalScaled.z > 0 ? 4 : 5;
+        u = normalScaled.x;
+        v = normalScaled.y;
+    }
+
+    int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+    int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
+    if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
+    if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
+
+    const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
+    return faceNo * 200 + indexU * nbSubDivInFace + indexV;
 }
 
 // Update the contact manifolds
@@ -84,14 +173,20 @@ void ContactManifoldSet::update(const Transform& transform1, const Transform& tr
 // Clear the contact manifold set
 void ContactManifoldSet::clear() {
 
+    // Destroy all the contact manifolds
+    for (int i=mNbManifolds-1; i>=0; i--) {
+        removeManifold(i);
+    }
+
+    assert(mNbManifolds == 0);
 }
 
 // Create a new contact manifold and add it to the set
-void ContactManifoldSet::createManifold() {
+void ContactManifoldSet::createManifold(short int normalDirectionId) {
-    assert(mNbManifolds < MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET);
+    assert(mNbManifolds < mNbMaxManifolds);
 
     mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
-                                    ContactManifold(mShape1, mShape2, mMemoryAllocator);
+                                    ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
     mNbManifolds++;
 }
 
@@ -104,5 +199,9 @@ void ContactManifoldSet::removeManifold(int index) {
     mManifolds[index]->~ContactManifold();
     mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
 
+    for (int i=index; (i+1) < mNbManifolds; i++) {
+        mManifolds[i] = mManifolds[i+1];
+    }
+
     mNbManifolds--;
 }

src/collision/ContactManifoldSet.h

@@ -32,7 +32,8 @@
 namespace reactphysics3d {
 
 // Constants
-const uint MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3;   // Maximum number of contact manifolds in the set
+const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3;   // Maximum number of contact manifolds in the set
+const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 4;    // N Number for the N x N subdivisions of the cubemap
 
 // Class ContactManifoldSet
 /**
@@ -68,11 +69,19 @@ class ContactManifoldSet {
         // -------------------- Methods -------------------- //
 
         /// Create a new contact manifold and add it to the set
-        void createManifold();
+        void createManifold(short normalDirectionId);
 
         /// Remove a contact manifold from the set
         void removeManifold(int index);
 
+        // 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;
+
     public:
 
         // -------------------- Methods -------------------- //

src/collision/shapes/CollisionShape.h

@@ -106,6 +106,10 @@ 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;
@@ -125,6 +129,19 @@ inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
     return shapeType != CONCAVE_MESH;
 }
 
+// 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/configuration.h

@@ -128,10 +128,13 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
 /// followin constant with the linear velocity and the elapsed time between two frames.
 const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
 
-/// This value is used to compare the normal of a contact point and the average
+/// Maximum number of contact manifolds in an overlapping pair that involves two
-/// normal of a whole contact manifold to decide if the point should be part
+/// convex collision shapes.
-/// of the manifold or not
+const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
-const decimal MANIFOLD_SIMILAR_NORMAL_DOTPRODUCT_THRESHOLD = 0.99f;
+
+/// Maximum number of contact manifolds in an overlapping pair that involves at
+/// least one concave collision shape.
+const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
 
 }
 

src/engine/DynamicsWorld.cpp

@@ -984,8 +984,15 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
 
         OverlappingPair* pair = it->second;
 
-        // Get the contact manifold
+        // For each contact manifold of the pair
-        contactManifolds.push_back(pair->getContactManifold());
+        ContactManifoldSet* manifoldSet = pair->getContactManifoldSet();
+        for (int i=0; i<manifoldSet->getNbContactManifolds(); i++) {
+
+            ContactManifold* manifold = manifoldSet->getContactManifold(i);
+
+            // Get the contact manifold
+            contactManifolds.push_back(manifold);
+        }
     }
 
     // Return all the contact manifold

src/engine/OverlappingPair.cpp

@@ -31,9 +31,9 @@ using namespace reactphysics3d;
 
 // Constructor
 OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
-                                 MemoryAllocator& memoryAllocator)
+                                 int nbMaxContactManifolds, MemoryAllocator& memoryAllocator)
                 : mShape1(shape1), mShape2(shape2),
-                  mContactManifoldSet(shape1, shape2, memoryAllocator),
+                  mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
                   mCachedSeparatingAxis(1.0, 1.0, 1.0) {
     
 }

src/engine/OverlappingPair.h

@@ -76,7 +76,8 @@ class OverlappingPair {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator);
+        OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
+                        int nbMaxContactManifolds, MemoryAllocator& memoryAllocator);
 
         /// Destructor
         ~OverlappingPair();
@@ -102,8 +103,8 @@ class OverlappingPair {
         /// Return the number of contacts in the cache
         uint getNbContactPoints() const;
 
-        /// Return the contact manifold
+        /// Return the contact manifold set
-        ContactManifold* getContactManifold();
+        ContactManifoldSet* getContactManifoldSet();
 
         /// Clear the contact points of the contact manifold
         void clearContactPoints();
@@ -157,7 +158,7 @@ inline uint OverlappingPair::getNbContactPoints() const {
 }
 
 // Return the contact manifold
-inline ContactManifold* OverlappingPair::getContactManifoldSet() {
+inline ContactManifoldSet* OverlappingPair::getContactManifoldSet() {
     return &mContactManifoldSet;
 }