Refactor islands creation

CMakeLists.txt

@@ -137,6 +137,7 @@ SET (REACTPHYSICS3D_HEADERS
     "src/engine/DynamicsWorld.h"
     "src/engine/EventListener.h"
     "src/engine/Island.h"
+    "src/engine/Islands.h"
     "src/engine/Material.h"
     "src/engine/OverlappingPair.h"
     "src/engine/Timer.h"

src/collision/CollisionDetection.cpp

@@ -42,6 +42,7 @@
 #include "utils/Profiler.h"
 #include "engine/EventListener.h"
 #include "collision/RaycastInfo.h"
+#include "engine/Islands.h"
 #include <cassert>
 #include <iostream>
 
@@ -53,22 +54,23 @@ using namespace std;
 // Constructor
 CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents,
                                        DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager)
-                   : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mTransformComponents(transformComponents),
+                   : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents),
                      mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
                      mOverlappingPairs(mMemoryManager.getPoolAllocator()),
-                     mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents),
+                     mBroadPhaseSystem(*this, mProxyShapesComponents, transformComponents, dynamicsComponents),
                      mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mMapBroadPhaseIdToProxyShapeEntity(memoryManager.getPoolAllocator()),
-                     mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getPoolAllocator()),
+                     mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()), mPotentialContactPoints(mMemoryManager.getSingleFrameAllocator()),
                      // TODO : We should probably use single frame allocator for mPotentialContactPoints, mPotentialContactManifolds,  mMapPairIdToOverlappingPairContacts
-                     mPotentialContactManifolds(mMemoryManager.getPoolAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
+                     mPotentialContactManifolds(mMemoryManager.getSingleFrameAllocator()), mContactPairs1(mMemoryManager.getPoolAllocator()),
                      mContactPairs2(mMemoryManager.getPoolAllocator()), mPreviousContactPairs(&mContactPairs1), mCurrentContactPairs(&mContactPairs2),
                      mMapPairIdToContactPairIndex1(mMemoryManager.getPoolAllocator()), mMapPairIdToContactPairIndex2(mMemoryManager.getPoolAllocator()),
                      mPreviousMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex1), mCurrentMapPairIdToContactPairIndex(&mMapPairIdToContactPairIndex2),
+                     mContactPairsIndicesOrderingForContacts(memoryManager.getSingleFrameAllocator()),
                      mContactManifolds1(mMemoryManager.getPoolAllocator()), mContactManifolds2(mMemoryManager.getPoolAllocator()),
                      mPreviousContactManifolds(&mContactManifolds1), mCurrentContactManifolds(&mContactManifolds2),
                      mContactPoints1(mMemoryManager.getPoolAllocator()),
                      mContactPoints2(mMemoryManager.getPoolAllocator()), mPreviousContactPoints(&mContactPoints1),
-                     mCurrentContactPoints(&mContactPoints2) {
+                     mCurrentContactPoints(&mContactPoints2), mMapBodyToContactPairs(mMemoryManager.getSingleFrameAllocator()) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -420,22 +422,18 @@ void CollisionDetection::computeNarrowPhase() {
     // Reduce the number of contact points in the manifolds
     reducePotentialContactManifolds(mOverlappingPairs);
 
-    // Create the actual contact manifolds and contacts (from potential contacts)
-    createContacts();
-
-    // Initialize the current contacts with the contacts from the previous frame (for warmstarting)
-    initContactsWithPreviousOnes();
-
     // Add all the contact manifolds (between colliding bodies) to the bodies
     addAllContactManifoldsToBodies();
 
     // Report contacts to the user
     reportAllContacts();
 
-    // Clear the list of narrow-phase infos
+    assert(mCurrentContactManifolds->size() == 0);
-    mNarrowPhaseInput.clear();
+    assert(mCurrentContactPoints->size() == 0);
 
-    // TODO : Do not forget to clear all the contact pair, contact manifolds and contact points lists
+    mContactPairsIndicesOrderingForContacts.reserve(mCurrentContactPairs->size());
+
+    mNarrowPhaseInput.clear();
 }
 
 // Swap the previous and current contacts lists
@@ -467,24 +465,26 @@ void CollisionDetection::swapPreviousAndCurrentContacts() {
     }
 }
 
