Merge branch 'sleeping' into develop

src/body/Body.cpp

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

src/body/Body.h

@@ -47,6 +47,18 @@ 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;
+
+        /// Elapsed time since the body velocity was bellow the sleep velocity
+        decimal mSleepTime;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -68,6 +80,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
+        virtual void setIsSleeping(bool isSleeping);
+
         /// Smaller than operator
         bool operator<(const Body& body2) const;
 
@@ -79,6 +109,10 @@ class Body {
 
         /// Not equal operator
         bool operator!=(const Body& body2) const;
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
 };
 
 // Return the id of the body
@@ -86,6 +120,48 @@ 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;
+
+    if (!mIsAllowedToSleep) setIsSleeping(false);
+}
+
+// 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) {
+
+    if (isSleeping) {
+        mSleepTime = decimal(0.0);
+    }
+    else {
+        if (mIsSleeping) {
+            mSleepTime = decimal(0.0);
+        }
+    }
+
+    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,42 @@ 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;
+            }
+            currentElement = currentElement->next;
+        }
+    }
+}
+
+

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,16 @@ 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;
+
+        /// Set the variable to know whether or not the body is sleeping
+        virtual void setIsSleeping(bool isSleeping);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class DynamicsWorld;
 };
 
 // Method that return the mass of the body
@@ -328,6 +349,24 @@ 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;
+}
+
+// Set the variable to know whether or not the body is sleeping
+inline void RigidBody::setIsSleeping(bool isSleeping) {
+
+    if (isSleeping) {
+        mLinearVelocity.setToZero();
+        mAngularVelocity.setToZero();
+        mExternalForce.setToZero();
+        mExternalTorque.setToZero();
+    }
+
+    Body::setIsSleeping(isSleeping);
+}
+
 }
 
  #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 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);
@@ -110,6 +110,17 @@ const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15;
 /// Number of iterations when solving the position constraints of the Sequential Impulse technique
 const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
 
+/// Time (in seconds) that a body must stay still to be considered sleeping
+const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
+
+/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+/// might enter sleeping mode.
+const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
+
+/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+/// might enter sleeping mode
+const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
+
 }
 
 #endif

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/constraint/HingeJoint.cpp

@@ -59,7 +59,7 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
 
     // Compute the inverse of the initial orientation difference between the two bodies
     mInitOrientationDifferenceInv = transform2.getOrientation() *
-                                 transform1.getOrientation().getInverse();
+                                    transform1.getOrientation().getInverse();
     mInitOrientationDifferenceInv.normalize();
     mInitOrientationDifferenceInv.inverse();
 }
@@ -732,7 +732,9 @@ void HingeJoint::enableMotor(bool isMotorEnabled) {
     mIsMotorEnabled = isMotorEnabled;
     mImpulseMotor = 0.0;
 
-    // TODO : Wake up the bodies of the joint here when sleeping is implemented
+    // Wake up the two bodies of the joint
+    mBody1->setIsSleeping(false);
+    mBody2->setIsSleeping(false);
 }
 
 // Set the minimum angle limit
@@ -770,7 +772,9 @@ void HingeJoint::resetLimits() {
     mImpulseLowerLimit = 0.0;
     mImpulseUpperLimit = 0.0;
 
-    // TODO : Wake up the bodies of the joint here when sleeping is implemented
+    // Wake up the two bodies of the joint
+    mBody1->setIsSleeping(false);
+    mBody2->setIsSleeping(false);
 }
 
 // Set the motor speed
@@ -780,7 +784,9 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) {
 
         mMotorSpeed = motorSpeed;
 
-        // TODO : Wake up the bodies of the joint here when sleeping is implemented
+        // Wake up the two bodies of the joint
+        mBody1->setIsSleeping(false);
+        mBody2->setIsSleeping(false);
     }
 }
 
@@ -792,7 +798,9 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
         assert(mMaxMotorTorque >= 0.0);
         mMaxMotorTorque = maxMotorTorque;
 
-        // TODO : Wake up the bodies of the joint here when sleeping is implemented
+        // Wake up the two bodies of the joint
+        mBody1->setIsSleeping(false);
+        mBody2->setIsSleeping(false);
     }
 }
 

src/constraint/SliderJoint.cpp

@@ -768,7 +768,9 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
     mIsMotorEnabled = isMotorEnabled;
     mImpulseMotor = 0.0;
 
-    // TODO : Wake up the bodies of the joint here when sleeping is implemented
+    // Wake up the two bodies of the joint
+    mBody1->setIsSleeping(false);
+    mBody2->setIsSleeping(false);
 }
 
 // Return the current translation value of the joint
@@ -830,7 +832,9 @@ void SliderJoint::resetLimits() {
     mImpulseLowerLimit = 0.0;
     mImpulseUpperLimit = 0.0;
 
-    // TODO : Wake up the bodies of the joint here when sleeping is implemented
+    // Wake up the two bodies of the joint
+    mBody1->setIsSleeping(false);
+    mBody2->setIsSleeping(false);
 }
 
 // Set the motor speed
@@ -840,7 +844,9 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) {
 
         mMotorSpeed = motorSpeed;
 
-        // TODO : Wake up the bodies of the joint here when sleeping is implemented
+        // Wake up the two bodies of the joint
+        mBody1->setIsSleeping(false);
+        mBody2->setIsSleeping(false);
     }
 }
 
@@ -852,6 +858,8 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
         assert(mMaxMotorForce >= 0.0);
         mMaxMotorForce = maxMotorForce;
 
-        // TODO : Wake up the bodies of the joint here when sleeping is implemented
+        // Wake up the two bodies of the joint
+        mBody1->setIsSleeping(false);
+        mBody2->setIsSleeping(false);
     }
 }

src/engine/ConstraintSolver.cpp

@@ -30,18 +30,14 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
+ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
-                                   std::vector<Vector3>& linearVelocities,
-                                   std::vector<Vector3>& angularVelocities,
-                                   std::vector<Vector3>& positions,
                                    std::vector<Quaternion>& orientations,
                                    const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-                 : mJoints(joints), mLinearVelocities(linearVelocities),
