Modify the policy to drop contact manifolds. First drop the old ones before the new ones

src/collision/CollisionDetection.cpp

@@ -71,7 +71,7 @@ void CollisionDetection::computeCollisionDetection() {
     computeMiddlePhase();
     
     // Compute the narrow-phase collision detection
-     computeNarrowPhase();
+    computeNarrowPhase();
 
     // Reset the linked list of narrow-phase info
     mNarrowPhaseInfoList = nullptr;
@@ -410,6 +410,9 @@ void CollisionDetection::processPotentialContacts(OverlappingPair* pair) {
 		potentialManifold = potentialManifold->mNext;
 	}
 
+    // Reset the isNew status of the manifolds
+    pair->resetIsNewManifoldStatus();
+
     // Clear the obsolete contact manifolds and contact points
     pair->clearObsoleteManifoldsAndContactPoints();
 

src/collision/ContactManifold.cpp

@@ -34,7 +34,7 @@ ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyS
                 : mShape1(shape1), mShape2(shape2), mContactPoints(nullptr), mContactNormalId(manifoldInfo->getContactNormalId()),
                   mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
                   mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
-                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
+                  mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false), mIsNew(true) {
     
     // For each contact point info in the manifold
     const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
@@ -85,6 +85,7 @@ void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo)
     mContactPoints = contactPoint;
 
     mNbContactPoints++;
+    assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
 }
 
 // Clear the obsolete contact points

src/collision/ContactManifold.h