-// Create the actual contact manifolds and contacts (from potential contacts)
+// Create the actual contact manifolds and contacts points
+/// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of
+/// same islands packed together linearly and contact pairs that are not part of islands at the end.
+/// This is used when we create contact manifolds and contact points so that there are also packed
+/// together linearly if they are part of the same island.
 void CollisionDetection::createContacts() {
 
     RP3D_PROFILE("CollisionDetection::createContacts()", mProfiler);
 
-    assert(mCurrentContactManifolds->size() == 0);
+    assert(mCurrentContactPairs->size() == mContactPairsIndicesOrderingForContacts.size());
-    assert(mCurrentContactPoints->size() == 0);
+
+    mCurrentContactManifolds->reserve(mCurrentContactPairs->size());
+    mCurrentContactPoints->reserve(mCurrentContactManifolds->size());
 
     // For each contact pair
-    for (auto it = mCurrentMapPairIdToContactPairIndex->begin(); it != mCurrentMapPairIdToContactPairIndex->end(); ++it) {
+    for (uint p=0; p < mContactPairsIndicesOrderingForContacts.size(); p++) {
-
-        const uint contactPairIndex = it->second;
-        assert(contactPairIndex < mCurrentContactPairs->size());
-        ContactPair& contactPair = (*mCurrentContactPairs)[contactPairIndex];
 
+        ContactPair& contactPair = (*mCurrentContactPairs)[mContactPairsIndicesOrderingForContacts[p]];
         assert(contactPair.potentialContactManifoldsIndices.size() > 0);
 
-        // Start index and numnber of contact manifolds for this contact pair
         contactPair.contactManifoldsIndex = mCurrentContactManifolds->size();
         contactPair.nbContactManifolds = contactPair.potentialContactManifoldsIndices.size();
         contactPair.contactPointsIndex = mCurrentContactPoints->size();
@@ -496,7 +496,8 @@ void CollisionDetection::createContacts() {
 
             // Start index and number of contact points for this manifold
             const uint contactPointsIndex = mCurrentContactPoints->size();
-            const int8 nbContactPoints = potentialManifold.potentialContactPointsIndices.size();
+            const int8 nbContactPoints = static_cast<int8>(potentialManifold.potentialContactPointsIndices.size());
+            contactPair.nbToTalContactPoints += nbContactPoints;
 
             // We create a new contact manifold
             ContactManifold contactManifold(contactPointsIndex, nbContactPoints, mMemoryManager.getPoolAllocator(), mWorld->mConfig);
@@ -504,9 +505,6 @@ void CollisionDetection::createContacts() {
             // Add the contact manifold
             mCurrentContactManifolds->add(contactManifold);
 
-            // Increase the number of total contact point of the contact pair
-            contactPair.nbToTalContactPoints += potentialManifold.potentialContactPointsIndices.size();
-
             assert(potentialManifold.potentialContactPointsIndices.size() > 0);
 
             // For each contact point of the manifold
@@ -523,9 +521,13 @@ void CollisionDetection::createContacts() {
         }
     }
 
+    // Initialize the current contacts with the contacts from the previous frame (for warmstarting)
+    initContactsWithPreviousOnes();
+
     // Reset the potential contacts
-    mPotentialContactPoints.clear();
+    mPotentialContactPoints.clear(true);
-    mPotentialContactManifolds.clear();
+    mPotentialContactManifolds.clear(true);
+    mContactPairsIndicesOrderingForContacts.clear(true);
 }
 
 // Initialize the current contacts with the contacts from the previous frame (for warmstarting)
@@ -800,6 +802,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
             const uint contactPointIndex = static_cast<uint>(mPotentialContactPoints.size());
 
             // TODO : We should probably use single frame allocator here for mPotentialContactPoints
+            //        If so, do not forget to call mPotentialContactPoints.clear(true) at the end of frame
             mPotentialContactPoints.add(contactPoint);
 
             bool similarManifoldFound = false;
@@ -846,6 +849,7 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
 
                 // Create a new contact manifold for the overlapping pair
                 // TODO : We should probably use single frame allocator here
+                //        If so, do not forget to call mPotentialContactPoints.clear(true) at the end of frame
                 ContactManifoldInfo contactManifoldInfo(pairId, mMemoryManager.getPoolAllocator());
 
                 // Add the contact point to the manifold
@@ -854,12 +858,34 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
                 // If the overlapping pair contact does not exists yet
                 if (pairContact == nullptr) {
 
+                    Entity body1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity();
+                    Entity body2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity();
+
                     // TODO : We should probably use a single frame allocator here
-                    ContactPair overlappingPairContact(pairId, mMemoryManager.getPoolAllocator());
                     const uint newContactPairIndex = mCurrentContactPairs->size();
+                    ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, newContactPairIndex, mMemoryManager.getPoolAllocator());
                     mCurrentContactPairs->add(overlappingPairContact);
                     pairContact = &((*mCurrentContactPairs)[newContactPairIndex]);
                     mCurrentMapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));
+
+                    auto itbodyContactPairs = mMapBodyToContactPairs.find(body1Entity);
+                    if (itbodyContactPairs != mMapBodyToContactPairs.end()) {
+                        itbodyContactPairs->second.add(newContactPairIndex);
+                    }
+                    else {
+                        List<uint> contactPairs(mMemoryManager.getSingleFrameAllocator(), 1);
+                        contactPairs.add(newContactPairIndex);
+                        mMapBodyToContactPairs.add(Pair<Entity, List<uint>>(body1Entity, contactPairs));
+                    }
+                    itbodyContactPairs = mMapBodyToContactPairs.find(body2Entity);
+                    if (itbodyContactPairs != mMapBodyToContactPairs.end()) {
+                        itbodyContactPairs->second.add(newContactPairIndex);
+                    }
+                    else {
+                        List<uint> contactPairs(mMemoryManager.getSingleFrameAllocator(), 1);
+                        contactPairs.add(newContactPairIndex);
+                        mMapBodyToContactPairs.add(Pair<Entity, List<uint>>(body2Entity, contactPairs));
+                    }
                 }
 
                 assert(pairContact != nullptr);