+                 : mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
-                   mAngularVelocities(angularVelocities), mPositions(positions),
                    mOrientations(orientations),
                    mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
-                   mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
+                   mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
-                   angularVelocities, positions, orientations, mapBodyToVelocityIndex){
+                                                                      mapBodyToVelocityIndex){
 
 }
 
@@ -50,10 +46,16 @@ ConstraintSolver::~ConstraintSolver() {
 
 }
 
-// Initialize the constraint solver
+// Initialize the constraint solver for a given island
-void ConstraintSolver::initialize(decimal dt) {
+void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
 
-    PROFILE("ConstraintSolver::initialize()");
+    PROFILE("ConstraintSolver::initializeForIsland()");
+
+    assert(mLinearVelocities != NULL);
+    assert(mAngularVelocities != NULL);
+    assert(island != NULL);
+    assert(island->getNbBodies() > 0);
+    assert(island->getNbJoints() > 0);
 
     // Set the current time step
     mTimeStep = dt;
@@ -62,54 +64,50 @@ void ConstraintSolver::initialize(decimal dt) {
     mConstraintSolverData.timeStep = mTimeStep;
     mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
 
-    // For each joint
+    // For each joint of the island
-    std::set<Constraint*>::iterator it;
+    Constraint** joints = island->getJoints();
-    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+    for (uint i=0; i<island->getNbJoints(); i++) {
-
-        Constraint* joint = (*it);
-
-        // Get the rigid bodies of the joint
-        RigidBody* body1 = joint->getBody1();
-        RigidBody* body2 = joint->getBody2();
-
-        // Add the bodies to the set of constrained bodies
-        mConstraintBodies.insert(body1);
-        mConstraintBodies.insert(body2);
 
         // Initialize the constraint before solving it
-        joint->initBeforeSolve(mConstraintSolverData);
+        joints[i]->initBeforeSolve(mConstraintSolverData);
 
         // Warm-start the constraint if warm-starting is enabled
         if (mIsWarmStartingActive) {
-            joint->warmstart(mConstraintSolverData);
+            joints[i]->warmstart(mConstraintSolverData);
         }
     }
 }
 
 // Solve the velocity constraints
-void ConstraintSolver::solveVelocityConstraints() {
+void ConstraintSolver::solveVelocityConstraints(Island* island) {
 
     PROFILE("ConstraintSolver::solveVelocityConstraints()");
 
-    // For each joint
+    assert(island != NULL);
-    std::set<Constraint*>::iterator it;
+    assert(island->getNbJoints() > 0);
-    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+
+    // For each joint of the island
+    Constraint** joints = island->getJoints();
+    for (uint i=0; i<island->getNbJoints(); i++) {
 
         // Solve the constraint
-        (*it)->solveVelocityConstraint(mConstraintSolverData);
+        joints[i]->solveVelocityConstraint(mConstraintSolverData);
     }
 }
 
 // Solve the position constraints
-void ConstraintSolver::solvePositionConstraints() {
+void ConstraintSolver::solvePositionConstraints(Island* island) {
 
     PROFILE("ConstraintSolver::solvePositionConstraints()");
 
-    // For each joint
+    assert(island != NULL);
-    std::set<Constraint*>::iterator it;
+    assert(island->getNbJoints() > 0);
-    for (it = mJoints.begin(); it != mJoints.end(); ++it) {
+
+    // For each joint of the island
+    Constraint** joints = island->getJoints();
+    for (uint i=0; i < island->getNbJoints(); i++) {
 
         // Solve the constraint
-        (*it)->solvePositionConstraint(mConstraintSolverData);
+        joints[i]->solvePositionConstraint(mConstraintSolverData);
     }
 }

src/engine/ConstraintSolver.h

@@ -30,6 +30,7 @@
 #include "../configuration.h"
 #include "mathematics/mathematics.h"
 #include "../constraint/Constraint.h"
+#include "Island.h"
 #include <map>
 #include <set>
 
@@ -47,11 +48,11 @@ struct ConstraintSolverData {
         /// Current time step of the simulation
         decimal timeStep;
 
-        /// Reference to the bodies linear velocities
+        /// Array with the bodies linear velocities
-        std::vector<Vector3>& linearVelocities;
+        Vector3* linearVelocities;
 
-        /// Reference to the bodies angular velocities
+        /// Array with the bodies angular velocities
-        std::vector<Vector3>& angularVelocities;
+        Vector3* angularVelocities;
 
         /// Reference to the bodies positions
         std::vector<Vector3>& positions;
@@ -67,13 +68,11 @@ struct ConstraintSolverData {
         bool isWarmStartingActive;
 
         /// Constructor
-        ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
+        ConstraintSolverData(std::vector<Vector3>& refPositions,
-                             std::vector<Vector3>& refAngularVelocities,
-                             std::vector<Vector3>& refPositions,
                              std::vector<Quaternion>& refOrientations,
                              const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
-                           :linearVelocities(refLinearVelocities),
+                           :linearVelocities(NULL),
-                            angularVelocities(refAngularVelocities),
+                            angularVelocities(NULL),
                             positions(refPositions), orientations(refOrientations),
                             mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
 
@@ -156,19 +155,13 @@ class ConstraintSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to all the joints of the world
+        /// Array of constrained linear velocities (state of the linear velocities
-        std::set<Constraint*>& mJoints;
-
-        /// Constrained bodies
-        std::set<RigidBody*> mConstraintBodies;
-
-        /// Reference to the array of constrained linear velocities (state of the linear velocities
         /// after solving the constraints)
-        std::vector<Vector3>& mLinearVelocities;
+        Vector3* mLinearVelocities;
 
-        /// Reference to the array of constrained angular velocities (state of the angular velocities
+        /// Array of constrained angular velocities (state of the angular velocities
         /// after solving the constraints)
-        std::vector<Vector3>& mAngularVelocities;
+        Vector3* mAngularVelocities;
 
         /// Reference to the array of bodies positions (for position error correction)
         std::vector<Vector3>& mPositions;
@@ -194,24 +187,20 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver(std::set<Constraint*>& joints,
+        ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
-                         std::vector<Vector3>& linearVelocities,
-                         std::vector<Vector3>& angularVelocities,
-                         std::vector<Vector3>& positions,
-                         std::vector<Quaternion>& orientations,
                          const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 
         /// Destructor
         ~ConstraintSolver();
 
-        /// Initialize the constraint solver
+        /// Initialize the constraint solver for a given island
-        void initialize(decimal dt);
+        void initializeForIsland(decimal dt, Island* island);
 
         /// Solve the constraints
-        void solveVelocityConstraints();
+        void solveVelocityConstraints(Island* island);
 
         /// Solve the position constraints
-        void solvePositionConstraints();
+        void solvePositionConstraints(Island* island);
 
         /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
         bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
@@ -219,13 +208,20 @@ class ConstraintSolver {
         /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
         void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
 
-        /// Return true if the body is in at least one constraint
+        /// Set the constrained velocities arrays
-        bool isConstrainedBody(RigidBody* body) const;
+        void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
+                                            Vector3* constrainedAngularVelocities);
 };
 
-// Return true if the body is in at least one constraint
+// Set the constrained velocities arrays
-inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
+inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-    return mConstraintBodies.count(body) == 1;
+                                                            Vector3* constrainedAngularVelocities) {
+    assert(constrainedLinearVelocities != NULL);
+    assert(constrainedAngularVelocities != NULL);
+    mLinearVelocities = constrainedLinearVelocities;
+    mAngularVelocities = constrainedAngularVelocities;
+    mConstraintSolverData.linearVelocities = mLinearVelocities;
+    mConstraintSolverData.angularVelocities = mAngularVelocities;
 }
 
 }

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/ContactSolver.cpp

@@ -36,18 +36,12 @@ using namespace std;
 // Constants initialization
 const decimal ContactSolver::BETA = decimal(0.2);
 const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
-const decimal ContactSolver::SLOP = decimal(0.01);
+const decimal ContactSolver::SLOP= decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
+ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-                             std::vector<Vector3>& constrainedLinearVelocities,
+              :mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
-                             std::vector<Vector3>& constrainedAngularVelocities,
+               mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL),
-                             const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-              :mContactManifolds(contactManifolds),
-               mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
-               mContactConstraints(NULL),
-               mLinearVelocities(constrainedLinearVelocities),
-               mAngularVelocities(constrainedAngularVelocities),
                mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
                mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
                mIsSolveFrictionAtContactManifoldCenterActive(true) {
@@ -59,26 +53,32 @@ ContactSolver::~ContactSolver() {
 
 }
 
-// Initialize the constraint solver
+// Initialize the constraint solver for a given island
-void ContactSolver::initialize(decimal dt) {
+void ContactSolver::initializeForIsland(decimal dt, Island* island) {
 
-    PROFILE("ContactSolver::initialize()");
+    PROFILE("ContactSolver::initializeForIsland()");
+
+    assert(island != NULL);
+    assert(island->getNbBodies() > 0);
+    assert(island->getNbContactManifolds() > 0);
+    assert(mSplitLinearVelocities != NULL);
+    assert(mSplitAngularVelocities != NULL);
 
     // Set the current time step
     mTimeStep = dt;
 
-    // TODO : Use better memory allocation here
+    mNbContactManifolds = island->getNbContactManifolds();
-    mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
 
-    mNbContactManifolds = 0;
+    mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
+    assert(mContactConstraints != NULL);
 
-    // For each contact manifold of the world
+    // For each contact manifold of the island
-    vector<ContactManifold*>::iterator it;
+    ContactManifold** contactManifolds = island->getContactManifold();
-    for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
+    for (uint i=0; i<mNbContactManifolds; i++) {
 
-        ContactManifold* externalManifold = *it;
+        ContactManifold* externalManifold = contactManifolds[i];
 
-        ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
+        ContactManifoldSolver& internalManifold = mContactConstraints[i];
 
         assert(externalManifold->getNbContactPoints() > 0);
 
@@ -86,10 +86,6 @@ void ContactSolver::initialize(decimal dt) {
         RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1();
         RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
 
-        // Add the two bodies of the constraint in the constraintBodies list
-        mConstraintBodies.insert(body1);
-        mConstraintBodies.insert(body2);
-
         // Get the position of the two bodies
         Vector3 x1 = body1->getTransform().getPosition();
         Vector3 x2 = body2->getTransform().getPosition();
@@ -173,46 +169,12 @@ void ContactSolver::initialize(decimal dt) {
                 internalManifold.frictionTwistImpulse = 0.0;
             }
         }
-
-        mNbContactManifolds++;
     }
 
-    // Allocated memory for split impulse velocities
-    // TODO : Use better memory allocation here
-    mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
-    mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
-    assert(mSplitLinearVelocities != NULL);
-    assert(mSplitAngularVelocities != NULL);
-
-    assert(mConstraintBodies.size() > 0);
-    assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
-    assert(mLinearVelocities.size() >= mConstraintBodies.size());
-    assert(mAngularVelocities.size() >= mConstraintBodies.size());
-
-    // Initialize the split impulse velocities
-    initializeSplitImpulseVelocities();
-
     // Fill-in all the matrices needed to solve the LCP problem
     initializeContactConstraints();
 }
 
-// Initialize the split impulse velocities
-void ContactSolver::initializeSplitImpulseVelocities() {
-
-    // For each current body that is implied in some constraint
-    set<RigidBody*>::iterator it;
-    for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
-        RigidBody* rigidBody = *it;
-        assert(rigidBody);
-
-        uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
-
-        // Initialize the split impulse velocities to zero
-        mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
-        mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
-    }
-}
-
 // Initialize the contact constraints before solving the system
 void ContactSolver::initializeContactConstraints() {
     
@@ -304,10 +266,10 @@ void ContactSolver::initializeContactConstraints() {
             // Compute the restitution velocity bias "b". We compute this here instead
             // of inside the solve() method because we need to use the velocity difference
             // at the beginning of the contact. Note that if it is a resting contact (normal
-            // velocity under a given threshold), we don't add a restitution velocity bias
+            // velocity bellow a given threshold), we do not add a restitution velocity bias
             contactPoint.restitutionBias = 0.0;
             decimal deltaVDotN = deltaV.dot(contactPoint.normal);
-            if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) {
+            if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) {
                 contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
             }
 
@@ -884,18 +846,8 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
 // Clean up the constraint solver
 void ContactSolver::cleanup() {
 
-    mConstraintBodies.clear();
-
     if (mContactConstraints != NULL) {
         delete[] mContactConstraints;
         mContactConstraints = NULL;
     }
-    if (mSplitLinearVelocities != NULL) {
-        delete[] mSplitLinearVelocities;
-        mSplitLinearVelocities = NULL;
-    }
-    if (mSplitAngularVelocities != NULL) {
-        delete[] mSplitAngularVelocities;
-        mSplitAngularVelocities = NULL;
-    }
 }

src/engine/ContactSolver.h

@@ -31,6 +31,7 @@
 #include "../configuration.h"
 #include "../constraint/Constraint.h"
 #include "ContactManifold.h"
+#include "Island.h"
 #include "Impulse.h"
 #include <map>
 #include <set>
@@ -311,9 +312,6 @@ class ContactSolver {
 
         // -------------------- Attributes -------------------- //
 
-        /// Reference to all the contact manifold of the world
-        std::vector<ContactManifold*>& mContactManifolds;
-
         /// Split linear velocities for the position contact solver (split impulse)
         Vector3* mSplitLinearVelocities;
 
@@ -329,14 +327,11 @@ class ContactSolver {
         /// Number of contact constraints
         uint mNbContactManifolds;
 
-        /// Constrained bodies
+        /// Array of linear velocities
-        std::set<RigidBody*> mConstraintBodies;
+        Vector3* mLinearVelocities;
 
-        /// Reference to the array of linear velocities
+        /// Array of angular velocities
-        std::vector<Vector3>& mLinearVelocities;
+        Vector3* mAngularVelocities;
-
-        /// Reference to the array of angular velocities
-        std::vector<Vector3>& mAngularVelocities;
 
         /// Reference to the map of rigid body to their index in the constrained velocities array
         const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@@ -353,9 +348,6 @@ class ContactSolver {
 
         // -------------------- Methods -------------------- //
 
-        /// Initialize the split impulse velocities
-        void initializeSplitImpulseVelocities();
-
         /// Initialize the contact constraints before solving the system
         void initializeContactConstraints();
 
@@ -403,16 +395,21 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(std::vector<ContactManifold*>& contactManifolds,
+        ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
-                      std::vector<Vector3>& constrainedLinearVelocities,
-                      std::vector<Vector3>& constrainedAngularVelocities,
-                      const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
 
         /// Destructor
         virtual ~ContactSolver();
 
-        /// Initialize the constraint solver
+        /// Initialize the constraint solver for a given island
-        void initialize(decimal dt);
+        void initializeForIsland(decimal dt, Island* island);
+
+        /// Set the split velocities arrays
+        void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
+                                      Vector3* splitAngularVelocities);
+
+        /// Set the constrained velocities arrays
+        void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
+                                            Vector3* constrainedAngularVelocities);
 
         /// Warm start the solver.
         void warmStart();
@@ -424,24 +421,6 @@ class ContactSolver {
         /// Solve the contacts
         void solve();
 
-        /// Return true if the body is in at least one constraint
-        bool isConstrainedBody(RigidBody* body) const;
-
-        /// Return the constrained linear velocity of a body after solving the constraints
-        Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
-
-        /// Return the split linear velocity
-        Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
-
-        /// Return the constrained angular velocity of a body after solving the constraints
-        Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
-
-        /// Return the split angular velocity
-        Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
-
-        /// Clean up the constraint solver
-        void cleanup();
-
         /// Return true if the split impulses position correction technique is used for contacts
         bool isSplitImpulseActive() const;
 
@@ -451,25 +430,27 @@ class ContactSolver {
         /// Activate or deactivate the solving of friction constraints at the center of
         /// the contact manifold instead of solving them at each contact point
         void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
+
+        /// Clean up the constraint solver
+        void cleanup();
 };
 
-// Return true if the body is in at least one constraint
+// Set the split velocities arrays
-inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
+inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
-    return mConstraintBodies.count(body) == 1;
+                                                    Vector3* splitAngularVelocities) {
+    assert(splitLinearVelocities != NULL);
+    assert(splitAngularVelocities != NULL);
+    mSplitLinearVelocities = splitLinearVelocities;
+    mSplitAngularVelocities = splitAngularVelocities;
 }
 
-// Return the split linear velocity
+// Set the constrained velocities arrays
-inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
+inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
-    assert(isConstrainedBody(body));
+                                                          Vector3* constrainedAngularVelocities) {
-    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
+    assert(constrainedLinearVelocities != NULL);
-    return mSplitLinearVelocities[indexBody];
+    assert(constrainedAngularVelocities != NULL);
-}
+    mLinearVelocities = constrainedLinearVelocities;
-
+    mAngularVelocities = constrainedAngularVelocities;
-// Return the split angular velocity
-inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
-    assert(isConstrainedBody(body));
-    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
-    return mSplitAngularVelocities[indexBody];
 }
 
 // Return true if the split impulses position correction technique is used for contacts

src/engine/DynamicsWorld.cpp

@@ -36,15 +36,19 @@ using namespace std;
 
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
-              : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
+              : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
-                mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
-                               mMapBodyToConstrainedVelocityIndex),
+                mContactSolver(mMapBodyToConstrainedVelocityIndex),
-                mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
-                                  mConstrainedPositions, mConstrainedOrientations,
                                   mMapBodyToConstrainedVelocityIndex),
                 mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
-                mIsDeactivationActive(DEACTIVATION_ENABLED) {
+                mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
+                mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0),
+                mIslands(NULL), mNbBodiesCapacity(0),
+                mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
+                mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
+                mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
 
 }
 
@@ -59,8 +63,26 @@ DynamicsWorld::~DynamicsWorld() {
         mMemoryAllocator.release((*it).second, sizeof(OverlappingPair));
     }
 
-    // Free the allocated memory for the constrained velocities
+    // Release the memory allocated for the islands
-    cleanupConstrainedVelocitiesArray();
+    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);
+    }
+
+    // Release the memory allocated for the bodies velocity arrays
+    if (mNbBodiesCapacity > 0) {
+        delete[] mSplitLinearVelocities;
+        delete[] mSplitAngularVelocities;
+        delete[] mConstrainedLinearVelocities;
+        delete[] mConstrainedAngularVelocities;
+    }
 
 #ifdef IS_PROFILING_ACTIVE
 
@@ -93,10 +115,16 @@ 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();
 
+        // Compute the islands (separate groups of bodies with constraints between each others)
+        computeIslands();
+
         // Integrate the velocities
         integrateRigidBodiesVelocities();
 
@@ -115,14 +143,10 @@ void DynamicsWorld::update() {
         // Solve the position correction for constraints
         solvePositionCorrection();
 
+        if (mIsSleepingEnabled) updateSleepingBodies();
+
         // Update the AABBs of the bodies
         updateRigidBodiesAABB();
-
-        // Cleanup of the contact solver
-        mContactSolver.cleanup();
-
-        // Cleanup the constrained velocities
-        cleanupConstrainedVelocitiesArray();
     }
 
     // Compute and set the interpolation factor to all the bodies
@@ -138,45 +162,47 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
 
     decimal dt = static_cast<decimal>(mTimer.getTimeStep());
     
-    // For each rigid body of the world
+    // For each island of the world
-    set<RigidBody*>::iterator it;
+    for (uint i=0; i < mNbIslands; i++) {
-    for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
 
-        RigidBody* rigidBody = *it;
+        RigidBody** bodies = mIslands[i]->getBodies();
-        assert(rigidBody != NULL);
 
-        // If the body is allowed to move
+        // For each body of the island
-        if (rigidBody->getIsMotionEnabled()) {
+        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
 
-            // Get the constrained velocity
+            // If the body is allowed to move
-            uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
+            if (bodies[b]->getIsMotionEnabled()) {
-            Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
-            Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
 
-            // Update the linear and angular velocity of the body
+                // Get the constrained velocity
-            rigidBody->setLinearVelocity(newLinVelocity);
+                uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-            rigidBody->setAngularVelocity(newAngVelocity);
+                Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
+                Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
 
-            // Add the split impulse velocity from Contact Solver (only used to update the position)
+                // Update the linear and angular velocity of the body
-            if (mContactSolver.isConstrainedBody(rigidBody) &&
+                bodies[b]->setLinearVelocity(newLinVelocity);
-                mContactSolver.isSplitImpulseActive()) {
+                bodies[b]->setAngularVelocity(newAngVelocity);
 
-                newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
+                // Add the split impulse velocity from Contact Solver (only used
-                newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
+                // to update the position)
+                if (mContactSolver.isSplitImpulseActive()) {
+
+                    newLinVelocity += mSplitLinearVelocities[indexArray];
+                    newAngVelocity += mSplitAngularVelocities[indexArray];
+                }
+
+                // Get current position and orientation of the body
+                const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
+                const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
+
+                // Compute the new position of the body
+                Vector3 newPosition = currentPosition + newLinVelocity * dt;
+                Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
+                        currentOrientation * decimal(0.5) * dt;
+
+                // Update the Transform of the body
+                Transform newTransform(newPosition, newOrientation.getUnit());
+                bodies[b]->setTransform(newTransform);
             }
-
-            // Get current position and orientation of the body
-            const Vector3& currentPosition = rigidBody->getTransform().getPosition();
-            const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
-
-            // Compute the new position of the body
-            Vector3 newPosition = currentPosition + newLinVelocity * dt;
-            Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
-                                        currentOrientation * decimal(0.5) * dt;
-
-            // Update the Transform of the body
-            Transform newTransform(newPosition, newOrientation.getUnit());
-            rigidBody->setTransform(newTransform);
         }
     }
 }
@@ -188,7 +214,7 @@ void DynamicsWorld::updateRigidBodiesAABB() {
 
     // For each rigid body of the world
     set<RigidBody*>::iterator it;
-    for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
+    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
         // If the body has moved
         if ((*it)->getHasMoved()) {
@@ -210,12 +236,49 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
 
     // Set the factor to all bodies
     set<RigidBody*>::iterator it;
-    for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
+    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
-        RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
+        (*it)->setInterpolationFactor(factor);
-        assert(rigidBody);
+    }
+}
 
-        rigidBody->setInterpolationFactor(factor);
+// Initialize the bodies velocities arrays for the next simulation step.
+void DynamicsWorld::initVelocityArrays() {
+
+    // Allocate memory for the bodies velocity arrays
+    uint nbBodies = mRigidBodies.size();
+    if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
+        if (mNbBodiesCapacity > 0) {
+            delete[] mSplitLinearVelocities;
+            delete[] mSplitAngularVelocities;
+        }
+        mNbBodiesCapacity = nbBodies;
+        mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
+        mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
+        mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
+        mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
+        assert(mSplitLinearVelocities != NULL);
+        assert(mSplitAngularVelocities != NULL);
+        assert(mConstrainedLinearVelocities != NULL);
+        assert(mConstrainedAngularVelocities != NULL);
+    }
+
+    // Reset the velocities arrays
+    for (uint i=0; i<mNbBodiesCapacity; i++) {
+        mSplitLinearVelocities[i].setToZero();
+        mSplitAngularVelocities[i].setToZero();
+    }
+
+    // Initialize the map of body indexes in the velocity arrays
+    mMapBodyToConstrainedVelocityIndex.clear();
+    std::set<RigidBody*>::const_iterator it;
+    uint indexBody = 0;
+    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+
+        // Add the body into the map
+        mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*,
+                                                  uint>(*it, indexBody));
+        indexBody++;
     }
 }
 
