Fix issue with overlapping pairs and lost contact report

include/reactphysics3d/body/RigidBody.h

@@ -57,7 +57,7 @@ class RigidBody : public Body {
         void awakeNeighborDisabledBodies();
 
         /// Remove the disabled overlapping pairs
-        void removeDisabledOverlappingPairs();
+        void enableOverlappingPairs();
 
         /// Disable the overlapping pairs if both bodies of the pair are disabled (sleeping or static)
         void checkForDisabledOverlappingPairs();

include/reactphysics3d/components/BodyComponents.h

@@ -159,7 +159,7 @@ RP3D_FORCE_INLINE void BodyComponents::removeColliderFromBody(Entity bodyEntity,
 }
 
 // Return a pointer to a body
-RP3D_FORCE_INLINE Body *BodyComponents::getBody(Entity bodyEntity) {
+RP3D_FORCE_INLINE Body* BodyComponents::getBody(Entity bodyEntity) {
 
     assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
 

include/reactphysics3d/engine/OverlappingPairs.h

@@ -140,12 +140,15 @@ class OverlappingPairs {
             /// True if the colliders of the overlapping pair are colliding in the current frame
             bool collidingInCurrentFrame;
 
+            /// True if at least one body of the pair is awake or not static
+            bool isEnabled;
+
             /// Constructor
             OverlappingPair(uint64 pairId, int32 broadPhaseId1, int32 broadPhaseId2, Entity collider1, Entity collider2,
-                            NarrowPhaseAlgorithmType narrowPhaseAlgorithmType)
+                            NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, bool isEnabled)
                : pairID(pairId), broadPhaseId1(broadPhaseId1), broadPhaseId2(broadPhaseId2), collider1(collider1) , collider2(collider2),
                  needToTestOverlap(false), narrowPhaseAlgorithmType(narrowPhaseAlgorithmType), collidingInPreviousFrame(false),
-                 collidingInCurrentFrame(false) {
+                 collidingInCurrentFrame(false), isEnabled(isEnabled) {
 
             }
 
@@ -164,8 +167,8 @@ class OverlappingPairs {
 
             /// Constructor
             ConvexOverlappingPair(uint64 pairId, int32 broadPhaseId1, int32 broadPhaseId2, Entity collider1, Entity collider2,
-                            NarrowPhaseAlgorithmType narrowPhaseAlgorithmType)
-              : OverlappingPair(pairId, broadPhaseId1, broadPhaseId2, collider1, collider2, narrowPhaseAlgorithmType) {
+                            NarrowPhaseAlgorithmType narrowPhaseAlgorithmType, bool isEnabled)
+              : OverlappingPair(pairId, broadPhaseId1, broadPhaseId2, collider1, collider2, narrowPhaseAlgorithmType, isEnabled) {
 
             }
         };
@@ -192,9 +195,10 @@ class OverlappingPairs {
                 /// Constructor
                 ConcaveOverlappingPair(uint64 pairId, int32 broadPhaseId1, int32 broadPhaseId2, Entity collider1, Entity collider2,
                                 NarrowPhaseAlgorithmType narrowPhaseAlgorithmType,
-                                bool isShape1Convex, MemoryAllocator& poolAllocator, MemoryAllocator& heapAllocator)
-                  : OverlappingPair(pairId, broadPhaseId1, broadPhaseId2, collider1, collider2, narrowPhaseAlgorithmType), mPoolAllocator(&poolAllocator),
-                    isShape1Convex(isShape1Convex), lastFrameCollisionInfos(heapAllocator, 16) {
+                                bool isShape1Convex, MemoryAllocator& poolAllocator, MemoryAllocator& heapAllocator, bool isEnabled,
+                                bool allocateLastFrameCollisionInfos = true)
+                  : OverlappingPair(pairId, broadPhaseId1, broadPhaseId2, collider1, collider2, narrowPhaseAlgorithmType, isEnabled), mPoolAllocator(&poolAllocator),
+                    isShape1Convex(isShape1Convex), lastFrameCollisionInfos(heapAllocator, allocateLastFrameCollisionInfos ? 16 : 0) {
 
                 }
 
@@ -296,17 +300,23 @@ class OverlappingPairs {
         /// Array of convex vs concave overlapping pairs
         Array<ConcaveOverlappingPair> mConcavePairs;
 
-        /// Array of disabled overlapping pairs (pairs with both bodies disabled)
-        Array<OverlappingPair> mDisabledPairs;
+        /// Array of disabled convex overlapping pairs (pairs with both bodies disabled)
+        Array<ConvexOverlappingPair> mDisabledConvexPairs;
+
+        /// Array of disabled concave overlapping pairs (pairs with both bodies disabled)
+        Array<ConcaveOverlappingPair> mDisabledConcavePairs;
 
         /// Map a convex pair id to the internal array index
         Map<uint64, uint64> mMapConvexPairIdToPairIndex;
 
         /// Map a concave pair id to the internal array index
         Map<uint64, uint64> mMapConcavePairIdToPairIndex;
 
-        /// Map a disable pair id to the internal array index
-        Map<uint64, uint64> mMapDisabledPairIdToPairIndex;
+        /// Map a disabled convex pair id to the internal array index
+        Map<uint64, uint64> mMapDisabledConvexPairIdToPairIndex;
+
+        /// Map a disable concave pair id to the internal array index
+        Map<uint64, uint64> mMapDisabledConcavePairIdToPairIndex;
 
         /// Reference to the colliders components
         ColliderComponents& mColliderComponents;
@@ -338,8 +348,11 @@ class OverlappingPairs {
         /// Swap two pairs in the array
         void swapPairs(uint64 index1, uint64 index2);
 
-        /// Remove a disabled overlapping pair
-        void removeDisabledPairWithIndex(uint64 pairIndex, bool removeFromColliders);
+        /// Remove a disabled convex overlapping pair
+        void removeDisabledConvexPairWithIndex(uint64 pairIndex, bool removeFromColliders);
+
+        /// Remove a disabled concave overlapping pair
+        void removeDisabledConcavePairWithIndex(uint64 pairIndex, bool removeFromColliders);
 
     public:
 
@@ -360,9 +373,24 @@ class OverlappingPairs {
         /// Deleted assignment operator
         OverlappingPairs& operator=(const OverlappingPairs& pair) = delete;
 
+        /// Enable an overlapping pair (because at least one body of the pair is awaken or not static anymore)
+        void enablePair(uint64 pairId);
+
         /// Disable an overlapping pair (because both bodies of the pair are disabled)
         void disablePair(uint64 pairId);
 
+        /// Enable a convex overlapping pair
+        void enableConvexPairWithIndex(uint64 pairIndex);
+
+        /// Disable a convex overlapping pair (because both bodies of the pair are disabled)
+        void disableConvexPairWithIndex(uint64 pairIndex);
+
+        /// Enable a concave overlapping pair
+        void enableConcavePairWithIndex(uint64 pairIndex);
+
+        /// Disable a concave overlapping pair (because both bodies of the pair are disabled)
+        void disableConcavePairWithIndex(uint64 pairIndex);
+
         /// Return true if a given pair is disabled (both bodies of the pair are disabled)
         bool isPairDisabled(uint64 pairId) const;
 
@@ -373,10 +401,10 @@ class OverlappingPairs {
         void removePair(uint64 pairId);
 
         /// Remove a convex pair at a given index
-        void removeConvexPairPairWithIndex(uint64 pairIndex, bool removeFromColliders = true);
+        void removeConvexPairWithIndex(uint64 pairIndex, bool removeFromColliders = true);
 
         // Remove a concave pair at a given index
-        void removeConcavePairPairWithIndex(uint64 pairIndex, bool removeFromColliders = true);
+        void removeConcavePairWithIndex(uint64 pairIndex, bool removeFromColliders = true);
 
         /// Delete all the obsolete last frame collision info
         void clearObsoleteLastFrameCollisionInfos();
@@ -442,19 +470,21 @@ RP3D_FORCE_INLINE OverlappingPairs::OverlappingPair* OverlappingPairs::getOverla
     if (it != mMapConcavePairIdToPairIndex.end()) {
         return &(mConcavePairs[static_cast<uint32>(it->second)]);
     }
-    else {
-        it = mMapDisabledPairIdToPairIndex.find(pairId);
-        if (it != mMapDisabledPairIdToPairIndex.end()) {
-            return &(mDisabledPairs[static_cast<uint32>(it->second)]);
-        }
+    it = mMapDisabledConvexPairIdToPairIndex.find(pairId);
+    if (it != mMapDisabledConvexPairIdToPairIndex.end()) {
+        return &(mDisabledConvexPairs[static_cast<uint32>(it->second)]);
+    }
+    it = mMapDisabledConcavePairIdToPairIndex.find(pairId);
+    if (it != mMapDisabledConcavePairIdToPairIndex.end()) {
+        return &(mDisabledConcavePairs[static_cast<uint32>(it->second)]);
     }
 
     return nullptr;
 }
 
 // Return true if a given pair is disabled (both bodies of the pair are disabled)
 RP3D_FORCE_INLINE bool OverlappingPairs::isPairDisabled(uint64 pairId) const {
-    return mMapDisabledPairIdToPairIndex.containsKey(pairId);
+    return mMapDisabledConvexPairIdToPairIndex.containsKey(pairId) || mMapDisabledConcavePairIdToPairIndex.containsKey(pairId);
 }
 
 #ifdef IS_RP3D_PROFILING_ENABLED

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -217,7 +217,7 @@ class CollisionDetectionSystem {
         void disableOverlappingPair(uint64 pairId);
 
         /// Remove an overlapping pair
-        void removeOverlappingPair(uint64 pairId);
+        void removeOverlappingPair(uint64 pairId, bool notifyLostContact);
 
         /// Remove a convex overlapping pair at a given index
         void removeConvexOverlappingPairWithIndex(uint64 pairIndex);

src/body/RigidBody.cpp

@@ -1040,8 +1040,8 @@ void RigidBody::setIsSleeping(bool isSleeping) {
     }
     else {
 
-        // Remove disabled overlapping pairs
-        removeDisabledOverlappingPairs();
+        // We need to enable the currently disabled overlapping pairs
+        enableOverlappingPairs();
 
         // Make sure the broad-phase with recompute the overlapping pairs with this body
         askForBroadPhaseCollisionCheck();
@@ -1052,8 +1052,8 @@ void RigidBody::setIsSleeping(bool isSleeping) {
          (isSleeping ? "true" : "false"),  __FILE__, __LINE__);
 }
 
-// Remove the disabled overlapping pairs
-void RigidBody::removeDisabledOverlappingPairs() {
+// Enable the currently disabled overlapping pairs
+void RigidBody::enableOverlappingPairs() {
 
     // For each collider of the body
     const Array<Entity>& colliderEntities = mWorld.mBodyComponents.getColliders(mEntity);
@@ -1062,12 +1062,16 @@ void RigidBody::removeDisabledOverlappingPairs() {
         // Get the currently overlapping pairs for this collider
         Array<uint64> overlappingPairs = mWorld.mCollidersComponents.getOverlappingPairs(colliderEntities[i]);
 
-        // We remove all the overlapping pairs (there should be only disabled overlapping pairs at this point)
+        // We enable all the overlapping pairs (there should be only disabled overlapping pairs at this point)
         const uint64 nbOverlappingPairs = overlappingPairs.size();
         for (uint64 j=0; j < nbOverlappingPairs; j++) {
 
-            mWorld.mCollisionDetection.removeOverlappingPair(overlappingPairs[j]);
-            std::cout << "Removing overlapping pair " << overlappingPairs[j] << std::endl;
+            OverlappingPairs::OverlappingPair* pair = mWorld.mCollisionDetection.mOverlappingPairs.getOverlappingPair(overlappingPairs[j]);
+
+            if (!pair->isEnabled) {
+
+                mWorld.mCollisionDetection.mOverlappingPairs.enablePair(overlappingPairs[j]);
+            }
         }
     }
 }
@@ -1103,8 +1107,6 @@ void RigidBody::awakeNeighborDisabledBodies() {
                     // Awake the neighbor colliding body
                     RigidBody* neighborBody = mWorld.mRigidBodyComponents.getRigidBody(isCurrentBody1 ? body2Entity : body1Entity);
                     neighborBody->setIsSleeping(false);
-
-                    std::cout << "Awake neighbor body " << neighborBody->getEntity().id << std::endl;
                 }
             }
         }
@@ -1136,7 +1138,6 @@ void RigidBody::checkForDisabledOverlappingPairs() {
             if (isBody1Disabled && isBody2Disabled) {
 
                 mWorld.mCollisionDetection.disableOverlappingPair(overlappingPairs[j]);
-                std::cout << "Disabling overlapping pair " << overlappingPairs[j] << std::endl;
             }
         }
     }