Move the constrained velocities outside the contact solver

src/engine/ContactSolver.cpp

@@ -37,10 +37,17 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
 const decimal ContactSolver::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(DynamicsWorld& world)
+ContactSolver::ContactSolver(DynamicsWorld& world, std::vector<Vector3>& constrainedLinearVelocities,
-    :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mContactConstraints(0),
+                             std::vector<Vector3>& constrainedAngularVelocities,
-      mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true),
+                             const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
-      mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) {
+              :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
+               mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL),
+               mConstrainedLinearVelocities(constrainedLinearVelocities),
+               mConstrainedAngularVelocities(constrainedAngularVelocities),
+               mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
+               mIsWarmStartingActive(true),
+               mIsSplitImpulseActive(true),
+               mIsSolveFrictionAtContactManifoldCenterActive(true) {
 
 }
 
@@ -72,10 +79,6 @@ void ContactSolver::initialize() {
         RigidBody* body1 = externalContactManifold.contacts[0]->getBody1();
         RigidBody* body2 = externalContactManifold.contacts[0]->getBody2();
 
-        // Fill in the body number maping
-        mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
-        mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
-
         // Add the two bodies of the constraint in the constraintBodies list
         mConstraintBodies.insert(body1);
         mConstraintBodies.insert(body2);
@@ -84,8 +87,8 @@ void ContactSolver::initialize() {
         Vector3 x1 = body1->getTransform().getPosition();
         Vector3 x2 = body2->getTransform().getPosition();
 
-        internalContactManifold.indexBody1 = mMapBodyToIndex[body1];
+        internalContactManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
-        internalContactManifold.indexBody2 = mMapBodyToIndex[body2];
+        internalContactManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
         internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
         internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
         internalContactManifold.isBody1Moving = body1->getIsMotionEnabled();
@@ -165,17 +168,17 @@ void ContactSolver::initialize() {
         mNbContactConstraints++;
     }
 
-    // Compute the number of bodies that are part of some active constraint
+    // Allocated memory for split impulse velocities
-    uint nbBodies = mConstraintBodies.size();
-
-    // Allocated memory for velocities
     // TODO : Use better memory allocation here
-    mLinearVelocities = new Vector3[nbBodies];
+    mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
-    mAngularVelocities = new Vector3[nbBodies];
+    mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
-    mSplitLinearVelocities = new Vector3[nbBodies];
+    assert(mSplitLinearVelocities != NULL);
-    mSplitAngularVelocities = new Vector3[nbBodies];
+    assert(mSplitAngularVelocities != NULL);
 
-    assert(mMapBodyToIndex.size() == nbBodies);
+    assert(mConstraintBodies.size() > 0);
+    assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
+    assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
+    assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
 }
 
 // Initialize the constrained bodies
@@ -187,10 +190,8 @@ void ContactSolver::initializeBodies() {
         RigidBody* rigidBody = *it;
         assert(rigidBody);
 
-        uint bodyNumber = mMapBodyToIndex[rigidBody];
+        uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
 
-        mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
-        mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
         mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
         mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
     }
@@ -214,10 +215,10 @@ void ContactSolver::initializeContactConstraints() {
         }
 
         // Get the velocities of the bodies
-        const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
+        const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
-        const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
+        const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
-        const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
+        const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
-        const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
+        const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
 
         // For each contact point constraint
         for (uint i=0; i<contactManifold.nbContacts; i++) {
@@ -474,10 +475,10 @@ void ContactSolver::solveContactConstraints() {
 
             decimal sumPenetrationImpulse = 0.0;
 
-            const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
+            const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
-            const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
+            const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
-            const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
+            const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
-            const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
+            const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
 
             for (uint i=0; i<contactManifold.nbContacts; i++) {
 
@@ -721,15 +722,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSo
 
     // Update the velocities of the bodies by applying the impulse P
     if (contactManifold.isBody1Moving) {
-        mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
+        mConstrainedLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
                                                     impulse.linearImpulseBody1;
-        mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
+        mConstrainedAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
                                                      impulse.angularImpulseBody1;
     }
     if (contactManifold.isBody2Moving) {
-        mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
+        mConstrainedLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
                                                     impulse.linearImpulseBody2;
-        mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
+        mConstrainedAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
                                                      impulse.angularImpulseBody2;
     }
 }
@@ -758,10 +759,6 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse,
 void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
                                            ContactPointSolver& contactPoint) const {
 
-    // Update the old friction vectors
-    //contact.oldFrictionVector1 = contact.frictionVector1;
-    //contact.oldFrictionVector2 = contact.frictionVector2;
-
     assert(contactPoint.normal.length() > 0.0);
 
     // Compute the velocity difference vector in the tangential plane
@@ -819,19 +816,19 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
 
 // Clean up the constraint solver
 void ContactSolver::cleanup() {
-    mMapBodyToIndex.clear();
+    //mMapBodyToIndex.clear();
     mConstraintBodies.clear();
 
-    if (mContactConstraints != 0) {
+    if (mContactConstraints != NULL) {
         delete[] mContactConstraints;
-        mContactConstraints = 0;
+        mContactConstraints = NULL;
     }
-    if (mLinearVelocities != 0) {
+    if (mSplitLinearVelocities != NULL) {
-        delete[] mLinearVelocities;
+        delete[] mSplitLinearVelocities;
-        mLinearVelocities = 0;
+        mSplitLinearVelocities = NULL;
     }
-    if (mAngularVelocities != 0) {
+    if (mSplitAngularVelocities != NULL) {
-        delete[] mAngularVelocities;
+        delete[] mSplitAngularVelocities;
-        mAngularVelocities = 0;
+        mSplitAngularVelocities = NULL;
     }
 }

src/engine/ContactSolver.h

@@ -62,7 +62,7 @@ struct Impulse {
     
 /*  -------------------------------------------------------------------
     Class ContactSolver :
-        This class represents the contacts solver that is used is solve rigid bodies contacts.
+        This class represents the contact solver that is used is solve rigid bodies contacts.
         The constraint solver is based on the "Sequential Impulse" technique described by
         Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
 
@@ -228,12 +228,6 @@ class ContactSolver {
         // Number of iterations of the constraints solver
         uint mNbIterations;
 
-        // Array of constrained linear velocities
-        Vector3* mLinearVelocities;
-
-        // Array of constrained angular velocities
-        Vector3* mAngularVelocities;
-
         // Split linear velocities for the position contact solver (split impulse)
         Vector3* mSplitLinearVelocities;
 
@@ -252,8 +246,16 @@ class ContactSolver {
         // Constrained bodies
         std::set<RigidBody*> mConstraintBodies;
 
-        // Map body to index
+        // Pointer to the array of constrained linear velocities (state of the linear velocities
-        std::map<RigidBody*, uint> mMapBodyToIndex;
+        // after solving the constraints)
+        std::vector<Vector3>& mConstrainedLinearVelocities;
+
+        // Pointer to the array of constrained angular velocities (state of the angular velocities
+        // after solving the constraints)
+        std::vector<Vector3>& mConstrainedAngularVelocities;
+
+        // Reference to the map of rigid body to their index in the constrained velocities array
+        const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
 
         // True if the warm starting of the solver is active
         bool mIsWarmStartingActive;
@@ -327,7 +329,9 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         // Constructor
-        ContactSolver(DynamicsWorld& mWorld);
+        ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
+                      std::vector<Vector3>& constrainedAngularVelocities, const std::map<RigidBody*, uint>&
+                      mapBodyToVelocityIndex);
 
         // Destructor
         virtual ~ContactSolver();
@@ -369,31 +373,17 @@ inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
     return mConstraintBodies.count(body) == 1;
 }
 
-// Return the constrained linear velocity of a body after solving the constraints
-inline Vector3 ContactSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
-    assert(isConstrainedBody(body));
-    uint indexBody = mMapBodyToIndex[body];
-    return mLinearVelocities[indexBody];
-}
-
 // Return the split linear velocity
 inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
     assert(isConstrainedBody(body));
-    uint indexBody = mMapBodyToIndex[body];
+    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
     return mSplitLinearVelocities[indexBody];
 }
 
-// Return the constrained angular velocity of a body after solving the constraints
-inline Vector3 ContactSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
-    assert(isConstrainedBody(body));
-    uint indexBody = mMapBodyToIndex[body];
-    return mAngularVelocities[indexBody];
-}
-
 // Return the split angular velocity
 inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
     assert(isConstrainedBody(body));
-    uint indexBody = mMapBodyToIndex[body];
+    const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
     return mSplitAngularVelocities[indexBody];
 }
 
@@ -454,6 +444,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
                    contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda);
 }
 
-} // End of ReactPhysics3D namespace
+}
 
 #endif

src/engine/DynamicsWorld.cpp

@@ -26,13 +26,15 @@
 // Libraries
 #include "DynamicsWorld.h"
 
-// We want to use the ReactPhysics3D namespace
+// Namespaces
 using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
 DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
-              : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mContactSolver(*this),
+              : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
+                mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
+                               mMapBodyToConstrainedVelocityIndex),
                 mIsDeactivationActive(DEACTIVATION_ENABLED) {
 
 }
@@ -46,6 +48,9 @@ DynamicsWorld::~DynamicsWorld() {
         (*it).second->OverlappingPair::~OverlappingPair();
         mMemoryPoolOverlappingPairs.freeObject((*it).second);
     }
+
+    // Free the allocated memory for the constrained velocities
+    cleanupConstrainedVelocitiesArray();
 }
 
 // Update the physics simulation
@@ -68,6 +73,9 @@ void DynamicsWorld::update() {
         // Compute the collision detection
         mCollisionDetection.computeCollisionDetection();
 
+        // Initialize the constrained velocities
+        initConstrainedVelocitiesArray();
+
         // If there are contacts
         if (!mContactManifolds.empty()) {
 
@@ -82,60 +90,61 @@ void DynamicsWorld::update() {
         resetBodiesMovementVariable();
 
         // Update the position and orientation of each body
-        updateAllBodiesMotion();
+        updateRigidBodiesPositionAndOrientation();
 
         // Cleanup of the contact solver
         mContactSolver.cleanup();
+
+        // Cleanup the constrained velocities
+        cleanupConstrainedVelocitiesArray();
     }
 
     // Compute and set the interpolation factor to all the bodies
     setInterpolationFactorToAllBodies();
 }
 
-// Compute the motion of all bodies and update their positions and orientations
+// Update the position and orientation of the rigid bodies
-// First this method compute the vector V2 = V_constraint + V_forces + V1 where
+void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
-// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext)
-// V2 is the final velocity after the timestep and V1 is the velocity before the
-// timestep.
-// After having computed the velocity V2, this method will update the position
-// and orientation of each body.
-// This method uses the semi-implicit Euler method to update the position and
-// orientation of the body
-void DynamicsWorld::updateAllBodiesMotion() {
     decimal dt = mTimer.getTimeStep();
-    Vector3 newLinearVelocity;
-    Vector3 newAngularVelocity;
     
-    // For each body of thephysics world
+    // For each rigid body of the world
     for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
 
         RigidBody* rigidBody = *it;
         assert(rigidBody);
 
-        // If the body is able to move
+        // If the body is allowed to move
         if (rigidBody->getIsMotionEnabled()) {
-            newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
-            newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
 
-            // If it's a constrained body
+            // Update the old Transform of the body
-            if (mContactSolver.isConstrainedBody(*it)) {
+            rigidBody->updateOldTransform();
-                // Get the constrained linear and angular velocities from the constraint solver
-                newLinearVelocity = mContactSolver.getConstrainedLinearVelocityOfBody(rigidBody);
-                newAngularVelocity = mContactSolver.getConstrainedAngularVelocityOfBody(rigidBody);
-            }
-            else {
-                // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
-                // external forces and torques.
-                newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
-                newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
 
-                // Add the velocity V1 to the new velocity
+            // Get the constrained velocity
-                newLinearVelocity += rigidBody->getLinearVelocity();
+            uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody];
-                newAngularVelocity += rigidBody->getAngularVelocity();
+            Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
+            Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
+
+            // Update the linear and angular velocity of the body
+            rigidBody->setLinearVelocity(newLinVelocity);
+            rigidBody->setAngularVelocity(newAngVelocity);
+
+            // Add the split impulse velocity from Contact Solver (only used to update the position)
+            if (mContactSolver.isConstrainedBody(rigidBody)) {
+                newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
+                newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
             }
-            
+
-            // Update the position and the orientation of the body according to the new velocity
+            // Get current position and orientation of the body
-            updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
+            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(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
+
+            // Update the Transform of the body
+            Transform newTransform(newPosition, newOrientation.getUnit());
+            rigidBody->setTransform(newTransform);
 
             // Update the AABB of the rigid body
             rigidBody->updateAABB();
@@ -143,40 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
     }
 }
 
-// Update the position and orientation of a body
-// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
-// orientation of the body
-void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
-                                                       Vector3 newLinVelocity,
-                                                       Vector3 newAngVelocity) {
-    decimal dt = mTimer.getTimeStep();
-
-    assert(rigidBody);
-
-    // Update the old position and orientation of the body
-    rigidBody->updateOldTransform();
-
-    // Update the linear and angular velocity of the body
-    rigidBody->setLinearVelocity(newLinVelocity);
-    rigidBody->setAngularVelocity(newAngVelocity);
-
-    // Split velocity (only used to update the position)
-    if (mContactSolver.isConstrainedBody(rigidBody)) {
-        newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
-        newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
-    }
-    
-    // Get current position and orientation of the body
-    const Vector3& currentPosition = rigidBody->getTransform().getPosition();
-    const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
-            
-    Vector3 newPosition = currentPosition + newLinVelocity * dt;
-    Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
-
-    Transform newTransform(newPosition, newOrientation.getUnit());
-    rigidBody->setTransform(newTransform);
-}
-
 // Compute and set the interpolation factor to all bodies
 void DynamicsWorld::setInterpolationFactorToAllBodies() {
     
@@ -194,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
     }
 }
 
+// Initialize the constrained velocities array at each step
+void DynamicsWorld::initConstrainedVelocitiesArray() {
+
+    // TODO : Use better memory allocation here
+    mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
+    mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
+
+    double dt = mTimer.getTimeStep();
+
+    // Fill in the mapping of rigid body to their index in the constrained
+    // velocities arrays
+    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));
+
+        // TODO : Move it somewhere else
+        mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce();
+        mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
+
+        i++;
+    }
+}
+
+// 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();
+}
+
 // Apply the gravity force to all bodies of the physics world
 void DynamicsWorld::applyGravity() {
 

src/engine/DynamicsWorld.h

@@ -84,6 +84,17 @@ class DynamicsWorld : public CollisionWorld {
         // Memory pool for the contacts
         MemoryPool<Contact> mMemoryPoolContacts;
 
+        // Array of constrained linear velocities (state of the linear velocities
+        // after solving the constraints)
+        std::vector<Vector3> mConstrainedLinearVelocities;
+
+        // Array of constrained angular velocities (state of the angular velocities
+        // after solving the constraints)
+        std::vector<Vector3> mConstrainedAngularVelocities;
+
+        // Map body to their index in the constrained velocities array
+        std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
+
         // -------------------- Methods -------------------- //
 
         // Private copy-constructor
@@ -93,7 +104,7 @@ class DynamicsWorld : public CollisionWorld {
         DynamicsWorld& operator=(const DynamicsWorld& world);
 
         // Compute the motion of all bodies and update their positions and orientations
-        void updateAllBodiesMotion();
+        void updateRigidBodiesPositionAndOrientation();
 
         // Update the position and orientation of a body
         void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
@@ -102,6 +113,12 @@ class DynamicsWorld : public CollisionWorld {
         // Compute and set the interpolation factor to all bodies
         void setInterpolationFactorToAllBodies();
 
+        // Initialize the constrained velocities array at each step
+        void initConstrainedVelocitiesArray();
+
+        // Cleanup the constrained velocities array at each step
+        void cleanupConstrainedVelocitiesArray();
+
         // Apply the gravity force to all bodies
         void applyGravity();
 
@@ -169,6 +186,9 @@ public :
         // Set the isGravityOn attribute
         void setIsGratityOn(bool isGravityOn);
 
+        // Return the number of rigid bodies in the world
+        uint getNbRigidBodies() const;
+
         // Add a constraint
         void addConstraint(Constraint* constraint);
 
@@ -286,6 +306,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
     mIsGravityOn = isGravityOn;
 }
 
+// Return the number of rigid bodies in the world
+inline uint DynamicsWorld::getNbRigidBodies() const {
+    return mRigidBodies.size();
+}
+
 // Return an iterator to the beginning of the bodies of the physics world
 inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
     return mRigidBodies.begin();