Fix issues in PhysicsWorld::createIslands() method and add more optimization in CollisionDetectionSystem::computeNarrowPhase() method

include/reactphysics3d/components/Components.h

@@ -113,6 +113,9 @@ class Components {
         /// Return true if there is a component for a given entity
         bool hasComponent(Entity entity) const;
 
+        /// Return true if there is a component for a given entiy and if so set the entity index
+        bool hasComponentGetIndex(Entity entity, uint32& entityIndex) const;
+
         /// Return the number of components
         uint32 getNbComponents() const;
 
@@ -134,6 +137,19 @@ inline bool Components::hasComponent(Entity entity) const {
     return mMapEntityToComponentIndex.containsKey(entity);
 }
 
+// Return true if there is a component for a given entiy and if so set the entity index
+inline bool Components::hasComponentGetIndex(Entity entity, uint32& entityIndex) const {
+
+    auto it = mMapEntityToComponentIndex.find(entity);
+
+    if (it != mMapEntityToComponentIndex.end()) {
+        entityIndex = it->second;
+        return true;
+    }
+
+    return false;
+}
+
 // Return the number of components
 inline uint32 Components::getNbComponents() const {
     return mNbComponents;

include/reactphysics3d/components/RigidBodyComponents.h

@@ -145,6 +145,9 @@ class RigidBodyComponents : public Components {
         /// For each body, the list of joints entities the body is part of
         List<Entity>* mJoints;
 
+        /// For each body, the list of the indices of contact pairs in which the body is involved
+        List<uint>* mContactPairs;
+
         // -------------------- Methods -------------------- //
 
         /// Allocate memory for a given number of components
@@ -342,6 +345,9 @@ class RigidBodyComponents : public Components {
         /// Remove a joint from a body component
         void removeJointFromBody(Entity bodyEntity, Entity jointEntity);
 
+        /// A an associated contact pairs into the contact pairs array of the body
+        void addContacPair(Entity bodyEntity, uint contactPairIndex);
+
         // -------------------- Friendship -------------------- //
 
         friend class PhysicsWorld;
@@ -769,6 +775,13 @@ inline void RigidBodyComponents::removeJointFromBody(Entity bodyEntity, Entity j
     mJoints[mMapEntityToComponentIndex[bodyEntity]].remove(jointEntity);
 }
 
+// A an associated contact pairs into the contact pairs array of the body
+inline void RigidBodyComponents::addContacPair(Entity bodyEntity, uint contactPairIndex) {
+
+    assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+    mContactPairs[mMapEntityToComponentIndex[bodyEntity]].add(contactPairIndex);
+}
+
 }
 
 #endif

include/reactphysics3d/systems/CollisionDetectionSystem.h

@@ -83,6 +83,9 @@ class CollisionDetectionSystem {
         /// Reference the collider components
         ColliderComponents& mCollidersComponents;
 
+        /// Reference to the rigid bodies components
+        RigidBodyComponents& mRigidBodyComponents;
+
         /// Collision Detection Dispatch configuration
         CollisionDispatch mCollisionDispatch;
 
@@ -163,9 +166,6 @@ class CollisionDetectionSystem {
         /// Pointer to the contact points of the current frame (either mContactPoints1 or mContactPoints2)
         List<ContactPoint>* mCurrentContactPoints;
 
-        /// Map a body entity to the list of contact pairs in which it is involved
-        Map<Entity, List<uint>> mMapBodyToContactPairs;
-
 #ifdef IS_RP3D_PROFILING_ENABLED
 
     /// Pointer to the profiler
@@ -224,14 +224,12 @@ class CollisionDetectionSystem {
         void processPotentialContacts(NarrowPhaseInfoBatch& narrowPhaseInfoBatch,
                                       bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
                                       Map<uint64, uint>* mapPairIdToContactPairIndex,
-                                      List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs,
+                                      List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs);
-                                      Map<Entity, List<uint>>& mapBodyToContactPairs);
 
         /// Process the potential contacts after narrow-phase collision detection
         void processAllPotentialContacts(NarrowPhaseInput& narrowPhaseInput, bool updateLastFrameInfo, List<ContactPointInfo>& potentialContactPoints,
                                          Map<uint64, uint>* mapPairIdToContactPairIndex,
-                                         List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs,
+                                         List<ContactManifoldInfo>& potentialContactManifolds, List<ContactPair>* contactPairs);
-                                         Map<Entity, List<uint>>& mapBodyToContactPairs);
 
         /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
         void reducePotentialContactManifolds(List<ContactPair>* contactPairs, List<ContactManifoldInfo>& potentialContactManifolds,

src/components/RigidBodyComponents.cpp

@@ -43,7 +43,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
                                 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
-                                sizeof(bool) + sizeof(bool) + sizeof(List<Entity>)) {
+                                sizeof(bool) + sizeof(bool) + sizeof(List<Entity>) + sizeof(List<uint>)) {
 
     // Allocate memory for the components data
     allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -89,6 +89,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
     bool* newIsGravityEnabled = reinterpret_cast<bool*>(newCentersOfMassWorld + nbComponentsToAllocate);
     bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
     List<Entity>* newJoints = reinterpret_cast<List<Entity>*>(newIsAlreadyInIsland + nbComponentsToAllocate);
+    List<uint>* newContactPairs = reinterpret_cast<List<uint>*>(newJoints + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -121,6 +122,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
         memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
         memcpy(newJoints, mJoints, mNbComponents * sizeof(List<Entity>));
+        memcpy(newContactPairs, mContactPairs, mNbComponents * sizeof(List<uint>));
 
         // Deallocate previous memory
         mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@@ -155,6 +157,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
     mIsGravityEnabled = newIsGravityEnabled;
     mIsAlreadyInIsland = newIsAlreadyInIsland;
     mJoints = newJoints;
+    mContactPairs = newContactPairs;
 }
 
 // Add a component
@@ -191,6 +194,7 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const
     mIsGravityEnabled[index] = true;
     mIsAlreadyInIsland[index] = false;
     new (mJoints + index) List<Entity>(mMemoryAllocator);
+    new (mContactPairs + index) List<uint>(mMemoryAllocator);
 
     // Map the entity with the new component lookup index
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
@@ -235,6 +239,7 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
     mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
     mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
     new (mJoints + destIndex) List<Entity>(mJoints[srcIndex]);
+    new (mContactPairs + destIndex) List<uint>(mContactPairs[srcIndex]);
 
     // Destroy the source component
     destroyComponent(srcIndex);
@@ -278,6 +283,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
     bool isGravityEnabled1 = mIsGravityEnabled[index1];
     bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
     List<Entity> joints1 = mJoints[index1];
+    List<uint> contactPairs1 = mContactPairs[index1];
 
     // Destroy component 1
     destroyComponent(index1);
@@ -312,6 +318,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
     mIsGravityEnabled[index2] = isGravityEnabled1;
     mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
     new (mJoints + index2) List<Entity>(joints1);
+    new (mContactPairs + index2) List<uint>(contactPairs1);
 
     // Update the entity to component index mapping
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
@@ -347,4 +354,5 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
     mCentersOfMassLocal[index].~Vector3();
     mCentersOfMassWorld[index].~Vector3();
     mJoints[index].~List<Entity>();
+    mContactPairs[index].~List<uint>();
 }

src/engine/PhysicsWorld.cpp

@@ -233,9 +233,9 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) {
     mCollisionBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled);
     mTransformComponents.setIsEntityDisabled(bodyEntity, isDisabled);
 
-    if (mRigidBodyComponents.hasComponent(bodyEntity)) {
+    assert(mRigidBodyComponents.hasComponent(bodyEntity));
+
     mRigidBodyComponents.setIsEntityDisabled(bodyEntity, isDisabled);
-    }
 
     // For each collider of the body
     const List<Entity>& collidersEntities = mCollisionBodyComponents.getColliders(bodyEntity);
@@ -245,8 +245,6 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) {
     }
 
     // Disable the joints of the body if necessary
-    if (mRigidBodyComponents.hasComponent(bodyEntity)) {
-
     // For each joint of the body
     const List<Entity>& joints = mRigidBodyComponents.getJoints(bodyEntity);
     for(uint32 i=0; i < joints.size(); i++) {
@@ -255,8 +253,7 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) {
         const Entity body2Entity = mJointsComponents.getBody2Entity(joints[i]);
 
         // If both bodies of the joint are disabled
-            if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) &&
+        if (mRigidBodyComponents.getIsEntityDisabled(body1Entity) && mRigidBodyComponents.getIsEntityDisabled(body2Entity)) {
-                mRigidBodyComponents.getIsEntityDisabled(body2Entity)) {
 
             // We disable the joint
             setJointDisabled(joints[i], true);
@@ -267,7 +264,6 @@ void PhysicsWorld::setBodyDisabled(Entity bodyEntity, bool isDisabled) {
             setJointDisabled(joints[i], false);
         }
     }
-    }
 }
 
 // Notify the world whether a joint is disabled or not
@@ -760,10 +756,10 @@ void PhysicsWorld::createIslands() {
     mIslands.reserveMemory();
 
     // Create a stack for the bodies to visit during the Depth First Search
-    Stack<uint32> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator(), mIslands.getNbMaxBodiesInIslandPreviousFrame());
+    Stack<Entity> bodyEntitiesToVisit(mMemoryManager.getSingleFrameAllocator(), mIslands.getNbMaxBodiesInIslandPreviousFrame());
 
     // List of static bodies added to the current island (used to reset the isAlreadyInIsland variable of static bodies)
-    List<uint32> staticBodiesAddedToIsland(mMemoryManager.getSingleFrameAllocator());
+    List<Entity> staticBodiesAddedToIsland(mMemoryManager.getSingleFrameAllocator());
 
     uint nbTotalManifolds = 0;
 
@@ -777,59 +773,55 @@ void PhysicsWorld::createIslands() {
         if (mRigidBodyComponents.mBodyTypes[b] == BodyType::STATIC) continue;
 
         // Reset the stack of bodies to visit
-        bodyEntityIndicesToVisit.clear();
+        bodyEntitiesToVisit.clear();
 
         // Add the body into the stack of bodies to visit
         mRigidBodyComponents.mIsAlreadyInIsland[b] = true;
-        bodyEntityIndicesToVisit.push(b);
+        bodyEntitiesToVisit.push(mRigidBodyComponents.mBodiesEntities[b]);
 
         // Create the new island
         uint32 islandIndex = mIslands.addIsland(nbTotalManifolds);
 
         // While there are still some bodies to visit in the stack
-        while (bodyEntityIndicesToVisit.size() > 0) {
+        while (bodyEntitiesToVisit.size() > 0) {
-
-            // Get the next body to visit from the stack
-            const uint32 bodyToVisitIndex = bodyEntityIndicesToVisit.pop();
 
             // Get the body entity
-            const Entity bodyToVisitEntity = mRigidBodyComponents.mBodiesEntities[bodyToVisitIndex];
+            const Entity bodyToVisitEntity = bodyEntitiesToVisit.pop();
 
             // Add the body into the island
             mIslands.addBodyToIsland(bodyToVisitEntity);
 
-            RigidBody* rigidBodyToVisit = mRigidBodyComponents.mRigidBodies[bodyToVisitIndex];
+            RigidBody* rigidBodyToVisit = mRigidBodyComponents.getRigidBody(bodyToVisitEntity);
 
             // Awake the body if it is sleeping (note that this called might change the body index in the mRigidBodyComponents array)
             rigidBodyToVisit->setIsSleeping(false);
 
-            // If the current body is static, we do not want to perform the DFS search across that body
+            // Compute the body index in the array (Note that it could have changed because of the previous call to rigidBodyToVisit->setIsSleeping(false))
-            if (rigidBodyToVisit->getType() == BodyType::STATIC) {
+            const uint32 bodyToVisitIndex = mRigidBodyComponents.getEntityIndex(bodyToVisitEntity);
 
-                // Get the new body index in the mRigidBodyComponents (this index might have changed due to the call to rigidBodyToVisite->setIsSleeping(false))
+            // If the currenbodyEntityIndicesToVisitt body is static, we do not want to perform the DFS search across that body
-                const uint32 newBodyIndex = mRigidBodyComponents.getEntityIndex(bodyToVisitEntity);
+            if (mRigidBodyComponents.mBodyTypes[bodyToVisitIndex] == BodyType::STATIC) {
 
-                staticBodiesAddedToIsland.add(newBodyIndex);
+                staticBodiesAddedToIsland.add(bodyToVisitEntity);
 
                 // Go to the next body
                 continue;
             }
 
             // If the body is involved in contacts with other bodies
-            auto itBodyContactPairs = mCollisionDetection.mMapBodyToContactPairs.find(bodyToVisitEntity);
-            if (itBodyContactPairs != mCollisionDetection.mMapBodyToContactPairs.end()) {
-
             // For each contact pair in which the current body is involded
-                List<uint>& contactPairs = itBodyContactPairs->second;
+            for (uint p=0; p < mRigidBodyComponents.mContactPairs[bodyToVisitIndex].size(); p++) {
-                for (uint p=0; p < contactPairs.size(); p++) {
 
-                    ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]];
+                ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[mRigidBodyComponents.mContactPairs[bodyToVisitIndex][p]];
 
                 // Check if the current contact pair has already been added into an island
                 if (pair.isAlreadyInIsland) continue;
 
+                const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
+
                 // If the colliding body is a RigidBody (and not a CollisionBody) and is not a trigger
-                    if (mRigidBodyComponents.hasComponent(pair.body1Entity) && mRigidBodyComponents.hasComponent(pair.body2Entity)
+                uint32 otherBodyIndex;
+                if (mRigidBodyComponents.hasComponentGetIndex(otherBodyEntity, otherBodyIndex)
                     && !mCollidersComponents.getIsTrigger(pair.collider1Entity) && !mCollidersComponents.getIsTrigger(pair.collider2Entity)) {
 
                     assert(pair.potentialContactManifoldsIndices.size() > 0);
@@ -839,14 +831,11 @@ void PhysicsWorld::createIslands() {
                     mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
                     pair.isAlreadyInIsland = true;
 
-                        const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
-                        const uint32 otherBodyIndex = mRigidBodyComponents.getEntityIndex(otherBodyEntity);
-
                     // Check if the other body has already been added to the island
                     if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue;
 
                     // Insert the other body into the stack of bodies to visit
-                        bodyEntityIndicesToVisit.push(otherBodyIndex);
+                    bodyEntitiesToVisit.push(otherBodyEntity);
                     mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true;
                 }
                 else {
@@ -855,7 +844,6 @@ void PhysicsWorld::createIslands() {
                     pair.isAlreadyInIsland = true;
                 }
             }
-            }
 
             // For each joint in which the current body is involved
             const List<Entity>& joints = mRigidBodyComponents.getJoints(rigidBodyToVisit->getEntity());
@@ -877,7 +865,7 @@ void PhysicsWorld::createIslands() {
                 if (mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex]) continue;
 
                 // Insert the other body into the stack of bodies to visit
-                bodyEntityIndicesToVisit.push(otherBodyIndex);
+                bodyEntitiesToVisit.push(otherBodyEntity);
                 mRigidBodyComponents.mIsAlreadyInIsland[otherBodyIndex] = true;
             }
         }
@@ -886,14 +874,17 @@ void PhysicsWorld::createIslands() {
         // can also be included in the other islands
         for (uint j=0; j < staticBodiesAddedToIsland.size(); j++) {
 
-            assert(mRigidBodyComponents.mBodyTypes[staticBodiesAddedToIsland[j]] == BodyType::STATIC);
+            assert(mRigidBodyComponents.getBodyType(staticBodiesAddedToIsland[j]) == BodyType::STATIC);
-            mRigidBodyComponents.mIsAlreadyInIsland[staticBodiesAddedToIsland[j]] = false;
+            mRigidBodyComponents.setIsAlreadyInIsland(staticBodiesAddedToIsland[j], false);
         }
 
         staticBodiesAddedToIsland.clear();
     }
 
-    mCollisionDetection.mMapBodyToContactPairs.clear(true);
+    // Clear the associated contacts pairs of rigid bodies
+    for (uint b=0; b < mRigidBodyComponents.getNbEnabledComponents(); b++) {
+        mRigidBodyComponents.mContactPairs[b].clear();
+    }
 }
 
 // Put bodies to sleep if needed.

src/systems/CollisionDetectionSystem.cpp

@@ -50,8 +50,9 @@ using namespace std;
 
 // Constructor
 CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, ColliderComponents& collidersComponents,  TransformComponents& transformComponents,
-                                       CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents, MemoryManager& memoryManager)
+                                                   CollisionBodyComponents& collisionBodyComponents, RigidBodyComponents& rigidBodyComponents,
-                   : mMemoryManager(memoryManager), mCollidersComponents(collidersComponents),
+                                                   MemoryManager& memoryManager)
+                   : mMemoryManager(memoryManager), mCollidersComponents(collidersComponents), mRigidBodyComponents(rigidBodyComponents),
                      mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
                      mNoCollisionPairs(mMemoryManager.getPoolAllocator()),
                      mOverlappingPairs(mMemoryManager.getPoolAllocator(), mMemoryManager.getSingleFrameAllocator(), mCollidersComponents,
@@ -67,7 +68,7 @@ CollisionDetectionSystem::CollisionDetectionSystem(PhysicsWorld* world, Collider
                      mContactManifolds2(mMemoryManager.getPoolAllocator()), mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
                      mContactPoints1(mMemoryManager.getPoolAllocator()),
                      mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
-                     mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) {
+                     mCurrentContactPoints(&mContactPoints2) {
 
 #ifdef IS_RP3D_PROFILING_ENABLED
 
@@ -481,8 +482,7 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar
                                                      List<ContactPointInfo>& potentialContactPoints,
                                                      Map<uint64, uint>* mapPairIdToContactPairIndex,
                                                      List<ContactManifoldInfo>& potentialContactManifolds,
-                                                     List<ContactPair>* contactPairs,
+                                                     List<ContactPair>* contactPairs) {
-                                                     Map<Entity, List<uint>>& mapBodyToContactPairs) {
 
     assert(contactPairs->size() == 0);
     assert(mapPairIdToContactPairIndex->size() == 0);
@@ -497,17 +497,17 @@ void CollisionDetectionSystem::processAllPotentialContacts(NarrowPhaseInput& nar
 
     // Process the potential contacts
     processPotentialContacts(sphereVsSphereBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+                             potentialContactManifolds, contactPairs);
     processPotentialContacts(sphereVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+                             potentialContactManifolds, contactPairs);
     processPotentialContacts(capsuleVsCapsuleBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+                             potentialContactManifolds, contactPairs);
     processPotentialContacts(sphereVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                            potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+                            potentialContactManifolds, contactPairs);
     processPotentialContacts(capsuleVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+                             potentialContactManifolds, contactPairs);
     processPotentialContacts(convexPolyhedronVsConvexPolyhedronBatch, updateLastFrameInfo, potentialContactPoints, mapPairIdToContactPairIndex,
-                             potentialContactManifolds, contactPairs, mapBodyToContactPairs);
+                             potentialContactManifolds, contactPairs);
 }
 
 // Compute the narrow-phase collision detection
@@ -528,7 +528,7 @@ void CollisionDetectionSystem::computeNarrowPhase() {
 
     // Process all the potential contacts after narrow-phase collision
     processAllPotentialContacts(mNarrowPhaseInput, true, mPotentialContactPoints, mCurrentMapPairIdToContactPairIndex,
-                                mPotentialContactManifolds, mCurrentContactPairs, mMapBodyToContactPairs);
+                                mPotentialContactManifolds, mCurrentContactPairs);
 
     // Reduce the number of contact points in the manifolds
     reducePotentialContactManifolds(mCurrentContactPairs, mPotentialContactManifolds, mPotentialContactPoints);
@@ -662,18 +662,15 @@ bool CollisionDetectionSystem::computeNarrowPhaseCollisionSnapshot(NarrowPhaseIn
         List<ContactPair> lostContactPairs(allocator);                  // Not used during collision snapshots
         List<ContactManifold> contactManifolds(allocator);
         List<ContactPoint> contactPoints(allocator);
-        Map<Entity, List<uint>> mapBodyToContactPairs(allocator);
 
         // Process all the potential contacts after narrow-phase collision
-        processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds,
+        processAllPotentialContacts(narrowPhaseInput, true, potentialContactPoints, &mapPairIdToContactPairIndex, potentialContactManifolds, &contactPairs);
-                                    &contactPairs, mapBodyToContactPairs);
 
         // Reduce the number of contact points in the manifolds
         reducePotentialContactManifolds(&contactPairs, potentialContactManifolds, potentialContactPoints);
 
         // Create the actual contact manifolds and contact points
-        createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds,
+        createSnapshotContacts(contactPairs, contactManifolds, contactPoints, potentialContactManifolds, potentialContactPoints);
-                               potentialContactPoints);
 
         // Report the contacts to the user
         reportContacts(callback, &contactPairs, &contactManifolds, &contactPoints, lostContactPairs);
@@ -728,6 +725,14 @@ void CollisionDetectionSystem::createContacts() {
         contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
         contactPair.contactPointsIndex = mCurrentContactPoints->size();
 
+        // Add the associated contact pair to both bodies of the pair (used to create islands later)
+        if (mRigidBodyComponents.hasComponent(contactPair.body1Entity)) {
+           mRigidBodyComponents.addContacPair(contactPair.body1Entity, p);
+        }
+        if (mRigidBodyComponents.hasComponent(contactPair.body2Entity)) {
+           mRigidBodyComponents.addContacPair(contactPair.body2Entity, p);
+        }
+
         // For each potential contact manifold of the pair
         for (uint m=0; m < contactPair.potentialContactManifoldsIndices.size(); m++) {
 
@@ -985,8 +990,7 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                                                         List<ContactPointInfo>& potentialContactPoints,
                                                         Map<uint64, uint>* mapPairIdToContactPairIndex,
                                                         List<ContactManifoldInfo>& potentialContactManifolds,
-                                                        List<ContactPair>* contactPairs,
+                                                        List<ContactPair>* contactPairs) {
-                                                        Map<Entity, List<uint>>& mapBodyToContactPairs) {
 
     RP3D_PROFILE("CollisionDetectionSystem::processPotentialContacts()", mProfiler);
 
@@ -1035,25 +1039,6 @@ void CollisionDetectionSystem::processPotentialContacts(NarrowPhaseInfoBatch& na
                 contactPairs->add(overlappingPairContact);
                 pairContact = &((*contactPairs)[newContactPairIndex]);
                 mapPairIdToContactPairIndex->add(Pair<uint64, uint>(pairId, newContactPairIndex));
-
-                auto itbodyContactPairs = mapBodyToContactPairs.find(body1Entity);
-                if (itbodyContactPairs != mapBodyToContactPairs.end()) {
-                    itbodyContactPairs->second.add(newContactPairIndex);
-                }
-                else {
-                    List<uint> contactPairs(mMemoryManager.getPoolAllocator(), 1);
-                    contactPairs.add(newContactPairIndex);
-                    mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body1Entity, contactPairs));
-                }
-                itbodyContactPairs = mapBodyToContactPairs.find(body2Entity);
-                if (itbodyContactPairs != mapBodyToContactPairs.end()) {
-                    itbodyContactPairs->second.add(newContactPairIndex);
-                }
-                else {
-                    List<uint> contactPairs(mMemoryManager.getPoolAllocator(), 1);
-                    contactPairs.add(newContactPairIndex);
-                    mapBodyToContactPairs.add(Pair<Entity, List<uint>>(body2Entity, contactPairs));
-                }
             }
             else { // If a ContactPair already exists for this overlapping pair, we use this one