src/collision/CollisionDetection.h

@@ -82,9 +82,6 @@ class CollisionDetection {
         /// Reference the proxy-shape components
         ProxyShapeComponents& mProxyShapesComponents;
 
-        /// Reference the transform components
-        TransformComponents& mTransformComponents;
-
         /// Collision Detection Dispatch configuration
         CollisionDispatch mCollisionDispatch;
 
@@ -138,6 +135,12 @@ class CollisionDetection {
         /// (either mMapPairIdToContactPairIndex1 or mMapPairIdToContactPairIndex2)
         Map<OverlappingPair::OverlappingPairId, uint>* mCurrentMapPairIdToContactPairIndex;
 
+        /// List of the indices of the contact pairs (in mCurrentContacPairs array) with contact pairs of
+        /// same islands packed together linearly and contact pairs that are not part of islands at the end.
+        /// This is used when we create contact manifolds and contact points so that there are also packed
+        /// together linearly if they are part of the same island.
+        List<uint> mContactPairsIndicesOrderingForContacts;
+
         /// First list with the contact manifolds
         List<ContactManifold> mContactManifolds1;
 
@@ -162,6 +165,9 @@ class CollisionDetection {
         /// 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_PROFILING_ACTIVE
 
 		/// Pointer to the profiler
@@ -214,7 +220,7 @@ class CollisionDetection {
         /// Reduce the potential contact manifolds and contact points of the overlapping pair contacts
         void reducePotentialContactManifolds(const OverlappingPairMap& overlappingPairs);
 
-        /// Create the actual contact manifolds and contacts (from potential contacts)
+        /// Create the actual contact manifolds and contacts points (from potential contacts) for a given contact pair
         void createContacts();
 
         /// Initialize the current contacts with the contacts from the previous frame (for warmstarting)

src/collision/ContactPair.h

@@ -50,6 +50,18 @@ struct ContactPair {
         /// Indices of the potential contact manifolds
         List<uint> potentialContactManifoldsIndices;
 
+        /// Entity of the first body of the manifold
+        Entity body1Entity;
+
+        /// Entity of the second body of the manifold
+        Entity body2Entity;
+
+        /// True if the manifold is already in an island
+        bool isAlreadyInIsland;
+
+        /// Index of the contact pair in the array of pairs
+        uint contactPairIndex;
+
         /// Index of the first contact manifold in the array
         uint contactManifoldsIndex;
 
@@ -65,8 +77,9 @@ struct ContactPair {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPair(OverlappingPair::OverlappingPairId pairId, MemoryAllocator& allocator)
+        ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, uint contactPairIndex, MemoryAllocator& allocator)
-            : pairId(pairId), potentialContactManifoldsIndices(allocator), contactManifoldsIndex(0), nbContactManifolds(0),
+            : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
+              isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
               contactPointsIndex(0), nbToTalContactPoints(0) {
 
         }

src/components/DynamicsComponents.cpp

@@ -35,7 +35,7 @@ using namespace reactphysics3d;
 
 // Constructor
 DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
-                    :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) {
+                    :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3) + sizeof(bool)) {
 
     // Allocate memory for the components data
     allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -57,6 +57,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     Entity* newBodies = static_cast<Entity*>(newBuffer);
     Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newBodies + nbComponentsToAllocate);
     Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
+    bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newAngularVelocities + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -65,6 +66,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newBodies, mBodies, mNbComponents * sizeof(Entity));
         memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Vector3));
         memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
 
         // Deallocate previous memory
         mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@@ -74,6 +76,7 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     mBodies = newBodies;
     mLinearVelocities = newLinearVelocities;
     mAngularVelocities = newAngularVelocities;
