Remove duplicate contact points in a manifold during collision detection and increase maximum number of contact points in a potential manifold

include/reactphysics3d/collision/ContactManifoldInfo.h

@@ -49,7 +49,7 @@ struct ContactManifoldInfo {
         uint8 nbPotentialContactPoints;
 
         /// Indices of the contact points in the mPotentialContactPoints array
-        uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
+        uint32 potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
 
 
         /// Overlapping pair id

include/reactphysics3d/configuration.h

@@ -124,7 +124,10 @@ constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3;
 constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS;
 
 /// Maximum number of contact points in potential contact manifold
-constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 16;
+constexpr uint16 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 256;
+
+/// Distance threshold to consider that two contact points in a manifold are the same
+constexpr decimal SAME_CONTACT_POINT_DISTANCE_THRESHOLD = decimal(0.01);
 
 /// Current version of ReactPhysics3D
 const std::string RP3D_VERSION = std::string("0.8.0");

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -291,6 +291,9 @@ class CollisionDetectionSystem {
         /// Remove an element in an array (and replace it by the last one in the array)
         void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const;
 
+        /// Remove the duplicated contact points in a given contact manifold
+        void removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold, const Array<ContactPointInfo>& potentialContactPoints) const;
+
     public :
 
         // -------------------- Methods -------------------- //

src/systems/CollisionDetectionSystem.cpp

@@ -1359,6 +1359,9 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array<ContactPair
             }
 
             assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+            // Remove the duplicated contact points in the manifold (if any)
+            removeDuplicatedContactPointsInManifold(manifold, potentialContactPoints);
         }
     }
 }
@@ -1575,6 +1578,37 @@ void CollisionDetectionSystem::removeItemAtInArray(uint array[], uint8 index, ui
     arraySize--;
 }
 
+// Remove the duplicated contact points in a given contact manifold
+void CollisionDetectionSystem::removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold,
+                                                                       const Array<ContactPointInfo>& potentialContactPoints) const {
+
+    RP3D_PROFILE("CollisionDetectionSystem::removeDuplicatedContactPointsInManifold()", mProfiler);
+
+    const decimal distThresholdSqr = SAME_CONTACT_POINT_DISTANCE_THRESHOLD * SAME_CONTACT_POINT_DISTANCE_THRESHOLD;
+
+    // For each contact point of the manifold
+    for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) {
+        for (uint32 j=i+1; j < manifold.nbPotentialContactPoints; j++) {
+
+            const ContactPointInfo& point1 = potentialContactPoints[manifold.potentialContactPointsIndices[i]];
+            const ContactPointInfo& point2 = potentialContactPoints[manifold.potentialContactPointsIndices[j]];
+
+            // Compute the distance between the two contact points
+            const decimal distSqr = (point2.localPoint1 - point1.localPoint1).lengthSquare();
+
+            // We have a found a duplicated contact point
+            if (distSqr < distThresholdSqr) {
+
+                // Remove the duplicated contact point
+                manifold.potentialContactPointsIndices[j] = manifold.potentialContactPointsIndices[manifold.nbPotentialContactPoints-1];
+                manifold.nbPotentialContactPoints--;
+
+                j--;
+            }
+        }
+    }
+}
+
 // Report contacts and triggers
 void CollisionDetectionSystem::reportContactsAndTriggers() {