@@ -143,6 +143,9 @@ class ContactManifold {
         /// True if the contact manifold is obsolete
         bool mIsObsolete;
 
+        /// True if the contact manifold is new (has just been added from potential contacts)
+        bool mIsNew;
+
         // -------------------- Methods -------------------- //
 
         /// Return true if the contact manifold has already been added into an island
@@ -204,6 +207,12 @@ class ContactManifold {
 
         /// Return the friction twist accumulated impulse
         decimal getFrictionTwistImpulse() const;
+
+        /// Return true if the manifold has just been created
+        bool getIsNew() const;
+
+        /// Set the isNew attribute
+        void setIsNew(bool isNew);
         
     public:
 
@@ -408,6 +417,18 @@ inline short ContactManifold::getContactNormalId() const {
     return mContactNormalId;
 }
 
+
+// Return true if the manifold has just been created
+inline bool ContactManifold::getIsNew() const {
+    return mIsNew;
+}
+
+// Set the isNew attribute
+inline void ContactManifold::setIsNew(bool isNew) {
+    mIsNew = isNew;
+}
+
+
 }
 #endif
 

src/collision/ContactManifoldSet.cpp

@@ -66,8 +66,9 @@ void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactMa
         if (mNbManifolds >= mNbMaxManifolds) {
 
             // We need to remove a manifold from the set.
-            // We seach for the one with the smallest maximum penetration depth among its contact points
-            ContactManifold* minDepthManifold = getManifoldWithSmallestContactPenetrationDepth(contactManifoldInfo->getLargestPenetrationDepth());
+            // We search for an old manifold with the smallest maximum penetration depth among its contact points.
+            // If we do not find an old manifold, we select a new one that has the smallest contact penetration depth.
+            ContactManifold* minDepthManifold = selectManifoldToRemove(contactManifoldInfo->getLargestPenetrationDepth());
 
             // If the manifold with the minimum penetration depth is an existing one
             if (minDepthManifold != nullptr) {
@@ -135,25 +136,53 @@ void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold,
    oldManifold->setIsObsolete(false, false);
 }
 
-// Return the manifold with the smallest contact penetration depth among its points
-ContactManifold* ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
+// Return the manifold to remove (because it is too old or has not the largest penetration depth)
+ContactManifold* ContactManifoldSet::selectManifoldToRemove(decimal penDepthNewManifold) const {
 
     assert(mNbManifolds == mNbMaxManifolds);
+    assert(mManifolds != nullptr);
 
-    ContactManifold* minDepthManifold = nullptr;
-    decimal minDepth = initDepth;
+    // Look for a manifold that is not new and with the smallest contact penetration depth.
+    // At the same time, we also look for a new manifold with the smallest contact penetration depth
+    // in case no old manifold exists.
+    ContactManifold* minDepthOldManifold = nullptr;
+    ContactManifold* minDepthNewManifold = nullptr;
+    decimal minDepthOld = DECIMAL_LARGEST;
+    decimal minDepthNew = penDepthNewManifold;
     ContactManifold* manifold = mManifolds;
     while (manifold != nullptr) {
-        decimal depth = manifold->getLargestContactDepth();
-        if (depth < minDepth) {
-            minDepth = depth;
-            minDepthManifold = manifold;
+
+        // Get the largest contact point penetration depth of the manifold
+        const decimal depth = manifold->getLargestContactDepth();
+
+        // If it is a new manifold
+        if (manifold->getIsNew()) {
+
+            if (depth < minDepthNew) {
+                minDepthNew = depth;
+                minDepthNewManifold = manifold;
+            }
+        }
+        else {
+
+            if (depth < minDepthOld) {
+                minDepthOld = depth;
+                minDepthOldManifold = manifold;
+            }
         }
 
         manifold = manifold->getNext();
     }
 
-    return minDepthManifold;
+    // If there was a contact manifold that was not new
+    if (minDepthOldManifold != nullptr) {
+
+        // Return the old manifold with the smallest penetration depth
+        return minDepthOldManifold;
+    }
+
+    // Otherwise, we return the new manifold with the smallest penetration depth
+    return minDepthNewManifold;
 }
 
 // Return the contact manifold with a similar average normal.

src/collision/ContactManifoldSet.h

@@ -74,8 +74,8 @@ class ContactManifoldSet {
         // Return the contact manifold with a similar average normal.
         ContactManifold* selectManifoldWithSimilarNormal(short int normalDirectionId) const;
 
-        /// Return the manifold with the smallest contact penetration depth
-        ContactManifold* getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
+        /// Return the manifold to remove (because it is too old or has not the largest penetration depth)
+        ContactManifold* selectManifoldToRemove(decimal penDepthNewManifold) const;
 
         /// Update a previous similar manifold with a new one
         void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
@@ -124,6 +124,9 @@ class ContactManifoldSet {
         /// Clear the obsolete contact manifolds and contact points
         void clearObsoleteManifoldsAndContactPoints();
 
+        /// Reset the isNew status of all the manifolds
+        void resetIsNewManifoldStatus();
+
         // 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
@@ -177,6 +180,17 @@ inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape
     }
 }
 
+
+// Reset the isNew status of all the manifolds
+inline void ContactManifoldSet::resetIsNewManifoldStatus() {
+
+    ContactManifold* manifold = mManifolds;
+    while (manifold != nullptr) {
+        manifold->setIsNew(false);
+        manifold = manifold->getNext();
+    }
+}
+
 }
 
 #endif

src/engine/OverlappingPair.h

@@ -172,6 +172,9 @@ class OverlappingPair {
         /// Clear the obsolete contact manifold and contact points
         void clearObsoleteManifoldsAndContactPoints();
 
+        /// Reset the isNew status of all the manifolds
+        void resetIsNewManifoldStatus();
+
         /// Return the pair of bodies index
         static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2);
 
@@ -269,6 +272,12 @@ inline void OverlappingPair::clearObsoleteManifoldsAndContactPoints() {
     mContactManifoldSet.clearObsoleteManifoldsAndContactPoints();
 }
 
+
+// Reset the isNew status of all the manifolds
+inline void OverlappingPair::resetIsNewManifoldStatus() {
+   mContactManifoldSet.resetIsNewManifoldStatus();
+}
+
 }
 
 #endif