@@ -228,66 +291,74 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
 
     PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
 
-    // TODO : Use better memory allocation here
+    // Initialize the bodies velocity arrays
-    mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
+    initVelocityArrays();
-    mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
 
     decimal dt = static_cast<decimal>(mTimer.getTimeStep());
 
-    // Fill in the mapping of rigid body to their index in the constrained
+    // For each island of the world
-    // velocities arrays
+    for (uint i=0; i < mNbIslands; i++) {
-    uint i = 0;
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-        RigidBody* rigidBody = *it;
-        mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
 
-        // If the body is allowed to move
+        RigidBody** bodies = mIslands[i]->getBodies();
-        if (rigidBody->getIsMotionEnabled()) {
 
-            // Integrate the external force to get the new velocity of the body
+        // For each body of the island
-            mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() +
+        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
-                    dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
-            mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() +
-                    dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
 
-            // If the gravity has to be applied to this rigid body
+            // Insert the body into the map of constrained velocities
-            if (rigidBody->isGravityEnabled() && mIsGravityOn) {
+            uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
 
-                // Integrate the gravity force
+            assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
-                mConstrainedLinearVelocities[i] += dt * rigidBody->getMassInverse() *
+            assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
-                                                   rigidBody->getMass() * mGravity;
+
+            // If the body is allowed to move
+            if (bodies[b]->getIsMotionEnabled()) {
+
+                // Integrate the external force to get the new velocity of the body
+                mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
+                        dt * bodies[b]->getMassInverse() * bodies[b]->getExternalForce();
+                mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
+                        dt * bodies[b]->getInertiaTensorInverseWorld() *
+                        bodies[b]->getExternalTorque();
+
+                // If the gravity has to be applied to this rigid body
+                if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) {
+
+                    // Integrate the gravity force
+                    mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->getMassInverse() *
+                            bodies[b]->getMass() * mGravity;
+                }
+
+                // Apply the velocity damping
+                // Damping force : F_c = -c' * v (c=damping factor)
+                // Equation      : m * dv/dt = -c' * v
+                //                 => dv/dt = -c * v (with c=c'/m)
+                //                 => dv/dt + c * v = 0
+                // Solution      : v(t) = v0 * e^(-c * t)
+                //                 => v(t + dt) = v0 * e^(-c(t + dt))
+                //                              = v0 * e^(-ct) * e^(-c * dt)
+                //                              = v(t) * e^(-c * dt)
+                //                 => v2 = v1 * e^(-c * dt)
+                // 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 angDampingFactor = bodies[b]->getAngularDamping();
+                decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
+                                              decimal(0.0), decimal(1.0));
+                decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
+                                               decimal(0.0), decimal(1.0));
+                mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0),
+                                                                 decimal(1.0));
+                mConstrainedAngularVelocities[indexBody] *= clamp(angularDamping, decimal(0.0),
+                                                                  decimal(1.0));
+
+                // Update the old Transform of the body
+                bodies[b]->updateOldTransform();
             }
 
