Continue to implement the sleeping technique

src/body/Body.cpp

@@ -32,7 +32,8 @@ using namespace reactphysics3d;
 
 // Constructor
 Body::Body(bodyindex id)
-     : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false) {
+     : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false),
+       mSleepTime(0) {
 
 }
 

src/body/Body.h

@@ -56,6 +56,9 @@ class Body {
         /// 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
@@ -93,7 +96,7 @@ class Body {
         bool isSleeping() const;
 
         /// Set the variable to know whether or not the body is sleeping
-        void setIsSleeping(bool isSleeping);
+        virtual void setIsSleeping(bool isSleeping);
 
         /// Smaller than operator
         bool operator<(const Body& body2) const;
@@ -135,6 +138,8 @@ inline bool Body::isAllowedToSleep() const {
 // 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
@@ -144,6 +149,16 @@ inline bool Body::isSleeping() const {
 
 // 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;
 }
 

src/body/RigidBody.h

@@ -194,6 +194,9 @@ class RigidBody : public CollisionBody {
         /// 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;
@@ -351,6 +354,19 @@ 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,7 +92,7 @@ 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 spleeping technique for inactive bodies is enabled
+/// True if the spleeping technique is enabled
 const bool SPLEEPING_ENABLED = true;
 
 /// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
@@ -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/HingeJoint.cpp

@@ -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/DynamicsWorld.cpp

@@ -36,7 +36,7 @@ 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),
                 mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
                 mContactSolver(mMapBodyToConstrainedVelocityIndex),
                 mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
@@ -45,7 +45,10 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
                 mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
                 mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
                 mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0),
-                mIslands(NULL), mNbBodiesCapacity(0) {
+                mIslands(NULL), mNbBodiesCapacity(0),
+                mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
+                mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
+                mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
 
 }
 
@@ -119,6 +122,9 @@ void DynamicsWorld::update() {
         // Compute the collision detection
         mCollisionDetection.computeCollisionDetection();
 
+        // Compute the islands (separate groups of bodies with constraints between each others)
+        computeIslands();
+
         // Integrate the velocities
         integrateRigidBodiesVelocities();
 
@@ -128,9 +134,6 @@ void DynamicsWorld::update() {
         // Update the timer
         mTimer.nextStep();
 
-        // Compute the islands (separate groups of bodies with constraints between each others)
-        computeIslands();
-
         // Solve the contacts and constraints
         solveContactsAndConstraints();
 
@@ -140,6 +143,8 @@ void DynamicsWorld::update() {
         // Solve the position correction for constraints
         solvePositionCorrection();
 
+        if (mIsSleepingEnabled) updateSleepingBodies();
+
         // Update the AABBs of the bodies
         updateRigidBodiesAABB();
     }
@@ -157,44 +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.isSplitImpulseActive()) {
+                bodies[b]->setLinearVelocity(newLinVelocity);
+                bodies[b]->setAngularVelocity(newAngVelocity);
 
-                newLinVelocity += mSplitLinearVelocities[indexArray];
+                // Add the split impulse velocity from Contact Solver (only used
-                newAngVelocity += mSplitAngularVelocities[indexArray];
+                // 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);
         }
     }
 }
@@ -206,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()) {
@@ -228,12 +236,9 @@ 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);
     }
 }
 
@@ -263,6 +268,18 @@ void DynamicsWorld::initVelocityArrays() {
         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++;
+    }
 }
 
 // Integrate the velocities of rigid bodies.
@@ -279,64 +296,69 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
 
     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;
-    mMapBodyToConstrainedVelocityIndex.clear();
-    for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
-        RigidBody* rigidBody = *it;
-        mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
 
-        assert(mSplitLinearVelocities[i] == Vector3(0, 0, 0));
+        RigidBody** bodies = mIslands[i]->getBodies();
-        assert(mSplitAngularVelocities[i] == Vector3(0, 0, 0));
 
-        // 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++) {
 
-            // Integrate the external force to get the new velocity of the body
+            // Insert the body into the map of constrained velocities
-            mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() +
+            uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
-                    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
+            assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
-            if (rigidBody->isGravityEnabled() && mIsGravityOn) {
+            assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
 
-                // Integrate the gravity force
+            // If the body is allowed to move
-                mConstrainedLinearVelocities[i] += dt * rigidBody->getMassInverse() *
+            if (bodies[b]->getIsMotionEnabled()) {
-                                                   rigidBody->getMass() * mGravity;
+
+                // 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
@@ -606,6 +628,10 @@ 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);
 
@@ -842,6 +868,61 @@ void DynamicsWorld::computeIslands() {
     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) {
 
@@ -894,3 +975,19 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
     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

@@ -83,7 +83,7 @@ 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)
@@ -120,6 +120,16 @@ class DynamicsWorld : public CollisionWorld {
         /// 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
@@ -162,6 +172,9 @@ class DynamicsWorld : public CollisionWorld {
         /// 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);
 
@@ -239,10 +252,10 @@ public :
         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;
@@ -265,6 +278,30 @@ 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;}
 
@@ -349,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
@@ -394,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