Implement the islands computation

src/body/Body.cpp

@@ -23,7 +23,7 @@
 *                                                                               *
 ********************************************************************************/
 
- // Libraries
+// Libraries
 #include "Body.h"
 #include "../collision/shapes/CollisionShape.h"
 
@@ -31,7 +31,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-Body::Body(bodyindex id) : mID(id) {
+Body::Body(bodyindex id)
+     : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false) {
 
 }
 

src/body/Body.h

@@ -47,6 +47,15 @@ class Body {
         /// ID of the body
         bodyindex mID;
 
+        /// 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;
+
+        /// True if the body is sleeping (for sleeping technique)
+        bool mIsSleeping;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -68,6 +77,24 @@ class Body {
         /// Return the id of the body
         bodyindex getID() const;
 
+        /// Return true if the body has already been added in an island (for the sleeping technique)
+        bool isAlreadyInIsland() const;
+
+        /// Set the value of to know if the body has already been added into an island
+        void setIsAlreadyInIsland(bool isAlreadyInIsland);
+
+        /// Return whether or not the body is allowed to sleep
+        bool isAllowedToSleep() const;
+
+        /// Set whether or not the body is allowed to go to sleep
+        void setIsAllowedToSleep(bool isAllowedToSleep);
+
+        /// Return whether or not the body is sleeping
+        bool isSleeping() const;
+
+        /// Set the variable to know whether or not the body is sleeping
+        void setIsSleeping(bool isSleeping);
+
         /// Smaller than operator
         bool operator<(const Body& body2) const;
 
@@ -79,6 +106,10 @@ class Body {
 
         /// Not equal operator
         bool operator!=(const Body& body2) const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
 };
 
 // Return the id of the body
@@ -86,6 +117,36 @@ inline bodyindex Body::getID() const {
     return mID;
 }
 
+// Return true if the body has already been added in an island (for the sleeping technique)
+inline bool Body::isAlreadyInIsland() const {
+    return mIsAlreadyInIsland;
+}
+
+// Set the value of to know if the body has already been added into an island
+inline void Body::setIsAlreadyInIsland(bool isAlreadyInIsland) {
+    mIsAlreadyInIsland = isAlreadyInIsland;
+}
+
+// Return whether or not the body is allowed to sleep
+inline bool Body::isAllowedToSleep() const {
+    return mIsAllowedToSleep;
+}
+
+// Set whether or not the body is allowed to go to sleep
+inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) {
+    mIsAllowedToSleep = isAllowedToSleep;
+}
+
+// Return whether or not the body is sleeping
+inline bool Body::isSleeping() const {
+    return mIsSleeping;
+}
+
+// Set the variable to know whether or not the body is sleeping
+inline void Body::setIsSleeping(bool isSleeping) {
+    mIsSleeping = isSleeping;
+}
+
 // Smaller than operator
 inline bool Body::operator<(const Body& body2) const {
     return (mID < body2.mID);

src/body/CollisionBody.cpp

@@ -25,6 +25,7 @@
 
  // Libraries
 #include "CollisionBody.h"
+#include "../engine/ContactManifold.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -33,7 +34,7 @@ using namespace reactphysics3d;
 CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape,
                              bodyindex id)
     : Body(id), mCollisionShape(collisionShape), mTransform(transform),
-      mIsActive(true), mHasMoved(false) {
+      mIsActive(true), mHasMoved(false), mContactManifoldsList(NULL) {
 
     assert(collisionShape);
 
@@ -50,5 +51,24 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisi
 
 // Destructor
 CollisionBody::~CollisionBody() {
-
+    assert(mContactManifoldsList == NULL);
+}
+
+// Reset the contact manifold lists
+void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) {
+
+    // Delete the linked list of contact manifolds of that body
+    ContactManifoldListElement* currentElement = mContactManifoldsList;
+    while (currentElement != NULL) {
+        ContactManifoldListElement* nextElement = currentElement->next;
+
+        // Delete the current element
+        currentElement->ContactManifoldListElement::~ContactManifoldListElement();
+        memoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
+
+        currentElement = nextElement;
+    }
+    mContactManifoldsList = NULL;
+
+    assert(mContactManifoldsList == NULL);
 }

src/body/CollisionBody.h

@@ -33,11 +33,15 @@
 #include "../mathematics/Transform.h"
 #include "../collision/shapes/AABB.h"
 #include "../collision/shapes/CollisionShape.h"
+#include "../memory/MemoryAllocator.h"
 #include "../configuration.h"
 
 /// Namespace reactphysics3d
 namespace reactphysics3d {
 
+// Class declarations
+struct ContactManifoldListElement;
+
 // Class CollisionBody
 /**
  * This class represents a body that is able to collide with others
@@ -76,6 +80,9 @@ class CollisionBody : public Body {
         /// True if the body has moved during the last frame
         bool mHasMoved;
 
+        /// First element of the linked list of contact manifolds involving this body
+        ContactManifoldListElement* mContactManifoldsList;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -84,6 +91,9 @@ class CollisionBody : public Body {
         /// Private assignment operator
         CollisionBody& operator=(const CollisionBody& body);
 
+        /// Reset the contact manifold lists
+        void resetContactManifoldsList(MemoryAllocator& memoryAllocator);
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -144,6 +154,13 @@ class CollisionBody : public Body {
 
         /// Update the Axis-Aligned Bounding Box coordinates
         void updateAABB();
+
+        /// Return the first element of the linked list of contact manifolds involving this body
+        const ContactManifoldListElement* getContactManifoldsLists() const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
 };
 
 // Return true if the body has moved during the last frame
@@ -243,6 +260,11 @@ inline void CollisionBody::updateAABB() {
     mCollisionShape->updateAABB(mAabb, mTransform);
 }
 
+// Return the first element of the linked list of contact manifolds involving this body
+inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const {
+    return mContactManifoldsList;
+}
+
 }
 
  #endif

src/body/RigidBody.cpp

@@ -25,6 +25,7 @@
 
 // Libraries
 #include "RigidBody.h"
+#include "constraint/Constraint.h"
 #include "../collision/shapes/CollisionShape.h"
 
 // We want to use the ReactPhysics3D namespace
@@ -36,13 +37,41 @@ RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3&
            : CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal),
              mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()),
              mMassInverse(decimal(1.0) / mass), mIsGravityEnabled(true),
-             mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)) {
+             mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) {
 
     assert(collisionShape);
 }
 
 // Destructor
 RigidBody::~RigidBody() {
-
+    assert(mJointsList == NULL);
 }
 
+// Remove a joint from the joints list
+void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Constraint* joint) {
+
+    assert(joint != NULL);
+    assert(mJointsList != NULL);
+
+    // Remove the joint from the linked list of the joints of the first body
+    if (mJointsList->joint == joint) {   // If the first element is the one to remove
+        JointListElement* elementToRemove = mJointsList;
+        mJointsList = elementToRemove->next;
+        elementToRemove->JointListElement::~JointListElement();
+        memoryAllocator.release(elementToRemove, sizeof(JointListElement));
+    }
+    else {  // If the element to remove is not the first one in the list
+        JointListElement* currentElement = mJointsList;
+        while (currentElement->next != NULL) {
+            if (currentElement->next->joint == joint) {
+                JointListElement* elementToRemove = currentElement->next;
+                currentElement->next = elementToRemove->next;
+                elementToRemove->JointListElement::~JointListElement();
+                memoryAllocator.release(elementToRemove, sizeof(JointListElement));
+                break;
+            }
+        }
+    }
+}
+
+

src/body/RigidBody.h

@@ -31,10 +31,15 @@
 #include "CollisionBody.h"
 #include "../engine/Material.h"
 #include "../mathematics/mathematics.h"
+#include "../memory/MemoryAllocator.h"
 
 /// Namespace reactphysics3d
 namespace reactphysics3d {
 
+// Class declarations
+struct JointListElement;
+class Constraint;
+
 // Class RigidBody
 /**
  * This class represents a rigid body of the physics
@@ -86,6 +91,9 @@ class RigidBody : public CollisionBody {
         /// Angular velocity damping factor
         decimal mAngularDamping;
 
+        /// First element of the linked list of joints involving this body
+        JointListElement* mJointsList;        
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -94,6 +102,9 @@ class RigidBody : public CollisionBody {
         /// Private assignment operator
         RigidBody& operator=(const RigidBody& body);
 
+        /// Remove a joint from the joints list
+        void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Constraint* joint);
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -179,6 +190,13 @@ class RigidBody : public CollisionBody {
 
         /// Set the angular damping factor
         void setAngularDamping(decimal angularDamping);
+
+        /// Return the first element of the linked list of joints involving this body
+        const JointListElement* getJointsList() const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
 };
 
 // Method that return the mass of the body
@@ -328,6 +346,11 @@ inline void RigidBody::setAngularDamping(decimal angularDamping) {
     mAngularDamping = angularDamping;
 }
 
+// Return the first element of the linked list of joints involving this body
+inline const JointListElement* RigidBody::getJointsList() const {
+    return mJointsList;
+}
+
 }
 
  #endif

src/configuration.h

@@ -92,8 +92,8 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
 /// Default bounciness factor for a rigid body
 const decimal DEFAULT_BOUNCINESS = decimal(0.5);
 
-/// True if the deactivation (sleeping) of inactive bodies is enabled
+/// True if the spleeping technique for inactive bodies is enabled
-const bool DEACTIVATION_ENABLED = true;
+const bool SPLEEPING_ENABLED = true;
 
 /// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
 const decimal OBJECT_MARGIN = decimal(0.04);

src/constraint/Constraint.cpp

@@ -33,7 +33,7 @@ Constraint::Constraint(const ConstraintInfo& constraintInfo)
            :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true),
             mType(constraintInfo.type),
             mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique),
-            mIsCollisionEnabled(constraintInfo.isCollisionEnabled){
+            mIsCollisionEnabled(constraintInfo.isCollisionEnabled), mIsAlreadyInIsland(false) {
 
     assert(mBody1 != NULL);
     assert(mBody2 != NULL);

src/constraint/Constraint.h

@@ -39,6 +39,32 @@ enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOI
 
 // Class declarations
 struct ConstraintSolverData;
+class Constraint;
+
+// Structure JointListElement
+/**
+ * This structure represents a single element of a linked list of joints
+ */
+struct JointListElement {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the actual joint
+        Constraint* joint;
+
+        /// Next element of the list
+        JointListElement* next;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        JointListElement(Constraint* initJoint, JointListElement* initNext)
+                        :joint(initJoint), next(initNext){
+
+        }
+};
 
 // Structure ConstraintInfo
 /**
@@ -87,9 +113,8 @@ struct ConstraintInfo {
 // Class Constraint
 /**
  * This abstract class represents a constraint in the physics engine.
- * A constraint can be a collision contact or a joint for
+ * A constraint can be a collision contact point or a joint for
- * instance. Each constraint can be made of several "mathematical
+ * instance.
- * constraints" needed to represent the main constraint.
  */
 class Constraint {
 
@@ -121,6 +146,9 @@ class Constraint {
         /// True if the two bodies of the constraint are allowed to collide with each other
         bool mIsCollisionEnabled;
 
+        /// True if the joint has already been added into an island
+        bool mIsAlreadyInIsland;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -154,6 +182,9 @@ class Constraint {
         /// Return true if the collision between the two bodies of the constraint is enabled
         bool isCollisionEnabled() const;
 
+        /// Return true if the joint has already been added into an island
+        bool isAlreadyInIsland() const;
+
         /// Return the number of bytes used by the constraint
         virtual size_t getSizeInBytes() const = 0;
 
@@ -168,6 +199,11 @@ class Constraint {
 
         /// Solve the position constraint
         virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+        friend class Island;
 };
 
 // Return the reference to the body 1
@@ -195,6 +231,11 @@ inline bool Constraint::isCollisionEnabled() const {
     return mIsCollisionEnabled;
 }
 
+// Return true if the joint has already been added into an island
+inline bool Constraint::isAlreadyInIsland() const {
+    return mIsAlreadyInIsland;
+}
+
 }
 
 #endif

src/engine/ContactManifold.cpp

@@ -30,10 +30,10 @@
 using namespace reactphysics3d;
 
 // Constructor
-ContactManifold::ContactManifold(Body* const body1, Body* const body2,
+ContactManifold::ContactManifold(CollisionBody* body1, CollisionBody* body2,
                                  MemoryAllocator& memoryAllocator)
                 : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0),
-                  mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0),
+                  mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
                   mMemoryAllocator(memoryAllocator) {
     
 }
@@ -107,9 +107,12 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans
 
     // Update the world coordinates and penetration depth of the contact points in the manifold
     for (uint i=0; i<mNbContactPoints; i++) {
-        mContactPoints[i]->setWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1());
+        mContactPoints[i]->setWorldPointOnBody1(transform1 *
-        mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2());
+                                                mContactPoints[i]->getLocalPointOnBody1());
-        mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
+        mContactPoints[i]->setWorldPointOnBody2(transform2 *
+                                                mContactPoints[i]->getLocalPointOnBody2());
+        mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
+                  mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
     }
 
     const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
@@ -172,7 +175,8 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) cons
 /// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
 /// when we compute this area, we do not calculate it exactly but we
 /// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
-/// this is only a guess that is faster to compute.
+/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
+/// by Erwin Coumans (http://wwww.bulletphysics.org).
 int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
 
     assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
@@ -185,28 +189,32 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& ne
     if (indexMaxPenetration != 0) {
         // Compute the area
         Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
-        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
+                          mContactPoints[2]->getLocalPointOnBody1();
         Vector3 crossProduct = vector1.cross(vector2);
         area0 = crossProduct.lengthSquare();
     }
     if (indexMaxPenetration != 1) {
         // Compute the area
         Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
-        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
+                          mContactPoints[2]->getLocalPointOnBody1();
         Vector3 crossProduct = vector1.cross(vector2);
         area1 = crossProduct.lengthSquare();
     }
     if (indexMaxPenetration != 2) {
         // Compute the area
         Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
-        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
+                          mContactPoints[1]->getLocalPointOnBody1();
         Vector3 crossProduct = vector1.cross(vector2);
         area2 = crossProduct.lengthSquare();
     }
     if (indexMaxPenetration != 3) {
         // Compute the area
         Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
-        Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1();
+        Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
+                          mContactPoints[1]->getLocalPointOnBody1();
         Vector3 crossProduct = vector1.cross(vector2);
         area3 = crossProduct.lengthSquare();
     }

src/engine/ContactManifold.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include <vector>
-#include "../body/Body.h"
+#include "../body/CollisionBody.h"
 #include "../constraint/ContactPoint.h"
 #include "../memory/MemoryAllocator.h"
 
@@ -38,6 +38,35 @@ namespace reactphysics3d {
 // Constants
 const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4;   // Maximum number of contacts in the manifold
 
+// Class declarations
+class ContactManifold;
+
+// Structure ContactManifoldListElement
+/**
+ * This structure represents a single element of a linked list of contact manifolds
+ */
+struct ContactManifoldListElement {
+
+    public:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Pointer to the actual contact manifold
+        ContactManifold* contactManifold;
+
+        /// Next element of the list
+        ContactManifoldListElement* next;
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        ContactManifoldListElement(ContactManifold* initContactManifold,
+                                   ContactManifoldListElement* initNext)
+                                  :contactManifold(initContactManifold), next(initNext) {
+
+        }
+};
+
 // Class ContactManifold
 /**
  * This class represents the set of contact points between two bodies.
@@ -59,11 +88,11 @@ class ContactManifold {
 
         // -------------------- Attributes -------------------- //
 
-        /// Pointer to the first body
+        /// Pointer to the first body of the contact
-        Body* const mBody1;
+        CollisionBody* mBody1;
 
-        /// Pointer to the second body
+        /// Pointer to the second body of the contact
-        Body* const mBody2;
+        CollisionBody* mBody2;
 
         /// Contact points in the manifold
         ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
@@ -86,6 +115,9 @@ class ContactManifold {
         /// Twist friction constraint accumulated impulse
         decimal mFrictionTwistImpulse;
 
+        /// True if the contact manifold has already been added into an island
+        bool mIsAlreadyInIsland;
+
         /// Reference to the memory allocator
         MemoryAllocator& mMemoryAllocator;
 
@@ -97,6 +129,12 @@ class ContactManifold {
         /// Private assignment operator
         ContactManifold& operator=(const ContactManifold& contactManifold);
 
+        /// Return a pointer to the first body of the contact manifold
+        CollisionBody* getBody1() const;
+
+        /// Return a pointer to the second body of the contact manifold
+        CollisionBody* getBody2() const;
+
         /// Return the index of maximum area
         int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
 
@@ -109,15 +147,16 @@ class ContactManifold {
         /// Remove a contact point from the manifold
         void removeContactPoint(uint index);
 
-        /// Return true if two vectors are approximatively equal
+        /// Return true if the contact manifold has already been added into an island
-        bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;
+        bool isAlreadyInIsland() const;
         
     public:
 
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactManifold(Body* const mBody1, Body* const mBody2, MemoryAllocator& memoryAllocator);
+        ContactManifold(CollisionBody* body1, CollisionBody* body2,
+                        MemoryAllocator& memoryAllocator);
 
         /// Destructor
         ~ContactManifold();
@@ -166,8 +205,23 @@ class ContactManifold {
 
         /// Return a contact point of the manifold
         ContactPoint* getContactPoint(uint index) const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+        friend class Island;
 };
 
+// Return a pointer to the first body of the contact manifold
+inline CollisionBody* ContactManifold::getBody1() const {
+    return mBody1;
+}
+
+// Return a pointer to the second body of the contact manifold
+inline CollisionBody* ContactManifold::getBody2() const {
+    return mBody2;
+}
+
 // Return the number of contact points in the manifold
 inline uint ContactManifold::getNbContactPoints() const {
     return mNbContactPoints;
@@ -229,6 +283,11 @@ inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
     return mContactPoints[index];
 }
 
+// Return true if the contact manifold has already been added into an island
+inline bool ContactManifold::isAlreadyInIsland() const {
+    return mIsAlreadyInIsland;
+}
+
 }
 #endif
 

src/engine/DynamicsWorld.cpp

@@ -44,7 +44,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
                                   mMapBodyToConstrainedVelocityIndex),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
-                mIsDeactivationActive(DEACTIVATION_ENABLED) {
+                mIsSleepingEnabled(SPLEEPING_ENABLED), mNbIslands(0), mNbIslandsCapacity(0),
+                mIslands(NULL) {
 
 }
 
@@ -59,6 +60,19 @@ DynamicsWorld::~DynamicsWorld() {
         mMemoryAllocator.release((*it).second, sizeof(OverlappingPair));
     }
 
+    // Release the memory allocated for the islands
+    for (uint i=0; i<mNbIslands; i++) {
+
+        // Call the island destructor
+        mIslands[i]->Island::~Island();
+
+        // Release the allocated memory for the island
+        mMemoryAllocator.release(mIslands[i], sizeof(Island));
+    }
+    if (mNbIslandsCapacity > 0) {
+        mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
+    }
+
     // Free the allocated memory for the constrained velocities
     cleanupConstrainedVelocitiesArray();
 
@@ -94,6 +108,9 @@ void DynamicsWorld::update() {
         // Remove all contact manifolds
         mContactManifolds.clear();
 
+        // Reset all the contact manifolds lists of each body
+        resetContactManifoldListsOfBodies();
+		
         // Compute the collision detection
         mCollisionDetection.computeCollisionDetection();
 
@@ -106,6 +123,9 @@ void DynamicsWorld::update() {
         // Update the timer
         mTimer.nextStep();
 
+        // Compute the islands (separate groups of bodies with constraints between each others)
+        computeIslands();
+
         // Solve the contacts and constraints
         solveContactsAndConstraints();
 
@@ -448,7 +468,8 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     // Remove the collision shape from the world
     removeCollisionShape(rigidBody->getCollisionShape());
 
-    // Destroy all the joints that contains the rigid body to be destroyed
+    // Destroy all the joints in which the rigid body to be destroyed is involved
+    // TODO : Iterate on the mJointList of the rigid body instead over all the joints of the world
     bodyindex idToRemove = rigidBody->getID();
     for (std::set<Constraint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
         if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) {
@@ -456,6 +477,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
         }
     }
 
+    // Reset the contact manifold list of the body
+    rigidBody->resetContactManifoldsList(mMemoryAllocator);
+
     // Call the destructor of the rigid body
     rigidBody->RigidBody::~RigidBody();
 
@@ -529,6 +553,9 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) {
     // Add the joint into the world
     mJoints.insert(newJoint);
 
+    // Add the joint into the joint list of the bodies involved in the joint
+    addJointToBody(newJoint);
+
     // Return the pointer to the created joint
     return newJoint;
 }
@@ -548,14 +575,235 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
     // Remove the joint from the world
     mJoints.erase(joint);
 
-    // Get the size in bytes of the joint
+    // Remove the joint from the joint list of the bodies involved in the joint
-    size_t nbBytes = joint->getSizeInBytes();
+    joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint);
+    joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint);
 
     // Call the destructor of the joint
     joint->Constraint::~Constraint();
 
     // Release the allocated memory
-    mMemoryAllocator.release(joint, nbBytes);
+    mMemoryAllocator.release(joint, joint->getSizeInBytes());
+}
+
+// Add the joint to the list of joints of the two bodies involved in the joint
+void DynamicsWorld::addJointToBody(Constraint* joint) {
+
+    assert(joint != NULL);
+
+    // Add the joint at the beginning of the linked list of joints of the first body
+    void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(JointListElement));
+    JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint,
+                                                                     joint->mBody1->mJointsList);
+    joint->mBody1->mJointsList = jointListElement1;
+
+    // Add the joint at the beginning of the linked list of joints of the second body
+    void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(JointListElement));
+    JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint,
+                                                                     joint->mBody2->mJointsList);
+    joint->mBody2->mJointsList = jointListElement2;
+}
+
+// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
+// in the corresponding contact
+void DynamicsWorld::addContactManifoldToBody(ContactManifold* contactManifold,
+                                             CollisionBody* body1, CollisionBody* body2) {
+
+    assert(contactManifold != NULL);
+
+    // Add the contact manifold at the beginning of the linked
+    // list of contact manifolds of the first body
+    void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+    ContactManifoldListElement* listElement1 = new (allocatedMemory1)
+                                                  ContactManifoldListElement(contactManifold,
+                                                                     body1->mContactManifoldsList);
+    body1->mContactManifoldsList = listElement1;
+
+    // Add the joint at the beginning of the linked list of joints of the second body
+    void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
+    ContactManifoldListElement* listElement2 = new (allocatedMemory2)
+                                                  ContactManifoldListElement(contactManifold,
+                                                                     body2->mContactManifoldsList);
+    body2->mContactManifoldsList = listElement2;
+}
+
+// Reset all the contact manifolds linked list of each body
+void DynamicsWorld::resetContactManifoldListsOfBodies() {
+
+    // For each rigid body of the world
+    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+
+        // Reset the contact manifold list of the body
+        (*it)->resetContactManifoldsList(mMemoryAllocator);
+    }
+}
+
+// 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() {
+
+    PROFILE("DynamicsWorld::computeIslands()");
+
+    uint nbBodies = mRigidBodies.size();
+
+    // Clear all the islands
+    for (uint i=0; i<mNbIslands; i++) {
+
+        // Call the island destructor
+        mIslands[i]->Island::~Island();
+
+        // Release the allocated memory for the island
+        mMemoryAllocator.release(mIslands[i], sizeof(Island));
+    }
+
+    // Allocate and create the array of islands
+    if (mNbIslandsCapacity != nbBodies && nbBodies > 0) {
+        if (mNbIslandsCapacity > 0) {
+            mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
+        }
+        mNbIslandsCapacity = nbBodies;
+        mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity);
+    }
+    mNbIslands = 0;
+
+    // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
+    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+        (*it)->mIsAlreadyInIsland = false;
+    }
+    for (std::vector<ContactManifold*>::iterator it = mContactManifolds.begin();
+         it != mContactManifolds.end(); ++it) {
+        (*it)->mIsAlreadyInIsland = false;
+    }
+    for (std::set<Constraint*>::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 = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack);
+
+    uint idIsland = 0;  // TODO : REMOVE THIS
+
+    // For each rigid body of the world
+    for (std::set<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->isAlreadyInIsland()) continue;
+
+        // If the body is not moving, we go to the next body
+        // TODO : When we will use STATIC bodies, we will need to take care of this case here
+        if (!body->getIsMotionEnabled()) continue;
+
+        // If the body is sleeping, we go to the next body
+        if (body->isSleeping()) continue;
+
+        // Reset the stack of bodies to visit
+        uint stackIndex = 0;
+        stackBodiesToVisit[stackIndex] = body;
+        stackIndex++;
+        body->mIsAlreadyInIsland = true;
+
+        // Create the new island
+        void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
+        mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(idIsland, nbBodies,mContactManifolds.size(),
+                                                                  mJoints.size(), mMemoryAllocator);
+        idIsland++;
+
+        // 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];
+
+            // Awake the body if it is slepping
+            bodyToVisit->setIsSleeping(false);
+
+            // Add the body into the island
+            mIslands[mNbIslands]->addBody(bodyToVisit);
+
+            // If the current body is not moving, we do not want to perform the DFS
+            // search across that body
+            if (!bodyToVisit->getIsMotionEnabled()) continue;
+
+            // For each contact manifold in which the current body is involded
+            ContactManifoldListElement* contactElement;
+            for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != NULL;
+                 contactElement = contactElement->next) {
+
+                ContactManifold* contactManifold = contactElement->contactManifold;
+
+                // Check if the current contact manifold has already been added into an island
+                if (contactManifold->isAlreadyInIsland()) continue;
+
+                // Add the contact manifold into the island
+                mIslands[mNbIslands]->addContactManifold(contactManifold);
+                contactManifold->mIsAlreadyInIsland = true;
+
+                // Get the other body of the contact manifold
+                RigidBody* body1 = dynamic_cast<RigidBody*>(contactManifold->getBody1());
+                RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
+                RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
+
+                // Check if the other body has already been added to the island
+                if (otherBody->isAlreadyInIsland()) 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 != NULL;
+                 jointElement = jointElement->next) {
+
+                Constraint* 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 = dynamic_cast<RigidBody*>(joint->getBody1());
+                RigidBody* body2 = dynamic_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->isAlreadyInIsland()) 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]->getIsMotionEnabled()) {
+                mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false;
+            }
+        }
+
+        mNbIslands++;
+     }
+
+    // Release the allocated memory for the stack of bodies to visit
+    mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack);
 }
 
 // Notify the world about a new broad-phase overlapping pair
@@ -565,8 +813,8 @@ void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair)
     bodyindexpair indexPair = addedPair->getBodiesIndexPair();
 
     // Add the pair into the set of overlapping pairs (if not there yet)
-    OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair))) OverlappingPair(
+    OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair)))
-                                        addedPair->body1, addedPair->body2, mMemoryAllocator);
+                              OverlappingPair(addedPair->body1, addedPair->body2, mMemoryAllocator);
     assert(newPair != NULL);
     std::pair<map<bodyindexpair, OverlappingPair*>::iterator, bool> check =
             mOverlappingPairs.insert(make_pair(indexPair, newPair));
@@ -604,4 +852,9 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
 
     // Add the contact manifold to the world
     mContactManifolds.push_back(overlappingPair->getContactManifold());
+
+    // Add the contact manifold into the list of contact manifolds
+    // of the two bodies involved in the contact
+    addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->mBody1,
+                             overlappingPair->mBody2);
 }

src/engine/DynamicsWorld.h

@@ -33,6 +33,7 @@
 #include "ConstraintSolver.h"
 #include "../body/RigidBody.h"
 #include "Timer.h"
+#include "Island.h"
 #include "../configuration.h"
 
 /// Namespace ReactPhysics3D
@@ -65,13 +66,14 @@ class DynamicsWorld : public CollisionWorld {
         /// Number of iterations for the position solver of the Sequential Impulses technique
         uint mNbPositionSolverIterations;
 
-        /// True if the deactivation (sleeping) of inactive bodies is enabled
+        /// True if the spleeping technique for inactive bodies is enabled
-        bool mIsDeactivationActive;
+        bool mIsSleepingEnabled;
 
         /// All the rigid bodies of the physics world
         std::set<RigidBody*> mRigidBodies;
 
         /// All the contact constraints
+        // TODO : Remove this variable (we will use the ones in the island now)
         std::vector<ContactManifold*> mContactManifolds;
 
         /// All the joints of the world
@@ -100,6 +102,15 @@ class DynamicsWorld : public CollisionWorld {
         /// Map body to their index in the constrained velocities array
         std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
 
+        /// Number of islands in the world
+        uint mNbIslands;
+
+        /// Current allocated capacity for the islands
+        uint mNbIslandsCapacity;
+
+        /// Array with all the islands of awaken bodies
+        Island** mIslands;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -136,6 +147,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Reset the boolean movement variable of each body
         void resetBodiesMovementVariable();
 
+        /// Compute the islands of awake bodies.
+        void computeIslands();
+
         /// Update the overlapping pair
         virtual void updateOverlappingPair(const BroadPhasePair* pair);
 
@@ -198,6 +212,17 @@ public :
         /// Destroy a joint
         void destroyJoint(Constraint* joint);
 
+        /// Add the joint to the list of joints of the two bodies involved in the joint
+        void addJointToBody(Constraint* joint);
+
+        //// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
+        //// in the corresponding contact.
+        void addContactManifoldToBody(ContactManifold* contactManifold,
+                                      CollisionBody *body1, CollisionBody *body2);
+
+        /// Reset all the contact manifolds linked list of each body
+        void resetContactManifoldListsOfBodies();
+
         /// Return the gravity vector of the world
         Vector3 getGravity() const;
 
@@ -227,6 +252,12 @@ public :
 
         /// Return a reference to the contact manifolds of the world
         const std::vector<ContactManifold*>& getContactManifolds() const;
+
+        // TODO : REMOVE THIS
+        Island** getIslands() { return mIslands;}
+
+        // TODO : REMOVE THIS
+        uint getNbIslands() const {return mNbIslands;}
 };
 
 // Start the physics simulation

src/engine/Island.cpp

@@ -0,0 +1,54 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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.    *
+*                                                                               *
+********************************************************************************/
+
+// Libraries
+#include "Island.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+Island::Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
+               MemoryAllocator& memoryAllocator)
+       : mID(id), mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0),
+         mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) {
+
+    // Allocate memory for the arrays
+    mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies;
+    mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies);
+    mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds;
+    mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate(
+                                                                mNbAllocatedBytesContactManifolds);
+    mNbAllocatedBytesJoints = sizeof(Constraint*) * nbMaxJoints;
+    mJoints = (Constraint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints);
+}
+
+// Destructor
+Island::~Island() {
+
+    // Release the memory of the arrays
+    mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies);
+    mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds);
+    mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints);
+}