-            // Apply the velocity damping
+            indexBody++;
-            // Damping force : F_c = -c' * v (c=damping factor)
-            // Equation      : m * dv/dt = -c' * v
-            //                 => dv/dt = -c * v (with c=c'/m)
-            //                 => dv/dt + c * v = 0
-            // Solution      : v(t) = v0 * e^(-c * t)
-            //                 => v(t + dt) = v0 * e^(-c(t + dt))
-            //                              = v0 * e^(-ct) * e^(-c * dt)
-            //                              = v(t) * e^(-c * dt)
-            //                 => v2 = v1 * e^(-c * dt)
-            // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
-            //                              => e^(-x) ~ 1 - x
-            //                 => v2 = v1 * (1 - c * dt)
-            decimal linDampingFactor = rigidBody->getLinearDamping();
-            decimal angDampingFactor = rigidBody->getAngularDamping();
-            decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor,
-                                          decimal(0.0), decimal(1.0));
-            decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor,
-                                          decimal(0.0), decimal(1.0));
-            mConstrainedLinearVelocities[i] *= clamp(linearDamping, decimal(0.0), decimal(1.0));
-            mConstrainedAngularVelocities[i] *= clamp(angularDamping, decimal(0.0), decimal(1.0));
-
-            // Update the old Transform of the body
-            rigidBody->updateOldTransform();
         }
