Remove contact manifolds that do not have contact points anymore after manifold update

src/collision/CollisionDetection.cpp

@@ -468,6 +468,8 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
 
         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->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));

src/collision/ContactManifold.cpp

@@ -59,10 +59,10 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
             // Delete the new contact
             contact->~ContactPoint();
             mMemoryAllocator.release(contact, sizeof(ContactPoint));
-            //removeContact(i);
+
+            assert(mNbContactPoints > 0);
 
             return;
-            //break;
 		}
 	}
     
@@ -76,6 +76,8 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
     // Add the new contact point in the manifold
     mContactPoints[mNbContactPoints] = contact;
     mNbContactPoints++;
+
+    assert(mNbContactPoints > 0);
 }
 
 // Remove a contact point from the manifold
@@ -94,6 +96,8 @@ void ContactManifold::removeContactPoint(uint index) {
     }
 
     mNbContactPoints--;
+
+    assert(mNbContactPoints >= 0);
 }
 
 // Update the contact manifold

src/collision/ContactManifoldSet.cpp

@@ -54,6 +54,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
 
         createManifold(normalDirectionId);
         mManifolds[0]->addContactPoint(contact);
+        assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
+        for (int i=0; i<mNbManifolds; i++) {
+            assert(mManifolds[i]->getNbContactPoints() > 0);
+        }
 
         return;
     }
@@ -69,6 +73,7 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
 
         // Add the contact point to that similar manifold
         mManifolds[similarManifoldIndex]->addContactPoint(contact);
+        assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
 
         return;
     }
@@ -79,6 +84,9 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
         // Create a new manifold for the contact point
         createManifold(normalDirectionId);
         mManifolds[mNbManifolds-1]->addContactPoint(contact);
+        for (int i=0; i<mNbManifolds; i++) {
+            assert(mManifolds[i]->getNbContactPoints() > 0);
+        }
 
         return;
     }
@@ -115,6 +123,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
     removeManifold(smallestDepthIndex);
     createManifold(normalDirectionId);
     mManifolds[mNbManifolds-1]->addContactPoint(contact);
+    assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
+    for (int i=0; i<mNbManifolds; i++) {
+        assert(mManifolds[i]->getNbContactPoints() > 0);
+    }
 
     return;
 }
@@ -173,9 +185,16 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
 // Update the contact manifolds
 void ContactManifoldSet::update() {
 
-    for (int i=0; i<mNbManifolds; i++) {
+    for (int i=mNbManifolds-1; i>=0; i--) {
+
+        // Update the contact manifold
         mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
                               mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
+
+        // Remove the contact manifold if has no contact points anymore
+        if (mManifolds[i]->getNbContactPoints() == 0) {
+            removeManifold(i);
+        }
     }
 }
 
@@ -202,6 +221,7 @@ void ContactManifoldSet::createManifold(short int normalDirectionId) {
 // Remove a contact manifold from the set
 void ContactManifoldSet::removeManifold(int index) {
 
+    assert(mNbManifolds > 0);
     assert(index >= 0 && index < mNbManifolds);
 
     // Delete the new contact

src/collision/ContactManifoldSet.h

@@ -135,7 +135,7 @@ inline int ContactManifoldSet::getNbContactManifolds() const {
 
 // Return a given contact manifold
 inline ContactManifold* ContactManifoldSet::getContactManifold(uint index) const {
-    assert(index < mNbManifolds);
+    assert(index >= 0 && index < mNbManifolds);
     return mManifolds[index];
 }
 

src/engine/DynamicsWorld.cpp

@@ -719,6 +719,8 @@ void DynamicsWorld::computeIslands() {
 
                 ContactManifold* contactManifold = contactElement->contactManifold;
 
+                assert(contactManifold->getNbContactPoints() > 0);
+
                 // Check if the current contact manifold has already been added into an island
                 if (contactManifold->isAlreadyInIsland()) continue;