Merge branch 'contacts-fix' into develop

include/reactphysics3d/collision/ContactManifoldInfo.h

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

include/reactphysics3d/collision/ContactPair.h

@@ -44,14 +44,14 @@ struct ContactPair {
 
         // -------------------- Attributes -------------------- //
 
+        /// Reference to the memory allocator
+        MemoryAllocator& allocator;
+
         /// Overlapping pair Id
         uint64 pairId;
 
-        /// Number of potential contact manifolds
-        uint8 nbPotentialContactManifolds;
-
         /// Indices of the potential contact manifolds
-        uint potentialContactManifoldsIndices[NB_MAX_POTENTIAL_CONTACT_MANIFOLDS];
+        Array<uint> potentialContactManifoldsIndices;
 
         /// Entity of the first body of the contact
         Entity body1Entity;
@@ -93,8 +93,9 @@ struct ContactPair {
 
         /// Constructor
         ContactPair(uint64 pairId, Entity body1Entity, Entity body2Entity, Entity collider1Entity,
-                    Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger)
+                    Entity collider2Entity, uint contactPairIndex, bool collidingInPreviousFrame, bool isTrigger, MemoryAllocator& allocator)
-            : pairId(pairId), nbPotentialContactManifolds(0), body1Entity(body1Entity), body2Entity(body2Entity),
+            : allocator(allocator), pairId(pairId), potentialContactManifoldsIndices(allocator, 1),
+              body1Entity(body1Entity), body2Entity(body2Entity),
               collider1Entity(collider1Entity), collider2Entity(collider2Entity),
               isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
               contactPointsIndex(0), nbToTalContactPoints(0), collidingInPreviousFrame(collidingInPreviousFrame), isTrigger(isTrigger) {
@@ -103,10 +104,9 @@ struct ContactPair {
 
         // Remove a potential manifold at a given index in the array
         void removePotentialManifoldAtIndex(uint index) {
-            assert(index < nbPotentialContactManifolds);
+            assert(index < potentialContactManifoldsIndices.size());
 
-            potentialContactManifoldsIndices[index] = potentialContactManifoldsIndices[nbPotentialContactManifolds - 1];
+            potentialContactManifoldsIndices.removeAtAndReplaceByLast(index);
-            nbPotentialContactManifolds--;
         }
 };
 

include/reactphysics3d/configuration.h

@@ -120,11 +120,8 @@ constexpr uint8 NB_MAX_CONTACT_POINTS_IN_NARROWPHASE_INFO = 16;
 /// Maximum number of contact manifolds in an overlapping pair
 constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3;
 
-/// Maximum number of potential contact manifolds in an overlapping pair
+/// Distance threshold to consider that two contact points in a manifold are the same
-constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS;
+constexpr decimal SAME_CONTACT_POINT_DISTANCE_THRESHOLD = decimal(0.01);
-
-/// Maximum number of contact points in potential contact manifold
-constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 16;
 
 /// Current version of ReactPhysics3D
 const std::string RP3D_VERSION = std::string("0.8.0");

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -265,6 +265,10 @@ class CollisionDetectionSystem {
         void reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
                                  const Array<ContactPointInfo>& potentialContactPoints) const;
 
+        /// Remove the duplicated contact points in a given contact manifold
+        void removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold,
+                                                     const Array<ContactPointInfo>& potentialContactPoints) const;
+
         /// Report contacts
         void reportContacts(CollisionCallback& callback, Array<ContactPair>* contactPairs,
                             Array<ContactManifold>* manifolds, Array<ContactPoint>* contactPoints, Array<ContactPair>& lostContactPairs);
@@ -289,7 +293,7 @@ class CollisionDetectionSystem {
         void filterOverlappingPairs(Entity body1Entity, Entity body2Entity, Array<uint64>& convexPairs, Array<uint64>& concavePairs) const;
 
         /// Remove an element in an array (and replace it by the last one in the array)
-        void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const;
+        void removeItemAtInArray(Array<uint>& array, uint32 index) const;
 
     public :
 

src/engine/PhysicsWorld.cpp

@@ -845,11 +845,11 @@ void PhysicsWorld::createIslands() {
 
                     mProcessContactPairsOrderIslands.add(contactPairIndex);
 
-                    assert(pair.nbPotentialContactManifolds > 0);
+                    assert(pair.potentialContactManifoldsIndices.size() > 0);
-                    nbTotalManifolds += pair.nbPotentialContactManifolds;
+                    nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
 
                     // Add the contact manifold into the island
-                    mIslands.nbContactManifolds[islandIndex] += pair.nbPotentialContactManifolds;
+                    mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
                     pair.isAlreadyInIsland = true;
 
                     // Check if the other body has already been added to the island

src/systems/CollisionDetectionSystem.cpp

@@ -192,7 +192,7 @@ void CollisionDetectionSystem::addLostContactPair(OverlappingPairs::OverlappingP
 
     // Create a lost contact pair
     ContactPair lostContactPair(overlappingPair.pairID, body1Entity, body2Entity, overlappingPair.collider1, overlappingPair.collider2, mLostContactPairs.size(),
-                                true, isTrigger);
+                                true, isTrigger, mMemoryManager.getHeapAllocator());
     mLostContactPairs.add(lostContactPair);
 }
 
@@ -742,7 +742,7 @@ void CollisionDetectionSystem::computeOverlapSnapshotContactPairs(NarrowPhaseInf
                 const bool isTrigger = mCollidersComponents.mIsTrigger[collider1Index] || mCollidersComponents.mIsTrigger[collider2Index];
 
                 // Create a new contact pair
-                ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false);
+                ContactPair contactPair(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId, body1Entity, body2Entity, collider1Entity, collider2Entity, contactPairs.size(), isTrigger, false, mMemoryManager.getHeapAllocator());
                 contactPairs.add(contactPair);
 
                 setOverlapContactPairId.add(narrowPhaseInfoBatch.narrowPhaseInfos[i].overlappingPairId);
@@ -840,27 +840,27 @@ void CollisionDetectionSystem::createContacts() {
         ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
 
         contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
-        contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
+        contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
         contactPair.contactPointsIndex = mCurrentContactPoints->size();
 
         // For each potential contact manifold of the pair
-        for (uint m=0; m < contactPair.nbPotentialContactManifolds; m++) {
+        for (uint m=0; m < contactPair.nbContactManifolds; m++) {
 
             ContactManifoldInfo& potentialManifold = mPotentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
 
             // Start index and number of contact points for this manifold
             const uint32 contactPointsIndex = mCurrentContactPoints->size();
-            const int8 nbContactPoints = static_cast<int8>(potentialManifold.nbPotentialContactPoints);
+            const uint32 nbContactPoints = static_cast<uint32>(potentialManifold.potentialContactPointsIndices.size());
             contactPair.nbToTalContactPoints += nbContactPoints;
 
             // Create and add the contact manifold
             mCurrentContactManifolds->emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
                                               contactPair.collider2Entity, contactPointsIndex, nbContactPoints);
 
-            assert(potentialManifold.nbPotentialContactPoints > 0);
+            assert(potentialManifold.potentialContactPointsIndices.size() > 0);
 
             // For each contact point of the manifold
-            for (uint c=0; c < potentialManifold.nbPotentialContactPoints; c++) {
+            for (uint c=0; c < nbContactPoints; c++) {
 
                 ContactPointInfo& potentialContactPoint = mPotentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
 
@@ -948,30 +948,30 @@ void CollisionDetectionSystem::createSnapshotContacts(Array<ContactPair>& contac
     for (uint32 p=0; p < nbContactPairs; p++) {
 
         ContactPair& contactPair = contactPairs[p];
-        assert(contactPair.nbPotentialContactManifolds > 0);
+        assert(contactPair.potentialContactManifoldsIndices.size() > 0);
 
         contactPair.contactManifoldsIndex = contactManifolds.size();
-        contactPair.nbContactManifolds = contactPair.nbPotentialContactManifolds;
+        contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
         contactPair.contactPointsIndex = contactPoints.size();
 
         // For each potential contact manifold of the pair
-        for (uint32 m=0; m < contactPair.nbPotentialContactManifolds; m++) {
+        for (uint32 m=0; m < contactPair.nbContactManifolds; m++) {
 
             ContactManifoldInfo& potentialManifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[m]];
 
             // Start index and number of contact points for this manifold
             const uint32 contactPointsIndex = contactPoints.size();
-            const uint8 nbContactPoints = potentialManifold.nbPotentialContactPoints;
+            const uint32 nbContactPoints = static_cast<uint32>(potentialManifold.potentialContactPointsIndices.size());
             contactPair.nbToTalContactPoints += nbContactPoints;
 
             // Create and add the contact manifold
             contactManifolds.emplace(contactPair.body1Entity, contactPair.body2Entity, contactPair.collider1Entity,
                                      contactPair.collider2Entity, contactPointsIndex, nbContactPoints);
 
-            assert(potentialManifold.nbPotentialContactPoints > 0);
+            assert(nbContactPoints > 0);
 
             // For each contact point of the manifold
-            for (uint32 c=0; c < potentialManifold.nbPotentialContactPoints; c++) {
+            for (uint32 c=0; c < nbContactPoints; c++) {
 
                 ContactPointInfo& potentialContactPoint = potentialContactPoints[potentialManifold.potentialContactPointsIndices[c]];
 
@@ -1171,13 +1171,13 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                 const uint32 newContactPairIndex = contactPairs->size();
 
                 contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
-                                      newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger);
+                                      newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator());
 
                 ContactPair* pairContact = &((*contactPairs)[newContactPairIndex]);
 
                 // Create a new potential contact manifold for the overlapping pair
                 uint32 contactManifoldIndex = static_cast<uint>(potentialContactManifolds.size());
-                potentialContactManifolds.emplace(pairId);
+                potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator());
                 ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
 
                 const uint32 contactPointIndexStart = static_cast<uint>(potentialContactPoints.size());
@@ -1185,23 +1185,18 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                 // Add the potential contacts
                 for (uint j=0; j < narrowPhaseInfoBatch.narrowPhaseInfos[i].nbContactPoints; j++) {
 
-                    if (contactManifoldInfo.nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
+                    // Add the contact point to the manifold
+                    contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndexStart + j);
 
-                        // Add the contact point to the manifold
+                    // Add the contact point to the array of potential contact points
-                        contactManifoldInfo.potentialContactPointsIndices[contactManifoldInfo.nbPotentialContactPoints] = contactPointIndexStart + j;
+                    const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j];
-                        contactManifoldInfo.nbPotentialContactPoints++;
 
-                        // Add the contact point to the array of potential contact points
+                    potentialContactPoints.add(contactPoint);
-                        const ContactPointInfo& contactPoint = narrowPhaseInfoBatch.narrowPhaseInfos[i].contactPoints[j];
-
-                        potentialContactPoints.add(contactPoint);
-                    }
                 }
 
                 // Add the contact manifold to the overlapping pair contact
                 assert(pairId == contactManifoldInfo.pairId);
-                pairContact->potentialContactManifoldsIndices[0] = contactManifoldIndex;
+                pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
-                pairContact->nbPotentialContactManifolds = 1;
             }
             else {
 
@@ -1218,7 +1213,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
 
                     const uint32 newContactPairIndex = contactPairs->size();
                     contactPairs->emplace(pairId, body1Entity, body2Entity, collider1Entity, collider2Entity,
-                                                       newContactPairIndex, overlappingPair->collidingInPreviousFrame , isTrigger);
+                                                       newContactPairIndex, overlappingPair->collidingInPreviousFrame, isTrigger, mMemoryManager.getHeapAllocator());
                     pairContact = &((*contactPairs)[newContactPairIndex]);
                     mapPairIdToContactPairIndex.add(Pair<uint64, uint>(pairId, newContactPairIndex));
 
@@ -1246,53 +1241,48 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                     bool similarManifoldFound = false;
 
                     // For each contact manifold of the overlapping pair
-                    for (uint m=0; m < pairContact->nbPotentialContactManifolds; m++) {
+                    const uint32 nbPotentialManifolds = pairContact->potentialContactManifoldsIndices.size();
+                    for (uint m=0; m < nbPotentialManifolds; m++) {
 
                        uint contactManifoldIndex = pairContact->potentialContactManifoldsIndices[m];
 
-                       if (potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints < NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD) {
+                       // Get the first contact point of the current manifold
+                       assert(potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.size() > 0);
+                       const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
+                       const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex];
 
-                           // Get the first contact point of the current manifold
+                        // If we have found a corresponding manifold for the new contact point
-                           assert(potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints > 0);
+                        // (a manifold with a similar contact normal direction)
-                           const uint manifoldContactPointIndex = potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[0];
+                        if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
-                           const ContactPointInfo& manifoldContactPoint = potentialContactPoints[manifoldContactPointIndex];
 
-                            // If we have found a corresponding manifold for the new contact point
+                            // Add the contact point to the manifold
-                            // (a manifold with a similar contact normal direction)
+                            potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices.add(contactPointIndex);
-                            if (manifoldContactPoint.normal.dot(contactPoint.normal) >= mWorld->mConfig.cosAngleSimilarContactManifold) {
 
-                                // Add the contact point to the manifold
+                            similarManifoldFound = true;
-                                potentialContactManifolds[contactManifoldIndex].potentialContactPointsIndices[potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints] = contactPointIndex;
-                                potentialContactManifolds[contactManifoldIndex].nbPotentialContactPoints++;
 
-                                similarManifoldFound = true;
+                            break;
-
+                        }
-                                break;
-                            }
-                       }
                     }
 
                     // If we have not found a manifold with a similar contact normal for the contact point
-                    if (!similarManifoldFound && pairContact->nbPotentialContactManifolds < NB_MAX_POTENTIAL_CONTACT_MANIFOLDS) {
+                    if (!similarManifoldFound) {
 
                         // Create a new potential contact manifold for the overlapping pair
                         uint32 contactManifoldIndex = potentialContactManifolds.size();
-                        potentialContactManifolds.emplace(pairId);
+                        potentialContactManifolds.emplace(pairId, mMemoryManager.getHeapAllocator());
                         ContactManifoldInfo& contactManifoldInfo = potentialContactManifolds[contactManifoldIndex];
 
                         // Add the contact point to the manifold
-                        contactManifoldInfo.potentialContactPointsIndices[0] = contactPointIndex;
+                        contactManifoldInfo.potentialContactPointsIndices.add(contactPointIndex);
-                        contactManifoldInfo.nbPotentialContactPoints = 1;
 
                         assert(pairContact != nullptr);
 
                         // Add the contact manifold to the overlapping pair contact
                         assert(pairContact->pairId == contactManifoldInfo.pairId);
-                        pairContact->potentialContactManifoldsIndices[pairContact->nbPotentialContactManifolds] = contactManifoldIndex;
+                        pairContact->potentialContactManifoldsIndices.add(contactManifoldIndex);
-                        pairContact->nbPotentialContactManifolds++;
                     }
 
-                    assert(pairContact->nbPotentialContactManifolds > 0);
+                    assert(pairContact->potentialContactManifoldsIndices.size() > 0);
                 }
             }
 
@@ -1315,12 +1305,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array<ContactPair
         ContactPair& contactPair = (*contactPairs)[i];
 
         // While there are too many manifolds in the contact pair
-        while(contactPair.nbPotentialContactManifolds > NB_MAX_CONTACT_MANIFOLDS) {
+        while(contactPair.potentialContactManifoldsIndices.size() > NB_MAX_CONTACT_MANIFOLDS) {
 
             // Look for a manifold with the smallest contact penetration depth.
             decimal minDepth = DECIMAL_LARGEST;
             int minDepthManifoldIndex = -1;
-            for (uint32 j=0; j < contactPair.nbPotentialContactManifolds; j++) {
+            for (uint32 j=0; j < contactPair.potentialContactManifoldsIndices.size(); j++) {
 
                 ContactManifoldInfo& manifold = potentialContactManifolds[contactPair.potentialContactManifoldsIndices[j]];
 
@@ -1345,12 +1335,12 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array<ContactPair
         const ContactPair& pairContact = (*contactPairs)[i];
 
         // For each potential contact manifold
-        for (uint32 j=0; j < pairContact.nbPotentialContactManifolds; j++) {
+        for (uint32 j=0; j < pairContact.potentialContactManifoldsIndices.size(); j++) {
 
             ContactManifoldInfo& manifold = potentialContactManifolds[pairContact.potentialContactManifoldsIndices[j]];
 
             // If there are two many contact points in the manifold
-            if (manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
+            if (manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD) {
 
                 Transform shape1LocalToWorldTransoform = mCollidersComponents.getLocalToWorldTransform(pairContact.collider1Entity);
 
@@ -1358,20 +1348,25 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array<ContactPair
                 reduceContactPoints(manifold, shape1LocalToWorldTransoform, potentialContactPoints);
             }
 
-            assert(manifold.nbPotentialContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+            assert(manifold.potentialContactPointsIndices.size() <= MAX_CONTACT_POINTS_IN_MANIFOLD);
+
+            // Remove the duplicated contact points in the manifold (if any)
+            removeDuplicatedContactPointsInManifold(manifold, potentialContactPoints);
         }
     }
 }
 
 // Return the largest depth of all the contact points of a potential manifold
 decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(const ContactManifoldInfo& manifold,
-                                                                        const Array<ContactPointInfo>& potentialContactPoints) const {
+                                                                              const Array<ContactPointInfo>& potentialContactPoints) const {
 
     decimal largestDepth = 0.0f;
 
-    assert(manifold.nbPotentialContactPoints > 0);
+    const uint32 nbContactPoints = static_cast<uint32>(manifold.potentialContactPointsIndices.size());
 
-    for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) {
+    assert(nbContactPoints > 0);
+
+    for (uint32 i=0; i < nbContactPoints; i++) {
         decimal depth = potentialContactPoints[manifold.potentialContactPointsIndices[i]].penetrationDepth;
 
         if (depth > largestDepth) {
@@ -1389,18 +1384,15 @@ decimal CollisionDetectionSystem::computePotentialManifoldLargestContactDepth(co
 void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold, const Transform& shape1ToWorldTransform,
                                              const Array<ContactPointInfo>& potentialContactPoints) const {
 
-    assert(manifold.nbPotentialContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
+    assert(manifold.potentialContactPointsIndices.size() > MAX_CONTACT_POINTS_IN_MANIFOLD);
 
     // The following algorithm only works to reduce to a maximum of 4 contact points
     assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
 
     // Array of the candidate contact points indices in the manifold. Every time that we have found a
     // point we want to keep, we will remove it from this array
-    uint candidatePointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD];
+    Array<uint> candidatePointsIndices(manifold.potentialContactPointsIndices);
-    uint8 nbCandidatePoints = manifold.nbPotentialContactPoints;
+    assert(manifold.potentialContactPointsIndices.size() == candidatePointsIndices.size());
-    for (uint8 i=0 ; i < manifold.nbPotentialContactPoints; i++) {
-        candidatePointsIndices[i] = manifold.potentialContactPointsIndices[i];
-    }
 
     int8 nbReducedPoints = 0;
 
@@ -1424,7 +1416,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     const Vector3 searchDirection(1, 1, 1);
     decimal maxDotProduct = DECIMAL_SMALLEST;
     uint elementIndexToKeep = 0;
-    for (uint i=0; i < nbCandidatePoints; i++) {
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
         decimal dotProduct = searchDirection.dot(element.localPoint1);
@@ -1435,7 +1427,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
         }
     }
     pointsToKeepIndices[0] = candidatePointsIndices[elementIndexToKeep];
-    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
+    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep);
     //candidatePointsIndices.removeAt(elementIndexToKeep);
     assert(nbReducedPoints == 1);
 
@@ -1444,7 +1436,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
 
     decimal maxDistance = decimal(0.0);
     elementIndexToKeep = 0;
-    for (uint i=0; i < nbCandidatePoints; i++) {
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
         const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@@ -1460,7 +1452,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
 
     }
     pointsToKeepIndices[1] = candidatePointsIndices[elementIndexToKeep];
-    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
+    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep);
     assert(nbReducedPoints == 2);
 
     // Compute the third contact point we need to keep.
@@ -1473,7 +1465,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     decimal minArea = decimal(0.0);
     decimal maxArea = decimal(0.0);
     bool isPreviousAreaPositive = true;
-    for (uint i=0; i < nbCandidatePoints; i++) {
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
         const ContactPointInfo& pointToKeep0 = potentialContactPoints[pointsToKeepIndices[0]];
@@ -1502,12 +1494,12 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     if (maxArea > (-minArea)) {
         isPreviousAreaPositive = true;
         pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMaxAreaIndex];
-        removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex, nbCandidatePoints);
+        removeItemAtInArray(candidatePointsIndices, thirdPointMaxAreaIndex);
     }
     else {
         isPreviousAreaPositive = false;
         pointsToKeepIndices[2] = candidatePointsIndices[thirdPointMinAreaIndex];
-        removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex, nbCandidatePoints);
+        removeItemAtInArray(candidatePointsIndices, thirdPointMinAreaIndex);
     }
     nbReducedPoints = 3;
 
@@ -1520,7 +1512,7 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
     decimal area;
 
     // For each remaining candidate points
-    for (uint i=0; i < nbCandidatePoints; i++) {
+    for (uint i=0; i < candidatePointsIndices.size(); i++) {
 
         const ContactPointInfo& element = potentialContactPoints[candidatePointsIndices[i]];
 
@@ -1557,22 +1549,51 @@ void CollisionDetectionSystem::reduceContactPoints(ContactManifoldInfo& manifold
         }
     }
     pointsToKeepIndices[3] = candidatePointsIndices[elementIndexToKeep];
-    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep, nbCandidatePoints);
+    removeItemAtInArray(candidatePointsIndices, elementIndexToKeep);
 
     // Only keep the four selected contact points in the manifold
-    manifold.potentialContactPointsIndices[0] = pointsToKeepIndices[0];
+    manifold.potentialContactPointsIndices.clear();
-    manifold.potentialContactPointsIndices[1] = pointsToKeepIndices[1];
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[0]);
-    manifold.potentialContactPointsIndices[2] = pointsToKeepIndices[2];
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[1]);
-    manifold.potentialContactPointsIndices[3] = pointsToKeepIndices[3];
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[2]);
-    manifold.nbPotentialContactPoints = 4;
+    manifold.potentialContactPointsIndices.add(pointsToKeepIndices[3]);
+}
+
+// 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.potentialContactPointsIndices.size(); i++) {
+        for (uint32 j=i+1; j < manifold.potentialContactPointsIndices.size(); 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.removeAtAndReplaceByLast(j);
+
+                j--;
+            }
+        }
+    }
 }
 
 // Remove an element in an array (and replace it by the last one in the array)
-void CollisionDetectionSystem::removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const {
+void CollisionDetectionSystem::removeItemAtInArray(Array<uint>& array, uint32 index) const {
-    assert(index < arraySize);
+    assert(index < array.size());
-    assert(arraySize > 0);
+    assert(array.size() > 0);
-    array[index] = array[arraySize - 1];
+    array.removeAtAndReplaceByLast(index);
-    arraySize--;
 }
 
 // Report contacts and triggers