-
-        i++;
     }
-
-    assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size());
 }
 
 // Solve the contacts and constraints
@@ -298,42 +369,59 @@ void DynamicsWorld::solveContactsAndConstraints() {
     // Get the current time step
     decimal dt = static_cast<decimal>(mTimer.getTimeStep());
 
-    // Check if there are contacts and constraints to solve
+    // Set the velocities arrays
-    bool isConstraintsToSolve = !mJoints.empty();
+    mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
-    bool isContactsToSolve = !mContactManifolds.empty();
+    mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
-    if (!isConstraintsToSolve && !isContactsToSolve) return;
+                                                  mConstrainedAngularVelocities);
+    mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
+                                                     mConstrainedAngularVelocities);
 
     // ---------- Solve velocity constraints for joints and contacts ---------- //
 
-    // If there are contacts
+    // For each island of the world
-    if (isContactsToSolve) {
+    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
-        // Initialize the solver
+        // Check if there are contacts and constraints to solve
-        mContactSolver.initialize(dt);
+        bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
+        bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
+        if (!isConstraintsToSolve && !isContactsToSolve) continue;
 
-        // Warm start the contact solver
+        // If there are contacts in the current island
-        mContactSolver.warmStart();
+        if (isContactsToSolve) {
+
+            // Initialize the solver
+            mContactSolver.initializeForIsland(dt, mIslands[islandIndex]);
+
+            // Warm start the contact solver
+            mContactSolver.warmStart();
+        }
+
+        // If there are constraints
+        if (isConstraintsToSolve) {
+
+            // Initialize the constraint solver
+            mConstraintSolver.initializeForIsland(dt, mIslands[islandIndex]);
+        }
+
+        // For each iteration of the velocity solver
+        for (uint i=0; i<mNbVelocitySolverIterations; i++) {
+
+            // Solve the constraints
+            if (isConstraintsToSolve) {
+                mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
+            }
+
+            // Solve the contacts
+            if (isContactsToSolve) mContactSolver.solve();
+        }        
+
+        // Cache the lambda values in order to use them in the next
+        // step and cleanup the contact solver
+        if (isContactsToSolve) {
+            mContactSolver.storeImpulses();
+            mContactSolver.cleanup();
+        }
     }
