Move isGravityEnabled and inertia tensors in DynamicsComponents

src/body/RigidBody.cpp

@@ -42,8 +42,7 @@ using namespace reactphysics3d;
 RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
           : CollisionBody(world, entity, id), mArrayIndex(0),
             mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
-            mIsGravityEnabled(true), mMaterial(world.mConfig),
+            mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
-            mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
 
     // Compute the inverse mass
     mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
@@ -92,14 +91,14 @@ void RigidBody::setType(BodyType type) {
 
         // Reset the inverse mass and inverse inertia tensor to zero
         mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0));
-        mInertiaTensorLocalInverse.setToZero();
+        mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
-        mInertiaTensorInverseWorld.setToZero();
+        mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
     }
     else {  // If it is a dynamic body
         mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
 
         if (mIsInertiaTensorSetByUser) {
-            mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+            mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
         }
     }
 
@@ -118,6 +117,27 @@ void RigidBody::setType(BodyType type) {
     mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
 }
 
+// Get the inverse local inertia tensor of the body (in body coordinates)
+const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
+    return mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
+}
+
+// Return the inverse of the inertia tensor in world coordinates.
+/// The inertia tensor I_w in world coordinates is computed with the
+/// local inverse inertia tensor I_b^-1 in body coordinates
+/// by I_w = R * I_b^-1 * R^T
+/// where R is the rotation matrix (and R^T its transpose) of the
+/// current orientation quaternion of the body
+/**
+ * @return The 3x3 inverse inertia tensor matrix of the body in world-space
+ *         coordinates
+ */
+Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
+
+    // Compute and return the inertia tensor in world coordinates
+    return mWorld.mDynamicsComponents.getInertiaTensorWorldInverse(mEntity);
+}
+
 // Method that return the mass of the body
 /**
  * @return The mass (in kilograms) of the body
@@ -171,7 +191,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
     if (mType != BodyType::DYNAMIC) return;
 
     // Compute the inverse local inertia tensor
-    mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+    mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -234,7 +254,7 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
     if (mType != BodyType::DYNAMIC) return;
 
     // Compute the inverse local inertia tensor
-    mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+    mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -337,7 +357,8 @@ void RigidBody::updateInertiaTensorInverseWorld() {
     // TODO : Make sure we do this in a system
 
     Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix();
-    mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
+    const Matrix3x3& inverseInertiaLocalTensor = mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
+    mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose());
 }
 
 // Add a collision shape to the body.
@@ -437,11 +458,11 @@ void RigidBody::removeCollisionShape(ProxyShape* proxyShape) {
  * @param isEnabled True if you want the gravity to be applied to this body
  */
 void RigidBody::enableGravity(bool isEnabled) {
-    mIsGravityEnabled = isEnabled;
+    mWorld.mDynamicsComponents.setIsGravityEnabled(mEntity, isEnabled);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
              "Body " + std::to_string(mID) + ": Set isGravityEnabled=" +
-             (mIsGravityEnabled ? "true" : "false"));
+             (isEnabled ? "true" : "false"));
 }
 
 // Set the linear damping factor. This is the ratio of the linear velocity
@@ -568,8 +589,8 @@ void RigidBody::recomputeMassInformation() {
 
     mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0));
     mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0));
-    if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
+    if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
-    if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
+    if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
     if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
     Matrix3x3 inertiaTensorLocal;
     inertiaTensorLocal.setToZero();
@@ -646,7 +667,7 @@ void RigidBody::recomputeMassInformation() {
         }
 
         // Compute the local inverse inertia tensor
-        mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
+        mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
     }
 
     // Update the world inverse inertia tensor
@@ -675,6 +696,14 @@ Vector3 RigidBody::getAngularVelocity() const {
     return mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
 }
 
+// Return true if the gravity needs to be applied to this rigid body
+/**
+ * @return True if the gravity is applied to the body
+ */
+bool RigidBody::isGravityEnabled() const {
+    return mWorld.mDynamicsComponents.getIsGravityEnabled(mEntity);
+}
+
 // Apply an external torque to the body.
 /// If the body is sleeping, calling this method will wake it up. Note that the
 /// force will we added to the sum of the applied torques and that this sum will be

src/body/RigidBody.h

