Refactor contacts and islands

src/body/Body.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
  * @param id ID of the new body
  */
 Body::Body(Entity entity, bodyindex id)
-     : mID(id), mEntity(entity), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
+     : mID(id), mEntity(entity), mIsAllowedToSleep(true), mIsActive(true),
        mIsSleeping(false), mSleepTime(0), mUserData(nullptr) {
 
 #ifdef IS_LOGGING_ACTIVE

src/body/Body.h

@@ -57,9 +57,6 @@ class Body {
         /// Identifier of the entity in the ECS
         Entity mEntity;
 
-        /// True if the body has already been added in an island (for sleeping technique)
-        bool mIsAlreadyInIsland;
-
         /// True if the body is allowed to go to sleep for better efficiency
         bool mIsAllowedToSleep;
 
@@ -75,8 +72,10 @@ class Body {
         bool mIsActive;
 
         /// True if the body is sleeping (for sleeping technique)
+        // TODO : DELETE THIS
         bool mIsSleeping;
 
+        // TODO : Move this into the body components
         /// Elapsed time since the body velocity was bellow the sleep velocity
         decimal mSleepTime;
 

src/body/CollisionBody.cpp

@@ -307,26 +307,6 @@ void CollisionBody::askForBroadPhaseCollisionCheck() const {
     }
 }
 
-// Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
-/// This method also returns the number of contact manifolds of the body.
-int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
-
-    mIsAlreadyInIsland = false;
-
-    int nbManifolds = 0;
-
-    // Reset the mIsAlreadyInIsland variable of the contact manifolds for
-    // this body
-    ContactManifoldListElement* currentElement = mContactManifoldsList;
-    while (currentElement != nullptr) {
-        currentElement->getContactManifold()->mIsAlreadyInIsland = false;
-        currentElement = currentElement->getNext();
-        nbManifolds++;
-    }
-
-    return nbManifolds;
-}
-
 // Return true if a point is inside the collision body
 /// This method returns true if a point is inside any collision shape of the body
 /**

src/body/CollisionBody.h

@@ -67,6 +67,7 @@ class CollisionBody : public Body {
 
         // -------------------- Attributes -------------------- //
 
+        // TODO : Move this into the dynamics components
         /// Type of body (static, kinematic or dynamic)
         BodyType mType;
 
@@ -98,9 +99,6 @@ class CollisionBody : public Body {
         /// (as if the body has moved).
         void askForBroadPhaseCollisionCheck() const;
 
-        /// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
-        int resetIsAlreadyInIslandAndCountManifolds();
-
         /// Set the variable to know whether or not the body is sleeping
         virtual void setIsSleeping(bool isSleeping) override;
 

src/body/RigidBody.cpp

@@ -178,6 +178,8 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
  */
 void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
 
+    // TODO : Check if we need to update the postion of the body here at the end (transform of the body)
+
     if (mType != BodyType::DYNAMIC) return;
 
     mIsCenterOfMassSetByUser = true;

src/collision/CollisionDetection.cpp

@@ -172,14 +172,6 @@ void CollisionDetection::updateOverlappingPairs(List<Pair<int, int>>& overlappin
             }
         }
     }
-
-    // For each new overlapping pair
-    for (uint i=0; i < newOverlappingPairs.size(); i++) {
-
-        // Wake up the two bodies of the new overlapping pair
-        mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
-        mWorld->notifyBodyDisabled(newOverlappingPairs[i]->getShape1()->getBody()->getEntity(), false);
-    }
 }
 
 // Compute the middle-phase collision detection
@@ -201,12 +193,15 @@ void CollisionDetection::computeMiddlePhase() {
         // Make all the last frame collision info obsolete
         pair->makeLastFrameCollisionInfosObsolete();
 
+        const Entity proxyShape1Entity = pair->getShape1()->getEntity();
+        const Entity proxyShape2Entity = pair->getShape2()->getEntity();
+
         ProxyShape* shape1 = pair->getShape1();
         ProxyShape* shape2 = pair->getShape2();
 
-        assert(shape1->getBroadPhaseId() != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != -1);
-        assert(shape2->getBroadPhaseId() != -1);
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity) != -1);
-        assert(shape1->getBroadPhaseId() != shape2->getBroadPhaseId());
+        assert(mProxyShapesComponents.getBroadPhaseId(proxyShape1Entity) != mProxyShapesComponents.getBroadPhaseId(proxyShape2Entity));
 
         // Check if the two shapes are still overlapping. Otherwise, we destroy the
         // overlapping pair
@@ -224,15 +219,18 @@ void CollisionDetection::computeMiddlePhase() {
         }
 
         // Check if the collision filtering allows collision between the two shapes