-
-    // If there are constraints
-    if (isConstraintsToSolve) {
-
-        // Initialize the constraint solver
-        mConstraintSolver.initialize(dt);
-    }
-
-    // For each iteration of the velocity solver
-    for (uint i=0; i<mNbVelocitySolverIterations; i++) {
-
-        // Solve the constraints
-        if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
-
-        // Solve the contacts
-        if (isContactsToSolve) mContactSolver.solve();
-    }
-
-    // Cache the lambda values in order to use them in the next step
-    if (isContactsToSolve) mContactSolver.storeImpulses();
 }
 
 // Solve the position error correction of the constraints
@@ -349,37 +437,36 @@ void DynamicsWorld::solvePositionCorrection() {
     // TODO : Use better memory allocation here
     mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
     mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
 
-        // If it is a constrained bodies (by a joint)
+    // For each island of the world
-        if (mConstraintSolver.isConstrainedBody(*it)) {
+    for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
 
-            uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
+        // For each body of the island
+        RigidBody** bodies = mIslands[islandIndex]->getBodies();
+        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
+
+            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
 
             // Get the position/orientation of the rigid body
-            const Transform& transform = (*it)->getTransform();
+            const Transform& transform = bodies[b]->getTransform();
             mConstrainedPositions[index] = transform.getPosition();
             mConstrainedOrientations[index]= transform.getOrientation();
         }
-    }
 
