From baf4d3f49248a988a41e5130900569f8526edc05 Mon Sep 17 00:00:00 2001
From: jingqi <jingqidev@163.com>
Date: Fri, 3 Dec 2021 11:57:23 +0800
Subject: [PATCH] Remove involved overlapping pairs when addNoCollisionPair()
 is called

---
 .../systems/CollisionDetectionSystem.h        |  5 ----
 src/systems/CollisionDetectionSystem.cpp      | 28 +++++++++++++++++++
 2 files changed, 28 insertions(+), 5 deletions(-)

diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h
index c8c4983c..d2bbe320 100644
--- a/include/reactphysics3d/systems/CollisionDetectionSystem.h
+++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h
@@ -413,11 +413,6 @@ RP3D_FORCE_INLINE void CollisionDetectionSystem::addCollider(Collider* collider,
     mMapBroadPhaseIdToColliderEntity.add(Pair<int, Entity>(broadPhaseId, collider->getEntity()));
 }
 
-// Add a pair of bodies that cannot collide with each other
-RP3D_FORCE_INLINE void CollisionDetectionSystem::addNoCollisionPair(Entity body1Entity, Entity body2Entity) {
-    mNoCollisionPairs.add(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity));
-}
-
 // Remove a pair of bodies that cannot collide with each other
 RP3D_FORCE_INLINE void CollisionDetectionSystem::removeNoCollisionPair(Entity body1Entity, Entity body2Entity) {
     mNoCollisionPairs.remove(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity));
diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp
index a169be62..0de78a0d 100644
--- a/src/systems/CollisionDetectionSystem.cpp
+++ b/src/systems/CollisionDetectionSystem.cpp
@@ -196,6 +196,34 @@ void CollisionDetectionSystem::addLostContactPair(OverlappingPairs::OverlappingP
     mLostContactPairs.add(lostContactPair);
 }
 
+// Add a pair of bodies that cannot collide with each other
+void CollisionDetectionSystem::addNoCollisionPair(Entity body1Entity, Entity body2Entity) {
+    mNoCollisionPairs.add(OverlappingPairs::computeBodiesIndexPair(body1Entity, body2Entity));
+
+    // If there already are OverlappingPairs involved, they should be removed; Or they will remain in collision state
+    Array<uint64> toBeRemove(mMemoryManager.getPoolAllocator());
+    const Array<Entity>& colliderEntities = mWorld->mCollisionBodyComponents.getColliders(body1Entity);
+    for (uint32 i = 0; i < colliderEntities.size(); ++i) {
+
+        // Get the currently overlapping pairs for colliders of body1
+        const Array<uint64>& overlappingPairs = mCollidersComponents.getOverlappingPairs(colliderEntities[i]);
+
+        for (uint32 j = 0; j < overlappingPairs.size(); ++j) {
+
+            OverlappingPairs::OverlappingPair *pair = mOverlappingPairs.getOverlappingPair(overlappingPairs[j]);
+            assert(pair != nullptr);
+
+            const Entity overlappingBody1 = mOverlappingPairs.mColliderComponents.getBody(pair->collider1);
+            const Entity overlappingBody2 = mOverlappingPairs.mColliderComponents.getBody(pair->collider2);
+            if (overlappingBody1 == body2Entity || overlappingBody2 == body2Entity)
+                toBeRemove.add(overlappingPairs[j]);
+        }
+    }
+
+    for (uint32 i = 0; i < toBeRemove.size(); ++i)
+        mOverlappingPairs.removePair(toBeRemove[i]);
+}
+
 // Take an array of overlapping nodes in the broad-phase and create new overlapping pairs if necessary
 void CollisionDetectionSystem::updateOverlappingPairs(const Array<Pair<int32, int32>>& overlappingNodes) {