-        if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) != 0 &&
+        if ((mProxyShapesComponents.getCollideWithMaskBits(proxyShape1Entity) & mProxyShapesComponents.getCollisionCategoryBits(proxyShape2Entity)) != 0 &&
-             (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) != 0)) {
+            (mProxyShapesComponents.getCollisionCategoryBits(proxyShape1Entity) & mProxyShapesComponents.getCollideWithMaskBits(proxyShape2Entity)) != 0) {
 
             CollisionBody* const body1 = shape1->getBody();
             CollisionBody* const body2 = shape2->getBody();
 
+            const Entity body1Entity = body1->getEntity();
+            const Entity body2Entity = body2->getEntity();
+
             // Check that at least one body is awake and not static
-            bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
+            bool isBody1Active = !mWorld->mBodyComponents.getIsEntityDisabled(body1Entity) && body1->getType() != BodyType::STATIC;
-            bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
+            bool isBody2Active = !mWorld->mBodyComponents.getIsEntityDisabled(body2Entity) && body2->getType() != BodyType::STATIC;
             if (!isBody1Active && !isBody2Active) continue;
 
             // Check if the bodies are in the set of bodies that cannot collide between each other
@@ -500,7 +498,9 @@ void CollisionDetection::createContacts() {
             contactPair.nbToTalContactPoints += nbContactPoints;
 
             // We create a new contact manifold
-            ContactManifold contactManifold(contactPointsIndex, nbContactPoints, mMemoryManager.getPoolAllocator(), mWorld->mConfig);
+            ContactManifold contactManifold(contactPair.body1Entity, contactPair.body2Entity, contactPair.proxyShape1Entity,
+                                            contactPair.proxyShape2Entity, contactPointsIndex, nbContactPoints,
+                                            mMemoryManager.getPoolAllocator(), mWorld->mConfig);
 
             // Add the contact manifold
             mCurrentContactManifolds->add(contactManifold);
@@ -861,9 +861,14 @@ void CollisionDetection::processPotentialContacts(NarrowPhaseInfoBatch& narrowPh
                     Entity body1Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getBody()->getEntity();
                     Entity body2Entity = narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getBody()->getEntity();
 
+                    assert(!mWorld->mBodyComponents.getIsEntityDisabled(body1Entity) || !mWorld->mBodyComponents.getIsEntityDisabled(body2Entity));
+
                     // TODO : We should probably use a single frame allocator here
                     const uint newContactPairIndex = mCurrentContactPairs->size();
-                    ContactPair overlappingPairContact(pairId, body1Entity, body2Entity, newContactPairIndex, mMemoryManager.getPoolAllocator());
+                    ContactPair overlappingPairContact(pairId, body1Entity, body2Entity,
+                                                       narrowPhaseInfoBatch.overlappingPairs[i]->getShape1()->getEntity(),
+                                                       narrowPhaseInfoBatch.overlappingPairs[i]->getShape2()->getEntity(),
+                                                       newContactPairIndex, mMemoryManager.getPoolAllocator());
                     mCurrentContactPairs->add(overlappingPairContact);
                     pairContact = &((*mCurrentContactPairs)[newContactPairIndex]);
                     mCurrentMapPairIdToContactPairIndex->add(Pair<OverlappingPair::OverlappingPairId, uint>(pairId, newContactPairIndex));

src/collision/ContactManifold.cpp

@@ -38,14 +38,17 @@ ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
                   mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
                   mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
                   mNext(nullptr), mPrevious(nullptr), mIsObsolete(false),
-                  mWorldSettings(worldSettings) {
+                  mWorldSettings(worldSettings), bodyEntity1(0, 0), bodyEntity2(0, 0), proxyShapeEntity1(0, 0), proxyShapeEntity2(0, 0) {
     
 }
 
 // Constructor
 // TODO : REMOVE worldSettings from this constructor
-ContactManifold::ContactManifold(uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings)
+ContactManifold::ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
-                :mMemoryAllocator(allocator), mContactPointsIndex(0), mWorldSettings(worldSettings) {
+                                 uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings)
+                :mMemoryAllocator(allocator), mContactPointsIndex(0), bodyEntity1(bodyEntity1), bodyEntity2(bodyEntity2),
+                 proxyShapeEntity1(proxyShapeEntity1), proxyShapeEntity2(proxyShapeEntity2), mFrictionImpulse1(0), mFrictionImpulse2(0),
+                 mFrictionTwistImpulse(0), mIsAlreadyInIsland(false), mWorldSettings(worldSettings) {
 
 
     mContactPoints = nullptr;

src/collision/ContactManifold.h

@@ -91,6 +91,7 @@ struct ContactManifoldListElement {
  */
 class ContactManifold {
 
+    // TODO : Check if we can use public fields in this class (maybe this class is used by users directly)
     private:
 
         // -------------------- Constants -------------------- //
@@ -108,6 +109,18 @@ class ContactManifold {
         /// Index of the first contact point of the manifold in the list of contact points
         uint mContactPointsIndex;
 
+        /// Entity of the first body in contact
+        Entity bodyEntity1;
+
+        /// Entity of the second body in contact
+        Entity bodyEntity2;
+
+        /// Entity of the first proxy-shape in contact
+        Entity proxyShapeEntity1;
+
+        /// Entity of the second proxy-shape in contact
+        Entity proxyShapeEntity2;
+
         /// Pointer to the first proxy shape of the contact
         ProxyShape* mShape1;
 
@@ -230,7 +243,9 @@ class ContactManifold {
                         const WorldSettings& worldSettings);
 
         /// Constructor
-        ContactManifold(uint contactPointsIndex, int8 nbContactPoints, MemoryAllocator& allocator, const WorldSettings& worldSettings);
+        ContactManifold(Entity bodyEntity1, Entity bodyEntity2, Entity proxyShapeEntity1, Entity proxyShapeEntity2,
+                        uint contactPointsIndex, int8 nbContactPoints,
+                        MemoryAllocator& allocator, const WorldSettings& worldSettings);
 
         /// Destructor
         ~ContactManifold();
@@ -241,6 +256,7 @@ class ContactManifold {
         /// Assignment operator
         ContactManifold& operator=(const ContactManifold& contactManifold) = default;
 
+        /*
         /// Return a pointer to the first proxy shape of the contact
         ProxyShape* getShape1() const;
 
@@ -252,6 +268,7 @@ class ContactManifold {
 
         /// Return a pointer to the second body of the contact manifold
         CollisionBody* getBody2() const;
+        */
 
         /// Return the number of contact points in the manifold
         int8 getNbContactPoints() const;
@@ -275,6 +292,7 @@ class ContactManifold {
         friend class CollisionDetection;
 };
 
+/*
 // Return a pointer to the first proxy shape of the contact
 inline ProxyShape* ContactManifold::getShape1() const {
     return mShape1;
@@ -294,6 +312,7 @@ inline CollisionBody* ContactManifold::getBody1() const {
 inline CollisionBody* ContactManifold::getBody2() const {
     return mShape2->getBody();
 }
+*/
 
 // Return the number of contact points in the manifold
 inline int8 ContactManifold::getNbContactPoints() const {

src/collision/ContactPair.h

@@ -50,12 +50,18 @@ struct ContactPair {
         /// Indices of the potential contact manifolds
         List<uint> potentialContactManifoldsIndices;
 
-        /// Entity of the first body of the manifold
+        /// Entity of the first body of the contact
         Entity body1Entity;
 
-        /// Entity of the second body of the manifold
+        /// Entity of the second body of the contact
         Entity body2Entity;
 
+        /// Entity of the first proxy-shape of the contact
+        Entity proxyShape1Entity;
+
+        /// Entity of the second proxy-shape of the contact
+        Entity proxyShape2Entity;
+
         /// True if the manifold is already in an island
         bool isAlreadyInIsland;
 
@@ -77,8 +83,10 @@ struct ContactPair {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, uint contactPairIndex, MemoryAllocator& allocator)
+        ContactPair(OverlappingPair::OverlappingPairId pairId, Entity body1Entity, Entity body2Entity, Entity proxyShape1Entity,
+                    Entity proxyShape2Entity, uint contactPairIndex, MemoryAllocator& allocator)
             : pairId(pairId), potentialContactManifoldsIndices(allocator), body1Entity(body1Entity), body2Entity(body2Entity),
+              proxyShape1Entity(proxyShape1Entity), proxyShape2Entity(proxyShape2Entity),
               isAlreadyInIsland(false), contactPairIndex(contactPairIndex), contactManifoldsIndex(0), nbContactManifolds(0),
               contactPointsIndex(0), nbToTalContactPoints(0) {
 

src/constraint/ContactPoint.cpp

@@ -51,7 +51,7 @@ ContactPoint::ContactPoint(const ContactPointInfo& contactInfo, const WorldSetti
                mPenetrationDepth(contactInfo.penetrationDepth),
                mLocalPointOnShape1(contactInfo.localPoint1),
                mLocalPointOnShape2(contactInfo.localPoint2),
-               mIsRestingContact(false), mIsObsolete(false),
+               mIsRestingContact(false), mPenetrationImpulse(0), mIsObsolete(false),
                mWorldSettings(worldSettings) {
 
     assert(mPenetrationDepth > decimal(0.0));

src/engine/CollisionWorld.h

@@ -163,12 +163,10 @@ class CollisionWorld {
         CollisionDispatch& getCollisionDispatch();
 
         /// Ray cast method
-        void raycast(const Ray& ray, RaycastCallback* raycastCallback,
+        void raycast(const Ray& ray, RaycastCallback* raycastCallback, unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
-                     unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
 
         /// Test if the AABBs of two bodies overlap
-        bool testAABBOverlap(const CollisionBody* body1,
+        bool testAABBOverlap(const CollisionBody* body1, const CollisionBody* body2) const;
-                             const CollisionBody* body2) const;
 
         /// Report all the bodies which have an AABB that overlaps with the AABB in parameter
         void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);

src/engine/ConstraintSolver.cpp

@@ -31,7 +31,7 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
+ConstraintSolver::ConstraintSolver(Islands& islands) : mIsWarmStartingActive(true), mIslands(islands) {
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -42,13 +42,12 @@ ConstraintSolver::ConstraintSolver() : mIsWarmStartingActive(true) {
 }
 
 // Initialize the constraint solver for a given island
-void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
+void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.bodyEntities[islandIndex].size() > 0);
-    assert(island->getNbBodies() > 0);
+    assert(mIslands.joints[islandIndex].size() > 0);
-    assert(island->getNbJoints() > 0);
 
     // Set the current time step
     mTimeStep = dt;
@@ -58,49 +57,44 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
     mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
 
     // For each joint of the island
-    Joint** joints = island->getJoints();
+    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
-    for (uint i=0; i<island->getNbJoints(); i++) {
 
         // Initialize the constraint before solving it
-        joints[i]->initBeforeSolve(mConstraintSolverData);
+        mIslands.joints[islandIndex][i]->initBeforeSolve(mConstraintSolverData);
 
         // Warm-start the constraint if warm-starting is enabled
         if (mIsWarmStartingActive) {
-            joints[i]->warmstart(mConstraintSolverData);
+            mIslands.joints[islandIndex][i]->warmstart(mConstraintSolverData);
         }
     }
 }
 
 // Solve the velocity constraints
-void ConstraintSolver::solveVelocityConstraints(Island* island) {
+void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.joints[islandIndex].size() > 0);
-    assert(island->getNbJoints() > 0);
 
     // For each joint of the island
-    Joint** joints = island->getJoints();
+    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
-    for (uint i=0; i<island->getNbJoints(); i++) {
 
         // Solve the constraint
-        joints[i]->solveVelocityConstraint(mConstraintSolverData);
+        mIslands.joints[islandIndex][i]->solveVelocityConstraint(mConstraintSolverData);
     }
 }
 
 // Solve the position constraints
-void ConstraintSolver::solvePositionConstraints(Island* island) {
+void ConstraintSolver::solvePositionConstraints(uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.joints[islandIndex].size() > 0);
-    assert(island->getNbJoints() > 0);
 
     // For each joint of the island
-    Joint** joints = island->getJoints();
+    for (uint i=0; i<mIslands.joints[islandIndex].size(); i++) {
-    for (uint i=0; i < island->getNbJoints(); i++) {
 
         // Solve the constraint
-        joints[i]->solvePositionConstraint(mConstraintSolverData);
+        mIslands.joints[islandIndex][i]->solvePositionConstraint(mConstraintSolverData);
     }
 }

src/engine/ConstraintSolver.h

@@ -29,6 +29,7 @@
 // Libraries
 #include "configuration.h"
 #include "mathematics/mathematics.h"
+#include "engine/Islands.h"
 
 namespace reactphysics3d {
 
@@ -153,6 +154,9 @@ class ConstraintSolver {
         /// True if the warm starting of the solver is active
         bool mIsWarmStartingActive;
 
+        /// Reference to the islands
+        Islands& mIslands;
+
         /// Constraint solver data used to initialize and solve the constraints
         ConstraintSolverData mConstraintSolverData;
 
@@ -167,19 +171,19 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver();
+        ConstraintSolver(Islands& islands);
 
         /// Destructor
         ~ConstraintSolver() = default;
 
         /// Initialize the constraint solver for a given island
-        void initializeForIsland(decimal dt, Island* island);
+        void initializeForIsland(decimal dt, uint islandIndex);
 
         /// Solve the constraints
-        void solveVelocityConstraints(Island* island);
+        void solveVelocityConstraints(uint islandIndex);
 
         /// Solve the position constraints
-        void solvePositionConstraints(Island* island);
+        void solvePositionConstraints(uint islandIndex);
 
         /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
         bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;

src/engine/ContactSolver.cpp

@@ -30,6 +30,8 @@
 #include "constraint/ContactPoint.h"
 #include "utils/Profiler.h"
 #include "engine/Island.h"
+#include "components/BodyComponents.h"
+#include "components/ProxyShapeComponents.h"
 #include "collision/ContactManifold.h"
 
 using namespace reactphysics3d;
@@ -41,11 +43,13 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings)
+ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
+                             ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings)
               :mMemoryManager(memoryManager), mSplitLinearVelocities(nullptr),
                mSplitAngularVelocities(nullptr), mContactConstraints(nullptr),
                mContactPoints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr),
-               mIsSplitImpulseActive(true), mWorldSettings(worldSettings) {
+               mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mBodyComponents(bodyComponents),
+               mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), mWorldSettings(worldSettings) {
 
 #ifdef IS_PROFILING_ACTIVE
         mProfiler = nullptr;
@@ -54,23 +58,18 @@ ContactSolver::ContactSolver(MemoryManager& memoryManager, const WorldSettings&
 }
 
 // Initialize the contact constraints
-void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
+void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
+
+    mAllContactManifolds = contactManifolds;
+    mAllContactPoints = contactPoints;
 
     RP3D_PROFILE("ContactSolver::init()", mProfiler);
 
     mTimeStep = timeStep;
 
     // TODO : Try not to count manifolds and contact points here
-    uint nbContactManifolds = 0;
+    uint nbContactManifolds = mAllContactManifolds->size();
-    uint nbContactPoints = 0;
+    uint nbContactPoints = mAllContactPoints->size();
-    for (uint i = 0; i < nbIslands; i++) {
-        uint nbManifoldsInIsland = islands[i]->getNbContactManifolds();
-        nbContactManifolds += nbManifoldsInIsland;
-
-        for (uint j=0; j < nbManifoldsInIsland; j++) {
-            nbContactPoints += islands[i]->getContactManifolds()[j]->getNbContactPoints();
-        }
-    }
 
     mNbContactManifolds = 0;
     mNbContactPoints = 0;
@@ -90,10 +89,10 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
     assert(mContactConstraints != nullptr);
 
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) {
+    for (uint i = 0; i < mIslands.getNbIslands(); i++) {
 
-        if (islands[islandIndex]->getNbContactManifolds() > 0) {
+        if (mIslands.nbContactManifolds[i] > 0) {
-            initializeForIsland(islands[islandIndex]);
+            initializeForIsland(i);
         }
     }
 
@@ -102,33 +101,36 @@ void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) {
 }
 
 // Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(Island* island) {
+void ContactSolver::initializeForIsland(uint islandIndex) {
 
     RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);
 
-    assert(island != nullptr);
+    assert(mIslands.bodyEntities[islandIndex].size() > 0);
-    assert(island->getNbBodies() > 0);
+    assert(mIslands.nbContactManifolds[islandIndex] > 0);
-    assert(island->getNbContactManifolds() > 0);
     assert(mSplitLinearVelocities != nullptr);
     assert(mSplitAngularVelocities != nullptr);
 
     // For each contact manifold of the island
-    ContactManifold** contactManifolds = island->getContactManifolds();
+    uint contactManifoldsIndex = mIslands.contactManifoldsIndices[islandIndex];
-    for (uint i=0; i<island->getNbContactManifolds(); i++) {
+    uint nbContactManifolds = mIslands.nbContactManifolds[islandIndex];
+    for (uint m=contactManifoldsIndex; m < contactManifoldsIndex + nbContactManifolds; m++) {
 
-        ContactManifold* externalManifold = contactManifolds[i];
+        ContactManifold& externalManifold = (*mAllContactManifolds)[m];
 
-        assert(externalManifold->getNbContactPoints() > 0);
+        assert(externalManifold.getNbContactPoints() > 0);
 
         // Get the two bodies of the contact
-        RigidBody* body1 = static_cast<RigidBody*>(externalManifold->getBody1());
+        RigidBody* body1 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity1));
-        RigidBody* body2 = static_cast<RigidBody*>(externalManifold->getBody2());
+        RigidBody* body2 = static_cast<RigidBody*>(mBodyComponents.getBody(externalManifold.bodyEntity2));
         assert(body1 != nullptr);
         assert(body2 != nullptr);
+        assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity1));
+        assert(!mBodyComponents.getIsEntityDisabled(externalManifold.bodyEntity2));
 
         // Get the two contact shapes
-        const ProxyShape* shape1 = externalManifold->getShape1();
+        // TODO : Do we really need to get the proxy-shape here
-        const ProxyShape* shape2 = externalManifold->getShape2();
+        const ProxyShape* shape1 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity1);
+        const ProxyShape* shape2 = mProxyShapeComponents.getProxyShape(externalManifold.proxyShapeEntity2);
 
         // Get the position of the two bodies
         const Vector3& x1 = body1->mCenterOfMassWorld;
@@ -143,10 +145,10 @@ void ContactSolver::initializeForIsland(Island* island) {
         mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
         mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
         mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
-        mContactConstraints[mNbContactManifolds].nbContacts = externalManifold->getNbContactPoints();
+        mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints();
         mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
         mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);
-        mContactConstraints[mNbContactManifolds].externalContactManifold = externalManifold;
+        mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold;
         mContactConstraints[mNbContactManifolds].normal.setToZero();
         mContactConstraints[mNbContactManifolds].frictionPointBody1.setToZero();
         mContactConstraints[mNbContactManifolds].frictionPointBody2.setToZero();
@@ -158,27 +160,30 @@ void ContactSolver::initializeForIsland(Island* island) {
         const Vector3& w2 = mAngularVelocities[mContactConstraints[mNbContactManifolds].indexBody2];
 
         // For each  contact point of the contact manifold
-        ContactPoint* externalContact = externalManifold->getContactPoints();
+        assert(externalManifold.getNbContactPoints() > 0);
-        assert(externalContact != nullptr);
+        uint contactPointsStartIndex = externalManifold.mContactPointsIndex;
-        while (externalContact != nullptr) {
+        uint nbContactPoints = externalManifold.mNbContactPoints;
+        for (uint c=contactPointsStartIndex; c < contactPointsStartIndex + nbContactPoints; c++) {
+
+            ContactPoint& externalContact = (*mAllContactPoints)[c];
 
             // Get the contact point on the two bodies
-            Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact->getLocalPointOnShape1();
+            Vector3 p1 = shape1->getLocalToWorldTransform() * externalContact.getLocalPointOnShape1();
-            Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact->getLocalPointOnShape2();
+            Vector3 p2 = shape2->getLocalToWorldTransform() * externalContact.getLocalPointOnShape2();
 
             new (mContactPoints + mNbContactPoints) ContactPointSolver();
-            mContactPoints[mNbContactPoints].externalContact = externalContact;
+            mContactPoints[mNbContactPoints].externalContact = &externalContact;
-            mContactPoints[mNbContactPoints].normal = externalContact->getNormal();
+            mContactPoints[mNbContactPoints].normal = externalContact.getNormal();
             mContactPoints[mNbContactPoints].r1.x = p1.x - x1.x;
             mContactPoints[mNbContactPoints].r1.y = p1.y - x1.y;
             mContactPoints[mNbContactPoints].r1.z = p1.z - x1.z;
             mContactPoints[mNbContactPoints].r2.x = p2.x - x2.x;
             mContactPoints[mNbContactPoints].r2.y = p2.y - x2.y;
             mContactPoints[mNbContactPoints].r2.z = p2.z - x2.z;
-            mContactPoints[mNbContactPoints].penetrationDepth = externalContact->getPenetrationDepth();
+            mContactPoints[mNbContactPoints].penetrationDepth = externalContact.getPenetrationDepth();
-            mContactPoints[mNbContactPoints].isRestingContact = externalContact->getIsRestingContact();
+            mContactPoints[mNbContactPoints].isRestingContact = externalContact.getIsRestingContact();
-            externalContact->setIsRestingContact(true);
+            externalContact.setIsRestingContact(true);
-            mContactPoints[mNbContactPoints].penetrationImpulse = externalContact->getPenetrationImpulse();
+            mContactPoints[mNbContactPoints].penetrationImpulse = externalContact.getPenetrationImpulse();
             mContactPoints[mNbContactPoints].penetrationSplitImpulse = 0.0;
 
             mContactConstraints[mNbContactManifolds].frictionPointBody1.x += p1.x;
@@ -240,8 +245,6 @@ void ContactSolver::initializeForIsland(Island* island) {
             mContactConstraints[mNbContactManifolds].normal.z += mContactPoints[mNbContactPoints].normal.z;
 
             mNbContactPoints++;
-
-            externalContact = externalContact->getNext();
         }
 
         mContactConstraints[mNbContactManifolds].frictionPointBody1 /=static_cast<decimal>(mContactConstraints[mNbContactManifolds].nbContacts);
@@ -252,13 +255,13 @@ void ContactSolver::initializeForIsland(Island* island) {
         mContactConstraints[mNbContactManifolds].r2Friction.x = mContactConstraints[mNbContactManifolds].frictionPointBody2.x - x2.x;
         mContactConstraints[mNbContactManifolds].r2Friction.y = mContactConstraints[mNbContactManifolds].frictionPointBody2.y - x2.y;
         mContactConstraints[mNbContactManifolds].r2Friction.z = mContactConstraints[mNbContactManifolds].frictionPointBody2.z - x2.z;
-        mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold->getFrictionVector1();
+        mContactConstraints[mNbContactManifolds].oldFrictionVector1 = externalManifold.getFrictionVector1();
-        mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold->getFrictionVector2();
+        mContactConstraints[mNbContactManifolds].oldFrictionVector2 = externalManifold.getFrictionVector2();
 
         // Initialize the accumulated impulses with the previous step accumulated impulses
-        mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold->getFrictionImpulse1();
+        mContactConstraints[mNbContactManifolds].friction1Impulse = externalManifold.getFrictionImpulse1();
-        mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold->getFrictionImpulse2();
+        mContactConstraints[mNbContactManifolds].friction2Impulse = externalManifold.getFrictionImpulse2();
-        mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold->getFrictionTwistImpulse();
+        mContactConstraints[mNbContactManifolds].frictionTwistImpulse = externalManifold.getFrictionTwistImpulse();
 
         // Compute the inverse K matrix for the rolling resistance constraint
         bool isBody1DynamicType = body1->getType() == BodyType::DYNAMIC;

src/engine/ContactSolver.h

@@ -30,6 +30,7 @@
 #include "configuration.h"
 #include "mathematics/Vector3.h"
 #include "mathematics/Matrix3x3.h"
+#include "engine/Islands.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -42,6 +43,8 @@ class MemoryManager;
 class Profiler;
 class Island;
 class RigidBody;
+class BodyComponents;
+class ProxyShapeComponents;
 
 // Class Contact Solver
 /**
@@ -280,18 +283,22 @@ class ContactSolver {
         MemoryManager& mMemoryManager;
 
         /// Split linear velocities for the position contact solver (split impulse)
+        // TODO : Use List<> here
         Vector3* mSplitLinearVelocities;
 
         /// Split angular velocities for the position contact solver (split impulse)
+        // TODO : Use List<> here
         Vector3* mSplitAngularVelocities;
 
         /// Current time step
         decimal mTimeStep;
 
         /// Contact constraints
+        // TODO : Use List<> here
         ContactManifoldSolver* mContactConstraints;
 
         /// Contact points
+        // TODO : Use List<> here
         ContactPointSolver* mContactPoints;
 
         /// Number of contact point constraints
@@ -301,11 +308,29 @@ class ContactSolver {
         uint mNbContactManifolds;
 
         /// Array of linear velocities
+        // TODO : Use List<> here
         Vector3* mLinearVelocities;
 
         /// Array of angular velocities
+        // TODO : Use List<> here
         Vector3* mAngularVelocities;
 
+        /// Reference to the islands
+        Islands& mIslands;
+
+        /// Pointer to the list of contact manifolds from narrow-phase
+        List<ContactManifold>* mAllContactManifolds;
+
+        /// Pointer to the list of contact points from narrow-phase
+        List<ContactPoint>* mAllContactPoints;
+
+        /// Reference to the body components
+        BodyComponents& mBodyComponents;
+
+        /// Reference to the proxy-shapes components
+        // TODO : Do we really need to use this ?
+        ProxyShapeComponents& mProxyShapeComponents;
+
         /// True if the split impulse position correction is active
         bool mIsSplitImpulseActive;
 
@@ -346,16 +371,17 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(MemoryManager& memoryManager, const WorldSettings& worldSettings);
+        ContactSolver(MemoryManager& memoryManager, Islands& islands, BodyComponents& bodyComponents,
+                      ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings);
 
         /// Destructor
         ~ContactSolver() = default;
 
         /// Initialize the contact constraints
-        void init(Island** islands, uint nbIslands, decimal timeStep);
+        void init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep);
 
         /// Initialize the constraint solver for a given island
-        void initializeForIsland(Island* island);
+        void initializeForIsland(uint islandIndex);
 
         /// Set the split velocities arrays
         void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,

src/engine/DynamicsWorld.cpp

@@ -47,10 +47,11 @@ using namespace std;
  * @param logger Pointer to the logger
  * @param profiler Pointer to the profiler
  */
-DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings,
+DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
-                             Logger* logger, Profiler* profiler)
               : CollisionWorld(worldSettings, logger, profiler),
-                mContactSolver(mMemoryManager, mConfig),
+                mIslands(mMemoryManager.getSingleFrameAllocator()),
+                mContactSolver(mMemoryManager, mIslands, mBodyComponents, mProxyShapesComponents, mConfig),
+                mConstraintSolver(mIslands),
                 mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
                 mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), 
                 mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
@@ -58,12 +59,11 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
                 mIsGravityEnabled(true), mConstrainedLinearVelocities(nullptr),
                 mConstrainedAngularVelocities(nullptr), mSplitLinearVelocities(nullptr),
                 mSplitAngularVelocities(nullptr), mConstrainedPositions(nullptr),
-                mConstrainedOrientations(nullptr), mNbIslands(0), mIslands(nullptr),
+                mConstrainedOrientations(nullptr),
                 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,32 +128,9 @@ void DynamicsWorld::update(decimal timeStep) {
     // Compute the collision detection
     mCollisionDetection.computeCollisionDetection();
 
-    // 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();
 
@@ -181,7 +158,7 @@ void DynamicsWorld::update(decimal timeStep) {
     resetBodiesForceAndTorque();
 
     // Reset the islands
-    mIslands2.clear();
+    mIslands.clear();
 
     // Reset the single frame memory allocator
     mMemoryManager.resetFrameAllocator();
@@ -194,19 +171,25 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
 
     RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
     
-    // For each island of the world
+    // TODO : We should loop over non-sleeping dynamic components here and not over islands
-    for (uint i=0; i < mNbIslands; i++) {
 
-        RigidBody** bodies = mIslands[i]->getBodies();
+    // For each island of the world
+    for (uint i=0; i < mIslands.getNbIslands(); i++) {
 
         // For each body of the island
-        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+        for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
+
+            const Entity bodyEntity = mIslands.bodyEntities[i][b];
+            RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
 
             // Get the constrained velocity
-            uint indexArray = bodies[b]->mArrayIndex;
+            uint indexArray = body->mArrayIndex;
             Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
             Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
 
+            // TODO : Remove this
+            Vector3 testLinVel = newLinVelocity;
+
             // Add the split impulse velocity from Contact Solver (only used
             // to update the position)
             if (mContactSolver.isSplitImpulseActive()) {
@@ -216,8 +199,8 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
             }
 
             // Get current position and orientation of the body
-            const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
+            const Vector3& currentPosition = body->mCenterOfMassWorld;
-            const Quaternion& currentOrientation = mTransformComponents.getTransform(bodies[b]->getEntity()).getOrientation();
+            const Quaternion& currentOrientation = mTransformComponents.getTransform(body->getEntity()).getOrientation();
 
             // Update the new constrained position and orientation of the body
             mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * mTimeStep;
@@ -235,31 +218,37 @@ void DynamicsWorld::updateBodiesState() {
 
     // TODO : Make sure we compute this in a system
 
+    // TODO : We should loop over non-sleeping dynamic components here and not over islands
+
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
         // For each body of the island
-        RigidBody** bodies = mIslands[islandIndex]->getBodies();
+        for (uint b=0; b < mIslands.bodyEntities[islandIndex].size(); b++) {
 
-        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
+            Entity bodyEntity = mIslands.bodyEntities[islandIndex][b];
+            RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
 
-            uint index = bodies[b]->mArrayIndex;
+            uint index = body->mArrayIndex;
 
             // Update the linear and angular velocity of the body
-            mDynamicsComponents.setLinearVelocity(bodies[b]->getEntity(), mConstrainedLinearVelocities[index]);
+            mDynamicsComponents.setLinearVelocity(bodyEntity, mConstrainedLinearVelocities[index]);
-            mDynamicsComponents.setAngularVelocity(bodies[b]->getEntity(), mConstrainedAngularVelocities[index]);
+            mDynamicsComponents.setAngularVelocity(bodyEntity, mConstrainedAngularVelocities[index]);
 
             // Update the position of the center of mass of the body
-            bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
+            body->mCenterOfMassWorld = mConstrainedPositions[index];
 
             // Update the orientation of the body
-            mTransformComponents.getTransform(bodies[b]->getEntity()).setOrientation(mConstrainedOrientations[index].getUnit());
+            mTransformComponents.getTransform(bodyEntity).setOrientation(mConstrainedOrientations[index].getUnit());
+
+            // TODO : REMOVE THIS
+            assert(mConstrainedOrientations[index].lengthSquare() < 1.5 * 1.5);
 
             // Update the transform of the body (using the new center of mass and new orientation)
-            bodies[b]->updateTransformWithCenterOfMass();
+            body->updateTransformWithCenterOfMass();
 
             // Update the world inverse inertia tensor of the body
-            bodies[b]->updateInertiaTensorInverseWorld();
+            body->updateInertiaTensorInverseWorld();
         }
     }
 
@@ -275,6 +264,8 @@ void DynamicsWorld::initVelocityArrays() {
     // Allocate memory for the bodies velocity arrays
     uint nbBodies = mRigidBodies.size();
 
+    assert(mDynamicsComponents.getNbComponents() == nbBodies);
+
     mSplitLinearVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
                                                                            nbBodies * sizeof(Vector3)));
     mSplitAngularVelocities = static_cast<Vector3*>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
@@ -317,33 +308,37 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
     // Initialize the bodies velocity arrays
     initVelocityArrays();
 
-    // For each island of the world
+    // TODO : We should loop over non-sleeping dynamic components here and not over islands
-    for (uint i=0; i < mNbIslands; i++) {
 
-        RigidBody** bodies = mIslands[i]->getBodies();
+    // For each island of the world
+    for (uint i=0; i < mIslands.getNbIslands(); i++) {
 
         // For each body of the island
-        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+        for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
 
-            // Insert the body into the map of constrained velocities
+            const Entity bodyEntity = mIslands.bodyEntities[i][b];
-            uint indexBody = bodies[b]->mArrayIndex;
+
+            RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
+
+            const uint indexBody = body->mArrayIndex;
 
             assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
             assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
+            assert(indexBody < mRigidBodies.size());
 
             // Integrate the external force to get the new velocity of the body
-            mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
+            mConstrainedLinearVelocities[indexBody] = body->getLinearVelocity() +
-                                        mTimeStep * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
+                                        mTimeStep * body->mMassInverse * body->mExternalForce;
-            mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
+            mConstrainedAngularVelocities[indexBody] = body->getAngularVelocity() +
-                                        mTimeStep * bodies[b]->getInertiaTensorInverseWorld() *
+                                        mTimeStep * body->getInertiaTensorInverseWorld() *
-                                        bodies[b]->mExternalTorque;
+                                        body->mExternalTorque;
 
             // If the gravity has to be applied to this rigid body
-            if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
+            if (body->isGravityEnabled() && mIsGravityEnabled) {
 
                 // Integrate the gravity force
-                mConstrainedLinearVelocities[indexBody] += mTimeStep * bodies[b]->mMassInverse *
+                mConstrainedLinearVelocities[indexBody] += mTimeStep * body->mMassInverse *
-                        bodies[b]->getMass() * mGravity;
+                        body->getMass() * mGravity;
             }
 
             // Apply the velocity damping
@@ -359,14 +354,12 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
             // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
             //                              => e^(-x) ~ 1 - x
             //                 => v2 = v1 * (1 - c * dt)
-            decimal linDampingFactor = bodies[b]->getLinearDamping();
+            decimal linDampingFactor = body->getLinearDamping();
-            decimal angDampingFactor = bodies[b]->getAngularDamping();
+            decimal angDampingFactor = body->getAngularDamping();
             decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
             decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
             mConstrainedLinearVelocities[indexBody] *= linearDamping;
             mConstrainedAngularVelocities[indexBody] *= angularDamping;
-
-            indexBody++;
         }
     }
 }
@@ -388,28 +381,28 @@ void DynamicsWorld::solveContactsAndConstraints() {
     // ---------- Solve velocity constraints for joints and contacts ---------- //
 
     // Initialize the contact solver
-    mContactSolver.init(mIslands, mNbIslands, mTimeStep);
+    mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep);
 
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
         // If there are constraints to solve
-        if (mIslands[islandIndex]->getNbJoints() > 0) {
+        if (mIslands.joints[islandIndex].size() > 0) {
 
             // Initialize the constraint solver
-            mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]);
+            mConstraintSolver.initializeForIsland(mTimeStep, islandIndex);
         }
     }
 
     // For each iteration of the velocity solver
     for (uint i=0; i<mNbVelocitySolverIterations; i++) {
 
-        for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+        for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
             // Solve the constraints
-            if (mIslands[islandIndex]->getNbJoints() > 0) {
+            if (mIslands.joints[islandIndex].size() > 0) {
 
-                mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+                mConstraintSolver.solveVelocityConstraints(islandIndex);
             }
         }
 
@@ -428,17 +421,17 @@ void DynamicsWorld::solvePositionCorrection() {
     if (mJoints.size() == 0) return;
 
     // For each island of the world
-    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
+    for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
 
         // ---------- Solve the position error correction for the constraints ---------- //
 
-        if (mIslands[islandIndex]->getNbJoints() > 0) {
+        if (mIslands.joints[islandIndex].size() > 0) {
 
             // For each iteration of the position (error correction) solver
             for (uint i=0; i<mNbPositionSolverIterations; i++) {
 
                 // Solve the position constraints
-                mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
+                mConstraintSolver.solvePositionConstraints(islandIndex);
             }
         }
     }
@@ -704,163 +697,6 @@ 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
-/// 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::computeIslands() {
-
-    RP3D_PROFILE("DynamicsWorld::computeIslands()", mProfiler);
-
-    uint nbBodies = mRigidBodies.size();
-
-    // Allocate and create the array of islands pointer. This memory is allocated
-    // in the single frame allocator
-    mIslands = static_cast<Island**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                             sizeof(Island*) * nbBodies));
-    mNbIslands = 0;
-
-    int nbContactManifolds = 0;
-
-    // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
-    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-        int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
-        nbContactManifolds += nbBodyManifolds;
-    }
-    for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
-        (*it)->mIsAlreadyInIsland = false;
-    }
-
-    // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search
-    size_t nbBytesStack = sizeof(RigidBody*) * nbBodies;
-    RigidBody** stackBodiesToVisit = static_cast<RigidBody**>(mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                                                      nbBytesStack));
-
-    // For each rigid body of the world
-    for (List<RigidBody*>::Iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-
-        RigidBody* body = *it;
-
-        // If the body has already been added to an island, we go to the next body
-        if (body->mIsAlreadyInIsland) continue;
-
-        // If the body is static, we go to the next body
-        if (body->getType() == BodyType::STATIC) continue;
-
-        // If the body is sleeping or inactive, we go to the next body
-        if (body->isSleeping() || !body->isActive()) continue;
-
-        // Reset the stack of bodies to visit
-        uint stackIndex = 0;
-        stackBodiesToVisit[stackIndex] = body;
-        stackIndex++;
-        body->mIsAlreadyInIsland = true;
-
-        // Create the new island
-        void* allocatedMemoryIsland = mMemoryManager.allocate(MemoryManager::AllocationType::Frame,
-                                                              sizeof(Island));
-        mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, mJoints.size(),
-                                                                  mMemoryManager);
-
-        // While there are still some bodies to visit in the stack
-        while (stackIndex > 0) {
-
-            // Get the next body to visit from the stack
-            stackIndex--;
-            RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex];
-            assert(bodyToVisit->isActive());
-
-            // Awake the body if it is sleeping
-            bodyToVisit->setIsSleeping(false);
-
-            // Add the body into the island
-            mIslands[mNbIslands]->addBody(bodyToVisit);
-
-            // If the current body is static, we do not want to perform the DFS
-            // search across that body
-            if (bodyToVisit->getType() == BodyType::STATIC) continue;
-
-            // For each contact manifold in which the current body is involded
-            ContactManifoldListElement* contactElement;
-            for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != nullptr;
-                 contactElement = contactElement->getNext()) {
-
-                ContactManifold* contactManifold = contactElement->getContactManifold();
-
-                assert(contactManifold->getNbContactPoints() > 0);
-
-                // Check if the current contact manifold has already been added into an island
-                if (contactManifold->isAlreadyInIsland()) continue;
-
-                // Get the other body of the contact manifold
-                RigidBody* body1 = dynamic_cast<RigidBody*>(contactManifold->getBody1());
-                RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
-
-                // If the colliding body is a RigidBody (and not a CollisionBody instead)
-                if (body1 != nullptr && body2 != nullptr) {
-
-                    // Add the contact manifold into the island
-                    mIslands[mNbIslands]->addContactManifold(contactManifold);
-                    contactManifold->mIsAlreadyInIsland = true;
-
-                    RigidBody* otherBody = (body1->getId() == bodyToVisit->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
-                    stackBodiesToVisit[stackIndex] = otherBody;
-                    stackIndex++;
-                    otherBody->mIsAlreadyInIsland = true;
-                }
-            }
-
-            // For each joint in which the current body is involved
-            JointListElement* jointElement;
-            for (jointElement = bodyToVisit->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
-                mIslands[mNbIslands]->addJoint(joint);
-                joint->mIsAlreadyInIsland = true;
-
-                // 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() == bodyToVisit->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
-                stackBodiesToVisit[stackIndex] = otherBody;
-                stackIndex++;
-                otherBody->mIsAlreadyInIsland = true;
-            }
-        }
-
-        // Reset the isAlreadyIsland variable of the static bodies so that they
-        // can also be included in the other islands
-        for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) {
-
-            if (mIslands[mNbIslands]->mBodies[i]->getType() == BodyType::STATIC) {
-                mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
-            }
-        }
-
-        mNbIslands++;
-     }
-}
-
 // 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
@@ -878,10 +714,11 @@ void DynamicsWorld::createIslands() {
     // list of contact pairs involving a non-rigid body
     List<uint> nonRigidBodiesContactPairs(mMemoryManager.getSingleFrameAllocator());
 
-    RP3D_PROFILE("DynamicsWorld::createIslandsAndContacts()", mProfiler);
+    RP3D_PROFILE("DynamicsWorld::createIslands()", mProfiler);
 
     // Reset all the isAlreadyInIsland variables of bodies and joints
-    for (uint b=0; b < mDynamicsComponents.getNbEnabledComponents(); b++) {
+    for (uint b=0; b < mDynamicsComponents.getNbComponents(); b++) {
+
         mDynamicsComponents.mIsAlreadyInIsland[b] = false;
     }
     for (List<Joint*>::Iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
@@ -914,7 +751,7 @@ void DynamicsWorld::createIslands() {
         bodyEntityIndicesToVisit.push(mDynamicsComponents.mBodies[b]);
 
         // Create the new island
-        uint32 islandIndex = mIslands2.addIsland(nbTotalManifolds);
+        uint32 islandIndex = mIslands.addIsland(nbTotalManifolds);
 
         // While there are still some bodies to visit in the stack
         while (bodyEntityIndicesToVisit.size() > 0) {
@@ -922,16 +759,17 @@ void DynamicsWorld::createIslands() {
             // 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);
+            mIslands.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));
+
+            // Awake the body if it is sleeping
+            rigidBodyToVisit->setIsSleeping(false);
+
             if (rigidBodyToVisit->getType() == BodyType::STATIC) continue;
 
             // If the body is involved in contacts with other bodies
@@ -962,7 +800,7 @@ void DynamicsWorld::createIslands() {
                         mCollisionDetection.mContactPairsIndicesOrderingForContacts.add(pair.contactPairIndex);
 
                         // Add the contact manifold into the island
-                        mIslands2.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
+                        mIslands.nbContactManifolds[islandIndex] += pair.potentialContactManifoldsIndices.size();
                         pair.isAlreadyInIsland = true;
 
                         const Entity otherBodyEntity = pair.body1Entity == bodyToVisitEntity ? pair.body2Entity : pair.body1Entity;
@@ -978,6 +816,7 @@ void DynamicsWorld::createIslands() {
 
                         // Add the contact pair index in the list of contact pairs that won't be part of islands
                         nonRigidBodiesContactPairs.add(pair.contactPairIndex);
+                        pair.isAlreadyInIsland = true;
                     }
                 }
             }
@@ -993,37 +832,31 @@ void DynamicsWorld::createIslands() {
                 if (joint->isAlreadyInIsland()) continue;
 
                 // Add the joint into the island
-                mIslands2.joints.add(joint);
+                mIslands.joints[islandIndex].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;
+                if (mDynamicsComponents.getIsAlreadyInIsland(otherBodyEntity)) continue;
 
                 // Insert the other body into the stack of bodies to visit
                 bodyEntityIndicesToVisit.push(otherBodyEntity);
-                otherBody->mIsAlreadyInIsland = true;
+                mDynamicsComponents.setIsAlreadyInIsland(otherBodyEntity, 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++) {
+        for (uint j=0; j < mDynamicsComponents.getNbEnabledComponents(); j++) {
 
             // 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]));
+            CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(mDynamicsComponents.mBodies[j]));
             if (body->getType() == BodyType::STATIC) {
-                mDynamicsComponents.mIsAlreadyInIsland[b] = false;
+                mDynamicsComponents.mIsAlreadyInIsland[j] = false;
             }
         }
     }
@@ -1031,6 +864,8 @@ void DynamicsWorld::createIslands() {
     // 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);
 
+    assert(mCollisionDetection.mCurrentContactPairs->size() == mCollisionDetection.mContactPairsIndicesOrderingForContacts.size());
+
     mCollisionDetection.mMapBodyToContactPairs.clear(true);
 }
 
@@ -1045,32 +880,36 @@ void DynamicsWorld::updateSleepingBodies() {
     const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
 
     // For each island of the world
-    for (uint i=0; i<mNbIslands; i++) {
+    for (uint i=0; i<mIslands.getNbIslands(); i++) {
 
         decimal minSleepTime = DECIMAL_LARGEST;
 
         // For each body of the island
-        RigidBody** bodies = mIslands[i]->getBodies();
+        for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
-        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+
+            const Entity bodyEntity = mIslands.bodyEntities[i][b];
+
+            // TODO : We should not have to do this cast here to get type of body
+            CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(bodyEntity));
 
             // Skip static bodies
-            if (bodies[b]->getType() == BodyType::STATIC) continue;
+            if (body->getType() == BodyType::STATIC) continue;
 
             // If the body is velocity is large enough to stay awake
-            if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||
+            if (mDynamicsComponents.getLinearVelocity(bodyEntity).lengthSquare() > sleepLinearVelocitySquare ||
-                bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare ||
+                mDynamicsComponents.getAngularVelocity(bodyEntity).lengthSquare() > sleepAngularVelocitySquare ||
-                !bodies[b]->isAllowedToSleep()) {
+                !body->isAllowedToSleep()) {
 
                 // Reset the sleep time of the body
-                bodies[b]->mSleepTime = decimal(0.0);
+                body->mSleepTime = decimal(0.0);
                 minSleepTime = decimal(0.0);
             }
-            else {  // If the body velocity is bellow the sleeping velocity threshold
+            else {  // If the body velocity is below the sleeping velocity threshold
 
                 // Increase the sleep time
-                bodies[b]->mSleepTime += mTimeStep;
+                body->mSleepTime += mTimeStep;
-                if (bodies[b]->mSleepTime < minSleepTime) {
+                if (body->mSleepTime < minSleepTime) {
-                    minSleepTime = bodies[b]->mSleepTime;
+                    minSleepTime = body->mSleepTime;
                 }
             }
         }
@@ -1081,8 +920,11 @@ void DynamicsWorld::updateSleepingBodies() {
         if (minSleepTime >= mTimeBeforeSleep) {
 
             // Put all the bodies of the island to sleep
-            for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+            for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
-                bodies[b]->setIsSleeping(true);
+
+                const Entity bodyEntity = mIslands.bodyEntities[i][b];
+                CollisionBody* body = static_cast<CollisionBody*>(mBodyComponents.getBody(bodyEntity));
+                body->setIsSleeping(true);
             }
         }
     }

src/engine/DynamicsWorld.h

@@ -55,6 +55,9 @@ class DynamicsWorld : public CollisionWorld {
 
         // -------------------- Attributes -------------------- //
 
+        /// All the islands of bodies of the current frame
+        Islands mIslands;
+
         /// Contact solver
         ContactSolver mContactSolver;
 
@@ -85,17 +88,21 @@ class DynamicsWorld : public CollisionWorld {
         /// True if the gravity force is on
         bool mIsGravityEnabled;
 
+        // TODO : Move this into dynamic components
         /// Array of constrained linear velocities (state of the linear velocities
         /// after solving the constraints)
         Vector3* mConstrainedLinearVelocities;
 
+        // TODO : Move this into dynamic components
         /// Array of constrained angular velocities (state of the angular velocities
         /// after solving the constraints)
         Vector3* mConstrainedAngularVelocities;
 
+        // TODO : Move this into dynamic components
         /// Split linear velocities for the position contact solver (split impulse)
         Vector3* mSplitLinearVelocities;
 
+        // TODO : Move this into dynamic components
         /// Split angular velocities for the position contact solver (split impulse)
         Vector3* mSplitAngularVelocities;
 
@@ -105,12 +112,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Array of constrained rigid bodies orientation (for position error correction)
         Quaternion* mConstrainedOrientations;
 
-        /// Number of islands in the world
-        uint mNbIslands;
-
-        /// Array with all the islands of awaken bodies
-        Island** mIslands;
-
         /// Sleep linear velocity threshold
         decimal mSleepLinearVelocity;
 
@@ -127,9 +128,6 @@ 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.

src/engine/Islands.h

@@ -0,0 +1,120 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 Daniel Chappuis                                       *
+*********************************************************************************
+*                                                                               *
+* This software is provided 'as-is', without any express or implied warranty.   *
+* In no event will the authors be held liable for any damages arising from the  *
+* use of this software.                                                         *
+*                                                                               *
+* Permission is granted to anyone to use this software for any purpose,         *
+* including commercial applications, and to alter it and redistribute it        *
+* freely, subject to the following restrictions:                                *
+*                                                                               *
+* 1. The origin of this software must not be misrepresented; you must not claim *
+*    that you wrote the original software. If you use this software in a        *
+*    product, an acknowledgment in the product documentation would be           *
+*    appreciated but is not required.                                           *
+*                                                                               *
+* 2. Altered source versions must be plainly marked as such, and must not be    *
+*    misrepresented as being the original software.                             *
+*                                                                               *
+* 3. This notice may not be removed or altered from any source distribution.    *
+*                                                                               *
+********************************************************************************/
+
+#ifndef REACTPHYSICS3D_ISLANDS_H
+#define REACTPHYSICS3D_ISLANDS_H
+
+// Libraries
+#include "configuration.h"
+#include "containers/List.h"
+#include "engine/Entity.h"
+#include "constraint/Joint.h"
+
+namespace reactphysics3d {
+
+// Declarations
+
+// Structure Islands
+/**
+ * This class contains all the islands of bodies during a frame.
+ * An island represent an isolated group of awake bodies that are connected with each other by
+ * some contraints (contacts or joints).
+ */
+struct Islands {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Reference to the memory allocator
+        MemoryAllocator& memoryAllocator;
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+
+        /// For each island, index of the first contact manifold of the island in the array of contact manifolds
+        List<uint> contactManifoldsIndices;
+
+        /// For each island, number of contact manifolds in the island
+        List<uint> nbContactManifolds;
+
+        /// For each island, list of all the entities of the bodies in the island
+        List<List<Entity>> bodyEntities;
+
+        // TODO : Use joints entities here and not pointers
+        /// For each island, list of the joints that are part of the island
+        List<List<Joint*>> joints;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Islands(MemoryAllocator& allocator)
+            :memoryAllocator(allocator), contactManifoldsIndices(allocator), nbContactManifolds(allocator), bodyEntities(allocator),
+             joints(allocator) {
+
+        }
+
+        /// Destructor
+        ~Islands() = default;
+
+        /// Assignment operator
+        Islands& operator=(const Islands& island) = default;
+
+        /// Copy-constructor
+        Islands(const Islands& island) = default;
+
+        /// Return the number of islands
+        uint32 getNbIslands() const {
+            return static_cast<uint32>(contactManifoldsIndices.size());
+        }
+
+        /// Add an island and return its index
+        uint32 addIsland(uint32 contactManifoldStartIndex) {
+
+            uint32 islandIndex = contactManifoldsIndices.size();
+
+            contactManifoldsIndices.add(contactManifoldStartIndex);
+            nbContactManifolds.add(0);
+            bodyEntities.add(List<Entity>(memoryAllocator));
+            joints.add(List<Joint*>(memoryAllocator));
+
+            return islandIndex;
+        }
+
+        /// Clear all the islands
+        void clear() {
+
+            contactManifoldsIndices.clear(true);
+            nbContactManifolds.clear(true);
+            bodyEntities.clear(true);
+            joints.clear(true);
+        }
+};
+
+}
+
+#endif

src/systems/BroadPhaseSystem.cpp

@@ -52,8 +52,8 @@ BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, Proxy
 }
 
 // Return true if the two broad-phase collision shapes are overlapping
-bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1,
+// TODO : Use proxy-shape entities in parameters
-                                                       const ProxyShape* shape2) const {
+bool BroadPhaseSystem::testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const {
 
     if (shape1->getBroadPhaseId() == -1 || shape2->getBroadPhaseId() == -1) return false;