-    // ---------- Solve the position error correction for the constraints ---------- //
+        // ---------- Solve the position error correction for the constraints ---------- //
 
-    // For each iteration of the position (error correction) solver
+        // For each iteration of the position (error correction) solver
-    for (uint i=0; i<mNbPositionSolverIterations; i++) {
+        for (uint i=0; i<mNbPositionSolverIterations; i++) {
 
-        // Solve the position constraints
+            // Solve the position constraints
-        mConstraintSolver.solvePositionConstraints();
+            mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
-    }
+        }
 
-    // ---------- Update the position/orientation of the rigid bodies ---------- //
+        // ---------- Update the position/orientation of the rigid bodies ---------- //
 
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+        for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
 
-        // If it is a constrained bodies (by a joint)
+            uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-        if (mConstraintSolver.isConstrainedBody(*it)) {
-
-            uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
 
             // Get the new position/orientation of the body
             const Vector3& newPosition = mConstrainedPositions[index];
@@ -387,22 +474,11 @@ void DynamicsWorld::solvePositionCorrection() {
 
             // Update the Transform of the body
             Transform newTransform(newPosition, newOrientation.getUnit());
-            (*it)->setTransform(newTransform);
+            bodies[b]->setTransform(newTransform);
         }
     }
 }
 
-// Cleanup the constrained velocities array at each step
-void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
-
-    // Clear the constrained velocites
-    mConstrainedLinearVelocities.clear();
-    mConstrainedAngularVelocities.clear();
-
-    // Clear the rigid body to velocities array index mapping
-    mMapBodyToConstrainedVelocityIndex.clear();
-}
-
 // Create a rigid body into the physics world
 RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
                                           const Matrix3x3& inertiaTensorLocal,
@@ -448,7 +524,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 +533,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 +609,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;
 }
@@ -545,10 +628,17 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
         mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2());
     }
 
+    // Wake up the two bodies of the joint
+    joint->getBody1()->setIsSleeping(false);
+    joint->getBody2()->setIsSleeping(false);
+
     // 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
+    joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint);
+    joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint);
+
     size_t nbBytes = joint->getSizeInBytes();
 
     // Call the destructor of the joint
@@ -558,6 +648,281 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
     mMemoryAllocator.release(joint, nbBytes);
 }
 
+// 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);
+}
+
+// Put bodies to sleep if needed.
+/// For each island, if all the bodies have been almost still for a long enough period of
+/// time, we put all the bodies of the island to sleep.
+void DynamicsWorld::updateSleepingBodies() {
+
+    PROFILE("DynamicsWorld::updateSleepingBodies()");
+
+    const decimal dt = static_cast<decimal>(mTimer.getTimeStep());
+    const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
+    const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
+
+    // For each island of the world
+    for (uint i=0; i<mNbIslands; i++) {
+
+        decimal minSleepTime = DECIMAL_LARGEST;
+
+        // For each body of the island
+        RigidBody** bodies = mIslands[i]->getBodies();
+        for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+
+            // Skip static bodies
+            if (!bodies[b]->getIsMotionEnabled()) continue;
+
+            // If the body is velocity is large enough to stay awake
+            if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare ||
+                bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare ||
+                !bodies[b]->isAllowedToSleep()) {
+
+                // Reset the sleep time of the body
+                bodies[b]->mSleepTime = decimal(0.0);
+                minSleepTime = decimal(0.0);
+            }
+            else {  // If the body velocity is bellow the sleeping velocity threshold
+
+                // Increase the sleep time
+                bodies[b]->mSleepTime += dt;
+                if (bodies[b]->mSleepTime < minSleepTime) {
+                    minSleepTime = bodies[b]->mSleepTime;
+                }
+            }
+        }
+
+        // If the velocity of all the bodies of the island is under the
+        // sleeping velocity threshold for a period of time larger than
+        // the time required to become a sleeping body
+        if (minSleepTime >= mTimeBeforeSleep) {
+
+            // Put all the bodies of the island to sleep
+            for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
+                bodies[b]->setIsSleeping(true);
+            }
+        }
+    }
+}
+
 // Notify the world about a new broad-phase overlapping pair
 void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
 