+    mIsAlreadyInIsland = newIsAlreadyInIsland;
     mNbAllocatedComponents = nbComponentsToAllocate;
 }
 
@@ -87,6 +90,7 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
     new (mBodies + index) Entity(bodyEntity);
     new (mLinearVelocities + index) Vector3(component.linearVelocity);
     new (mAngularVelocities + index) Vector3(component.angularVelocity);
+    mIsAlreadyInIsland[index] = false;
 
     // Map the entity with the new component lookup index
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
@@ -107,6 +111,7 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
     new (mBodies + destIndex) Entity(mBodies[srcIndex]);
     new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
     new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
+    mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
 
     // Destroy the source component
     destroyComponent(srcIndex);
@@ -129,6 +134,7 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     Entity entity1(mBodies[index1]);
     Vector3 linearVelocity1(mLinearVelocities[index1]);
     Vector3 angularVelocity1(mAngularVelocities[index1]);
+    bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
 
     // Destroy component 1
     destroyComponent(index1);
@@ -139,6 +145,7 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     new (mBodies + index2) Entity(entity1);
     new (mLinearVelocities + index2) Vector3(linearVelocity1);
     new (mAngularVelocities + index2) Vector3(angularVelocity1);
+    mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
 
     // Update the entity to component index mapping
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));

src/components/DynamicsComponents.h

@@ -59,6 +59,9 @@ class DynamicsComponents : public Components {
         /// Array with the angular velocity of each component
         Vector3* mAngularVelocities;
 
+        /// Array with the boolean value to know if the body has already been added into an island
+        bool* mIsAlreadyInIsland;
+
         // -------------------- Methods -------------------- //
 
         /// Allocate memory for a given number of components
@@ -105,15 +108,22 @@ class DynamicsComponents : public Components {
         /// Return the angular velocity of an entity
         Vector3& getAngularVelocity(Entity bodyEntity) const;
 
+        /// Return true if the entity is already in an island
+        bool getIsAlreadyInIsland(Entity bodyEntity) const;
+
         /// Set the linear velocity of an entity
         void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity);
 
         /// Set the angular velocity of an entity
         void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity);
 
+        /// Set the value to know if the entity is already in an island
+        bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
+
         // -------------------- Friendship -------------------- //
 
         friend class BroadPhaseSystem;
+        friend class DynamicsWorld;
 };
 
 // Return the linear velocity of an entity
@@ -148,6 +158,22 @@ inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vect
    mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity;
 }
 
+// Return true if the entity is already in an island
+inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+/// Set the value to know if the entity is already in an island
+inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland;
+}
+
 }
 
 #endif

src/engine/DynamicsWorld.cpp

@@ -32,7 +32,9 @@
 #include "utils/Profiler.h"
 #include "engine/EventListener.h"
 #include "engine/Island.h"
+#include "engine/Islands.h"
 #include "collision/ContactManifold.h"
+#include "containers/Stack.h"
 
 // Namespaces
 using namespace reactphysics3d;
@@ -60,7 +62,8 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
                 mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
                 mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity),
                 mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