src/engine/Island.h

@@ -0,0 +1,186 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
+* Copyright (c) 2010-2013 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_ISLAND_H
+#define REACTPHYSICS3D_ISLAND_H
+
+// Libraries
+#include "../memory/MemoryAllocator.h"
+#include "../body/RigidBody.h"
+#include "../constraint/Constraint.h"
+#include "ContactManifold.h"
+
+namespace reactphysics3d {
+
+// Class Island
+/**
+ * An island represent an isolated group of awake bodies that are connected with each other by
+ * some contraints (contacts or joints).
+ */
+class Island {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        // TODO : REMOVE THIS
+        uint mID;
+
+        /// Array with all the bodies of the island
+        RigidBody** mBodies;
+
+        /// Array with all the contact manifolds between bodies of the island
+        ContactManifold** mContactManifolds;
+
+        /// Array with all the joints between bodies of the island
+        Constraint** mJoints;
+
+        /// Current number of bodies in the island
+        uint mNbBodies;
+
+        /// Current number of contact manifold in the island
+        uint mNbContactManifolds;
+
+        /// Current number of joints in the island
+        uint mNbJoints;
+
+        /// Reference to the memory allocator
+        MemoryAllocator& mMemoryAllocator;
+
+        /// Number of bytes allocated for the bodies array
+        size_t mNbAllocatedBytesBodies;
+
+        /// Number of bytes allocated for the contact manifolds array
+        size_t mNbAllocatedBytesContactManifolds;
+
+        /// Number of bytes allocated for the joints array
+        size_t mNbAllocatedBytesJoints;
+
+        // -------------------- Methods -------------------- //
+
+        /// Private assignment operator
+        Island& operator=(const Island& island);
+
+        /// Private copy-constructor
+        Island(const Island& island);
+
+    public:
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints,
+               MemoryAllocator& memoryAllocator);
+
+        /// Destructor
+        ~Island();
+
+        /// Add a body into the island
+        void addBody(RigidBody* body);
+
+        /// Add a contact manifold into the island
+        void addContactManifold(ContactManifold* contactManifold);
+
+        /// Add a joint into the island
+        void addJoint(Constraint* joint);
+
+        /// Return the number of bodies in the island
+        uint getNbBodies() const;
+
+        /// Return the number of contact manifolds in the island
+        uint getNbContactManifolds() const;
+
+        /// Return the number of joints in the island
+        uint getNbJoints() const;
+
+        /// Return a pointer to the array of bodies
+        RigidBody** getBodies();
+
+        /// Return a pointer to the array of contact manifolds
+        ContactManifold** getContactManifold();
+
+        /// Return a pointer to the array of joints
+        Constraint** getJoints();
+
+        // TODO : REMOVE THIS
+        uint getID() const {return mID;}
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
+};
+
+// Add a body into the island
+inline void Island::addBody(RigidBody* body) {
+    assert(!body->isSleeping());
+    mBodies[mNbBodies] = body;
+    mNbBodies++;
+}
+
+// Add a contact manifold into the island
+inline void Island::addContactManifold(ContactManifold* contactManifold) {
+    mContactManifolds[mNbContactManifolds] = contactManifold;
+    mNbContactManifolds++;
+}
+
+// Add a joint into the island
+inline void Island::addJoint(Constraint* joint) {
+    mJoints[mNbJoints] = joint;
+    mNbJoints++;
+}
+
+// Return the number of bodies in the island
+inline uint Island::getNbBodies() const {
+    return mNbBodies;
+}
+
+// Return the number of contact manifolds in the island
+inline uint Island::getNbContactManifolds() const {
+    return mNbContactManifolds;
+}
+
+// Return the number of joints in the island
+inline uint Island::getNbJoints() const {
+    return mNbJoints;
+}
+
+// Return a pointer to the array of bodies
+inline RigidBody** Island::getBodies() {
+    return mBodies;
+}
+
+// Return a pointer to the array of contact manifolds
+inline ContactManifold** Island::getContactManifold() {
+    return mContactManifolds;
+}
+
+// Return a pointer to the array of joints
+inline Constraint** Island::getJoints() {
+    return mJoints;
+}
+
+}
+
+#endif

src/engine/OverlappingPair.h

@@ -47,10 +47,10 @@ class OverlappingPair {
         // -------------------- Attributes -------------------- //
 
         /// Pointer to the first body of the contact
-        CollisionBody* const mBody1;
+        CollisionBody* mBody1;
 
         /// Pointer to the second body of the contact
-        CollisionBody* const mBody2;
+        CollisionBody* mBody2;
 
         /// Persistent contact manifold
         ContactManifold mContactManifold;
@@ -100,6 +100,10 @@ class OverlappingPair {
 
         /// Return the contact manifold
         ContactManifold* getContactManifold();
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
 };
 
 // Return the pointer to first body

src/memory/MemoryAllocator.cpp

@@ -148,10 +148,10 @@ void* MemoryAllocator::allocate(size_t size) {
         assert(nbUnits * unitSize <= BLOCK_SIZE);
         for (size_t i=0; i < nbUnits - 1; i++) {
             MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i);
-            MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i + 1));
+            MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1));
             unit->nextUnit = nextUnit;
         }
-        MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (nbUnits - 1));
+        MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1));
         lastUnit->nextUnit = NULL;
 
         // Add the new allocated block into the list of free memory units in the heap