@@ -565,8 +930,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 +969,25 @@ 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);
+}
+
+// Enable/Disable the sleeping technique
+void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
+    mIsSleepingEnabled = isSleepingEnabled;
+
+    if (!mIsSleepingEnabled) {
+
+        // For each body of the world
+        std::set<RigidBody*>::iterator it;
+        for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+
+            // Wake up the rigid body
+            (*it)->setIsSleeping(false);
+        }
+    }
 }

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
@@ -81,15 +83,21 @@ class DynamicsWorld : public CollisionWorld {
         Vector3 mGravity;
 
         /// True if the gravity force is on
-        bool mIsGravityOn;
+        bool mIsGravityEnabled;
 
         /// Array of constrained linear velocities (state of the linear velocities
         /// after solving the constraints)
-        std::vector<Vector3> mConstrainedLinearVelocities;
+        Vector3* mConstrainedLinearVelocities;
 
         /// Array of constrained angular velocities (state of the angular velocities
         /// after solving the constraints)
-        std::vector<Vector3> mConstrainedAngularVelocities;
+        Vector3* mConstrainedAngularVelocities;
+
+        /// Split linear velocities for the position contact solver (split impulse)
+        Vector3* mSplitLinearVelocities;
+
+        /// Split angular velocities for the position contact solver (split impulse)
+        Vector3* mSplitAngularVelocities;
 
         /// Array of constrained rigid bodies position (for position error correction)
         std::vector<Vector3> mConstrainedPositions;
@@ -100,6 +108,28 @@ 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;
+
+        /// Current allocated capacity for the bodies
+        uint mNbBodiesCapacity;
+
+        /// Sleep linear velocity threshold
+        decimal mSleepLinearVelocity;
+
+        /// Sleep angular velocity threshold
+        decimal mSleepAngularVelocity;
+
+        /// Time (in seconds) before a body is put to sleep if its velocity
+        /// becomes smaller than the sleep velocity.
+        decimal mTimeBeforeSleep;
+
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -121,6 +151,9 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
+        /// Initialize the bodies velocities arrays for the next simulation step.
+        void initVelocityArrays();
+
         /// Integrate the velocities of rigid bodies.
         void integrateRigidBodiesVelocities();
 
@@ -136,6 +169,12 @@ class DynamicsWorld : public CollisionWorld {
         /// Reset the boolean movement variable of each body
         void resetBodiesMovementVariable();
 
+        /// Compute the islands of awake bodies.
+        void computeIslands();
+
+        /// Put bodies to sleep if needed.
+        void updateSleepingBodies();
+
         /// Update the overlapping pair
         virtual void updateOverlappingPair(const BroadPhasePair* pair);
 
@@ -198,14 +237,25 @@ 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;
 
         /// Return if the gravity is on
-        bool getIsGravityOn() const;
+        bool isGravityEnabled() const;
 
-        /// Set the isGravityOn attribute
+        /// Enable/Disable the gravity
-        void setIsGratityOn(bool isGravityOn);
+        void setIsGratityEnabled(bool isGravityEnabled);
 
         /// Return the number of rigid bodies in the world
         uint getNbRigidBodies() const;
@@ -227,6 +277,36 @@ public :
 
         /// Return a reference to the contact manifolds of the world
         const std::vector<ContactManifold*>& getContactManifolds() const;
+
+        /// Return true if the sleeping technique is enabled
+        bool isSleepingEnabled() const;
+
+        /// Enable/Disable the sleeping technique
+        void enableSleeping(bool isSleepingEnabled);
+
+        /// Return the current sleep linear velocity
+        decimal getSleepLinearVelocity() const;
+
+        /// Set the sleep linear velocity.
+        void setSleepLinearVelocity(decimal sleepLinearVelocity);
+
+        /// Return the current sleep angular velocity
+        decimal getSleepAngularVelocity() const;
+
+        /// Set the sleep angular velocity.
+        void setSleepAngularVelocity(decimal sleepAngularVelocity);
+
+        /// Return the time a body is required to stay still before sleeping
+        decimal getTimeBeforeSleep() const;
+
+        /// Set the time a body is required to stay still before sleeping
+        void setTimeBeforeSleep(decimal timeBeforeSleep);
+
+        // TODO : REMOVE THIS
+        Island** getIslands() { return mIslands;}
+
+        // TODO : REMOVE THIS
+        uint getNbIslands() const {return mNbIslands;}
 };
 
 // Start the physics simulation
@@ -306,14 +386,14 @@ inline Vector3 DynamicsWorld::getGravity() const {
     return mGravity;
 }
 
-// Return if the gravity is on
+// Return if the gravity is enaled
-inline bool DynamicsWorld::getIsGravityOn() const {
+inline bool DynamicsWorld::isGravityEnabled() const {
-    return mIsGravityOn;
+    return mIsGravityEnabled;
 }
 
-// Set the isGravityOn attribute
+// Enable/Disable the gravity
-inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
+inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) {
-    mIsGravityOn = isGravityOn;
+    mIsGravityEnabled = isGravityEnabled;
 }
 
 // Return the number of rigid bodies in the world
@@ -351,6 +431,51 @@ inline long double DynamicsWorld::getPhysicsTime() const {
     return mTimer.getPhysicsTime();
 }
 
+// Return true if the sleeping technique is enabled
+inline bool DynamicsWorld::isSleepingEnabled() const {
+    return mIsSleepingEnabled;
+}
+
+// Return the current sleep linear velocity
+inline decimal DynamicsWorld::getSleepLinearVelocity() const {
+    return mSleepLinearVelocity;
+}
+
+// Set the sleep linear velocity.
+/// When the velocity of a body becomes smaller than the sleep linear/angular
+/// velocity for a given amount of time, the body starts sleeping and does not need
+/// to be simulated anymore.
+inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) {
+    assert(sleepLinearVelocity >= decimal(0.0));
+    mSleepLinearVelocity = sleepLinearVelocity;
+}
+
+// Return the current sleep angular velocity
+inline decimal DynamicsWorld::getSleepAngularVelocity() const {
+    return mSleepAngularVelocity;
+}
+
+// Set the sleep angular velocity.
+/// When the velocity of a body becomes smaller than the sleep linear/angular
+/// velocity for a given amount of time, the body starts sleeping and does not need
+/// to be simulated anymore.
+inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity) {
+    assert(sleepAngularVelocity >= decimal(0.0));
+    mSleepAngularVelocity = sleepAngularVelocity;
+}
+
+// Return the time a body is required to stay still before sleeping
+inline decimal DynamicsWorld::getTimeBeforeSleep() const {
+    return mTimeBeforeSleep;
+}
+
+
+// Set the time a body is required to stay still before sleeping
+inline void DynamicsWorld::setTimeBeforeSleep(decimal timeBeforeSleep) {
+    assert(timeBeforeSleep >= decimal(0.0));
+    mTimeBeforeSleep = timeBeforeSleep;
+}
+
 }
 
 #endif

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