-                mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
+                mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0),
+                mIslands2(mMemoryManager.getSingleFrameAllocator()) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -128,6 +131,32 @@ void DynamicsWorld::update(decimal timeStep) {
     // Compute the islands (separate groups of bodies with constraints between each others)
     computeIslands();
 
+    // Create the islands
+    createIslands();
+
+    // TODO : DELETE THIS
+    /*
+    std::cout << "--------------------- FRAME ------------------------ " << std::endl;
+    std::cout << " ---- OLD ISLANDS -----" << std::endl;
+    for (uint i=0; i < mNbIslands; i++) {
+        std::cout << "Island " << i << std::endl;
+        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+            std::cout << "Body Id : " << mIslands[i]->getBodies()[b]->getId() << std::endl;
+        }
+    }
+
+    std::cout << " ---- NEW ISLANDS -----" << std::endl;
+    for (uint i=0; i < mIslands2.getNbIslands(); i++) {
+        std::cout << "Island " << i << std::endl;
+        for (uint b=0; b < mIslands2.bodyEntities[i].size(); b++) {
+            std::cout << "Body Id : " << mBodyComponents.getBody(mIslands2.bodyEntities[i][b])->getId() << std::endl;
+        }
+    }
+    */
+
+    // Create the actual narrow-phase contacts
+    mCollisionDetection.createContacts();
+
     // Integrate the velocities
     integrateRigidBodiesVelocities();
 
@@ -151,6 +180,9 @@ void DynamicsWorld::update(decimal timeStep) {
     // Reset the external force and torque applied to the bodies
     resetBodiesForceAndTorque();
 
+    // Reset the islands
+    mIslands2.clear();
+
     // Reset the single frame memory allocator
     mMemoryManager.resetFrameAllocator();
 }
@@ -672,6 +704,7 @@ uint DynamicsWorld::computeNextAvailableJointId() {
     return jointId;
 }
 
+// TODO : DELETE THIS
 // Compute the islands of awake bodies.
 /// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
 /// between each other. This method computes the islands at each time step as follows: For each
@@ -828,6 +861,179 @@ void DynamicsWorld::computeIslands() {
      }
 }
 