@@ -71,15 +71,6 @@ class RigidBody : public CollisionBody {
         /// by the user with respect to the center of mass of the body
         Matrix3x3 mUserInertiaTensorLocalInverse;
 
-        /// Inverse of the inertia tensor of the body
-        Matrix3x3 mInertiaTensorLocalInverse;
-
-        /// Inverse of the world inertia tensor of the body
-        Matrix3x3 mInertiaTensorInverseWorld;
-
-        /// True if the gravity needs to be applied to this rigid body
-        bool mIsGravityEnabled;
-
         /// Material properties of the rigid body
         Material mMaterial;
 
@@ -229,35 +220,6 @@ class RigidBody : public CollisionBody {
         friend class FixedJoint;
 };
 
-// Get the inverse local inertia tensor of the body (in body coordinates)
-inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
-    return mInertiaTensorLocalInverse;
-}
-
-// Return the inverse of the inertia tensor in world coordinates.
-/// The inertia tensor I_w in world coordinates is computed with the
-/// local inverse inertia tensor I_b^-1 in body coordinates
-/// by I_w = R * I_b^-1 * R^T
-/// where R is the rotation matrix (and R^T its transpose) of the
-/// current orientation quaternion of the body
-/**
- * @return The 3x3 inverse inertia tensor matrix of the body in world-space
- *         coordinates
- */
-inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
-
-    // Compute and return the inertia tensor in world coordinates
-    return mInertiaTensorInverseWorld;
-}
-
-// Return true if the gravity needs to be applied to this rigid body
-/**
- * @return True if the gravity is applied to the body
- */
-inline bool RigidBody::isGravityEnabled() const {
-    return mIsGravityEnabled;
-}
-
 // Return a reference to the material properties of the rigid body
 /**
  * @return A reference to the material of the body

src/components/DynamicsComponents.cpp

@@ -37,7 +37,8 @@ using namespace reactphysics3d;
 DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
                     :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
                                            sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) +
-                                           sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool)) {
+                                           sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) +
+                                           sizeof(bool) + sizeof(bool)) {
 
     // Allocate memory for the components data
     allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -69,7 +70,10 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
     decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
     decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
-    bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newInverseMasses + nbComponentsToAllocate);
+    Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
+    Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
+    bool* newIsGravityEnabled = reinterpret_cast<bool*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
+    bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -88,6 +92,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
         memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
         memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
+        memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
+        memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
+        memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
         memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
 
         // Deallocate previous memory
@@ -108,6 +115,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     mAngularDampings = newAngularDampings;
     mInitMasses = newInitMasses;
     mInverseMasses = newInverseMasses;
+    mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
+    mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
+    mIsGravityEnabled = newIsGravityEnabled;
     mIsAlreadyInIsland = newIsAlreadyInIsland;
     mNbAllocatedComponents = nbComponentsToAllocate;
 }
@@ -132,6 +142,9 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
     mAngularDampings[index] = decimal(0.0);
     mInitMasses[index] = decimal(1.0);
     mInverseMasses[index] = decimal(1.0);
+    new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+    new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+    mIsGravityEnabled[index] = true;
     mIsAlreadyInIsland[index] = false;
 
     // Map the entity with the new component lookup index
@@ -163,6 +176,9 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
     mAngularDampings[destIndex] = mAngularDampings[srcIndex];
     mInitMasses[destIndex] = mInitMasses[srcIndex];
     mInverseMasses[destIndex] = mInverseMasses[srcIndex];
+    mInverseInertiaTensorsLocal[destIndex] = mInverseInertiaTensorsLocal[srcIndex];
+    mInverseInertiaTensorsWorld[destIndex] = mInverseInertiaTensorsWorld[srcIndex];
+    mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
     mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
 
     // Destroy the source component
@@ -196,6 +212,9 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     decimal angularDamping1 = mAngularDampings[index1];
     decimal initMass1 = mInitMasses[index1];
     decimal inverseMass1 = mInverseMasses[index1];
+    Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
+    Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
+    bool isGravityEnabled1 = mIsGravityEnabled[index1];
     bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
 
     // Destroy component 1
@@ -217,6 +236,9 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     mAngularDampings[index2] = angularDamping1;
     mInitMasses[index2] = initMass1;
     mInverseMasses[index2] = inverseMass1;
+    mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
+    mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
+    mIsGravityEnabled[index2] = isGravityEnabled1;
     mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
 
     // Update the entity to component index mapping
@@ -245,4 +267,6 @@ void DynamicsComponents::destroyComponent(uint32 index) {
     mSplitAngularVelocities[index].~Vector3();
     mExternalForces[index].~Vector3();
     mExternalTorques[index].~Vector3();
+    mInverseInertiaTensorsLocal[index].~Matrix3x3();
+    mInverseInertiaTensorsWorld[index].~Matrix3x3();
 }

src/components/DynamicsComponents.h

@@ -30,6 +30,7 @@
 #include "mathematics/Transform.h"
 #include "engine/Entity.h"
 #include "components/Components.h"
+#include "mathematics/Matrix3x3.h"
 #include "containers/Map.h"
 
 // ReactPhysics3D namespace
@@ -89,6 +90,15 @@ class DynamicsComponents : public Components {
         /// Array with the inverse mass of each component
         decimal* mInverseMasses;
 
+        /// Array with the inverse of the inertia tensor of each component
+        Matrix3x3* mInverseInertiaTensorsLocal;
+
+        /// Array with the inverse of the world inertia tensor of each component
+        Matrix3x3* mInverseInertiaTensorsWorld;
+
+        /// True if the gravity needs to be applied to this component
+        bool* mIsGravityEnabled;
+
         /// Array with the boolean value to know if the body has already been added into an island
         bool* mIsAlreadyInIsland;
 
@@ -168,6 +178,15 @@ class DynamicsComponents : public Components {
         /// Return the mass inverse of an entity
         decimal getMassInverse(Entity bodyEntity) const;
 
+        /// Return the inverse local inertia tensor of an entity
+        const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
+
+        /// Return the inverse world inertia tensor of an entity
+        const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
+
+        /// Return true if gravity is enabled for this entity
+        bool getIsGravityEnabled(Entity bodyEntity) const;
+
         /// Return true if the entity is already in an island
         bool getIsAlreadyInIsland(Entity bodyEntity) const;
 
@@ -207,6 +226,15 @@ class DynamicsComponents : public Components {
         /// Set the inverse mass of an entity
         void setMassInverse(Entity bodyEntity, decimal inverseMass);
 
+        /// Set the inverse local inertia tensor of an entity
+        void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
+
+        /// Set the inverse world inertia tensor of an entity
+        void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
+
+        /// Set the value to know if the gravity is enabled for this entity
+        bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled);
+
         /// Set the value to know if the entity is already in an island
         bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
 
@@ -334,6 +362,22 @@ inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const {
    return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]];
 }
 
+// Return the inverse local inertia tensor of an entity
+inline const Matrix3x3& DynamicsComponents::getInertiaTensorLocalInverse(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the inverse world inertia tensor of an entity
+inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
+}
+
 // Set the constrained linear velocity of an entity
 inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
 
@@ -414,6 +458,30 @@ inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal invers
    mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass;
 }
 
+// Set the inverse local inertia tensor of an entity
+inline void DynamicsComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse;
+}
+
+// Set the inverse world inertia tensor of an entity
+inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
+}
+
+// Return true if gravity is enabled for this entity
+inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]];
+}
+
 // Return true if the entity is already in an island
 inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
 
@@ -422,6 +490,14 @@ inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
    return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]];
 }
 
+// Set the value to know if the gravity is enabled for this entity
+inline bool DynamicsComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]] = isGravityEnabled;
+}
+
 /// Set the value to know if the entity is already in an island
 inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) {
 

src/engine/DynamicsWorld.cpp

@@ -295,60 +295,52 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
     // Reset the split velocities of the bodies
     resetSplitVelocities();
 
-    // TODO : We should loop over non-sleeping dynamic components here and not over islands
+    // Integration component velocities using force/torque
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
 
-    // For each island of the world
+        assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
-    for (uint i=0; i < mIslands.getNbIslands(); i++) {
+        assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
 
-        // For each body of the island
+        // Integrate the external force to get the new velocity of the body
-        for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
+        mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mLinearVelocities[i] + mTimeStep *
+                                                              mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i];
+        mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mAngularVelocities[i] +
+                                                               mTimeStep * mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i];
+    }
 
-            const Entity bodyEntity = mIslands.bodyEntities[i][b];
+    // Apply gravity force
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+        // If the gravity has to be applied to this rigid body
+        if (mDynamicsComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
 
-            RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
+            // Integrate the gravity force
-
+            mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] + mTimeStep *
-            const uint indexBody = body->mArrayIndex;
+                                                                  mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mInitMasses[i] * mGravity;
-
-            assert(mDynamicsComponents.getSplitLinearVelocity(bodyEntity) == Vector3(0, 0, 0));
-            assert(mDynamicsComponents.getSplitAngularVelocity(bodyEntity) == Vector3(0, 0, 0));
-            assert(indexBody < mRigidBodies.size());
-
-            // Integrate the external force to get the new velocity of the body
-            mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getLinearVelocity(bodyEntity) +
-                                        mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) * mDynamicsComponents.getExternalForce(bodyEntity));
-            mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getAngularVelocity(bodyEntity) +
-                                        mTimeStep * body->getInertiaTensorInverseWorld() * mDynamicsComponents.getExternalTorque(bodyEntity));
-
-            // If the gravity has to be applied to this rigid body
-            if (body->isGravityEnabled() && mIsGravityEnabled) {
-
-                // Integrate the gravity force
-                mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) +
-                                                                 mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) *
-                        body->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 = mDynamicsComponents.getLinearDamping(bodyEntity);
-            decimal angDampingFactor = mDynamicsComponents.getAngularDamping(bodyEntity);
-            decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
-            decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
-            mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) * linearDamping);
-            mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity) * angularDamping);
         }
     }
+
+    // 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)
+    for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
+
+        const decimal linDampingFactor = mDynamicsComponents.mLinearDampings[i];
+        const decimal angDampingFactor = mDynamicsComponents.mAngularDampings[i];
+        const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
+        const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
+        mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] * linearDamping;
+        mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i] * angularDamping;
+    }
 }
 
 // Solve the contacts and constraints