+// Compute the islands using potential contacts and joints
+/// We compute the islands before creating the actual contacts here because we want all
+/// the contact manifolds and contact points of the same island
+/// to be packed together into linear arrays of manifolds and contacts for better caching.
+/// An island is an isolated group of rigid bodies that have constraints (joints or contacts)
+/// between each other. This method computes the islands at each time step as follows: For each
+/// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body
+/// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to
+/// find all the bodies that are connected with it (the bodies that share joints or contacts with
+/// it). Then, we create an island with this group of connected bodies.
+void DynamicsWorld::createIslands() {
+
+    // TODO : Check if we handle kinematic bodies correctly in islands creation
+
+    // list of contact pairs involving a non-rigid body
+    List<uint> nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator());
+
+    RP3D_PROFILE("DynamicsWorld::createIslandsAndContacts()", mProfiler);
+
+    // Reset all the isAlreadyInIsland variables of bodies and joints
+    for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
+        mDynamicsComponents.mIsAlreadyInIsland[b] = false;
+    }
+    for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
+        (*it)->mIsAlreadyInIsland = false;
+    }
+
+    // Create a stack for the bodies to visit during the Depth First Search
+    Stack<Entity> bodyEntityIndicesToVisit(mMemoryManager.getSingleFrameAllocator());
+
+    uint nbTotalManifolds = 0;
+
+    // For each dynamic component
+    // TODO : Here we iterate on dynamic component where we can have static, kinematic and dynamic bodies. Maybe we should
+    //        not use a dynamic component for a static body.
+    for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
+
+        // If the body has already been added to an island, we go to the next body
+        if (mDynamicsComponents.mIsAlreadyInIsland[b]) continue;
+
+        // If the body is static, we go to the next body
+        // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+        CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[b]));
+        if (body->getType() == BodyType::STATIC) continue;
+
+        // Reset the stack of bodies to visit
+        bodyEntityIndicesToVisit.clear();
+
+        // Add the body into the stack of bodies to visit
+        mDynamicsComponents.mIsAlreadyInIsland[b] = true;
+        bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]);
+
+        // Create the new island
+        uint32 islandIndex = mIslands2.addIsland(nbTotalManifolds);
+
+        // While there are still some bodies to visit in the stack
+        while (bodyEntityIndicesToVisit.size() > 0) {
+
+            // Get the next body to visit from the stack
+            const Entity bodyToVisitEntity = bodyEntityIndicesToVisit.pop();
+
+            // Awake the body if it is sleeping
+            notifyBodyDisabled(bodyToVisitEntity, false);
+
+            // Add the body into the island
+            mIslands2.bodyEntities[islandIndex].add(bodyToVisitEntity);
+
+            // If the current body is static, we do not want to perform the DFS
+            // search across that body
+            // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+            RigidBody* rigidBodyToVisit = static_cast<RigidBody*>(mBodyComponents.getBody(bodyToVisitEntity));
+            if (rigidBodyToVisit->getType() == BodyType::STATIC) 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 < contactPairs.size(); p++) {
+
+                    ContactPair& pair = (*mCollisionDetection.mCurrentContactPairs)[contactPairs[p]];
+                    assert(pair.potentialContactManifoldsIndices.size() > 0);
+
+                    // Check if the current contact pair has already been added into an island
+                    if (pair.isAlreadyInIsland) continue;
+
+                    // Get the other body of the contact manifold
+                    // TODO : Maybe avoid those casts here
+                    RigidBody* body1 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body1Entity));
+                    RigidBody* body2 = dynamic_cast<RigidBody*>(mBodyComponents.getBody(pair.body2Entity));
+
+                    // If the colliding body is a RigidBody (and not a CollisionBody instead)
+                    if (body1 != nullptr && body2 != nullptr) {
+
+                        nbTotalManifolds += pair.potentialContactManifoldsIndices.size();
+
+                        // Add the pair into the list of pair to process to create contacts
+                        mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex);
+
+                        // Add the contact manifold into the island
+                        mIslands2.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
+                        pair.isAlreadyInIsland = true;
+
+                        const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
+
+                        // Check if the other body has already been added to the island
+                        if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
+
+                        // Insert the other body into the stack of bodies to visit
+                        bodyEntityIndicesToVisit.push(otherBodyEntity);
+                        mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, true);
+                    }
+                    else {
+
+                        // Add the contact pair index in the list of contact pairs that won't be part of islands
+                        nonRigidBodiesContactPairs.add(pair.contactPairIndex);
+                    }
+                }
+            }
+
+            // For each joint in which the current body is involved
+            JointListElement* jointElement;
+            for (jointElement = rigidBodyToVisit->mJointsList; jointElement != nullptr;
+                 jointElement = jointElement->next) {
+
+                Joint* joint = jointElement->joint;
+
+                // Check if the current joint has already been added into an island
+                if (joint->isAlreadyInIsland()) continue;
+
+                // Add the joint into the island
+                mIslands2.joints.add(joint);
+                joint->mIsAlreadyInIsland = true;
+
+                const Entity body1Entity = joint->getBody1()->getEntity();
+                const Entity body2Entity = joint->getBody2()->getEntity();
+                const Entity otherBodyEntity = body1Entity == bodyToVisitEntity ? body2Entity : body1Entity;
+
+                // TODO : Maybe avoid those casts here
+                // Get the other body of the contact manifold
+                RigidBody* body1 = static_cast<RigidBody*>(joint->getBody1());
+                RigidBody* body2 = static_cast<RigidBody*>(joint->getBody2());
+                RigidBody* otherBody = (body1->getId() == rigidBodyToVisit->getId()) ? body2 : body1;
+
+                // Check if the other body has already been added to the island
+                if (otherBody->mIsAlreadyInIsland) continue;
+
+                // Insert the other body into the stack of bodies to visit
+                bodyEntityIndicesToVisit.push(otherBodyEntity);
+                otherBody->mIsAlreadyInIsland = true;
+            }
+        }
+
+        // Reset the isAlreadyIsland variable of the static bodies so that they
+        // can also be included in the other islands
+        for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
+
+            // If the body is static, we go to the next body
+            // TODO : Do not use pointer to rigid body here (maybe move getType() into a component)
+            CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[b]));
+            if (body->getType() == BodyType::STATIC) {
+                mDynamicsComponents.mIsAlreadyInIsland[b] = false;
+            }
+        }
+    }
+
+    // Add the contact pairs that are not part of islands at the end of the array of pairs for contacts creations
+    mCollisionDetection.mContactPairsIndicesOrderingForContacts.addRange(nonRigidBodiesContactPairs);
+
+    mCollisionDetection.mMapBodyToContactPairs.clear(true);
+}
+
 // Put bodies to sleep if needed.
 /// For each island, if all the bodies have been almost still for a long enough period of
 /// time, we put all the bodies of the island to sleep.

src/engine/DynamicsWorld.h

@@ -33,6 +33,7 @@
 #include "utils/Logger.h"
 #include "engine/ContactSolver.h"
 #include "components/DynamicsComponents.h"
+#include "engine/Islands.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -126,6 +127,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Current joint id
         uint mCurrentJointId;
 
+        /// All the islands of bodies of the current frame
+        Islands mIslands2;
+
         // -------------------- Methods -------------------- //
 
         /// Integrate the positions and orientations of rigid bodies.
@@ -149,6 +153,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute the islands of awake bodies.
         void computeIslands();
 
+        /// Compute the islands using potential contacts and joints and create the actual contacts.
+        void createIslands();
+
         /// Update the postion/orientation of the bodies
         void updateBodiesState();