Add DynamicsSystem class

CMakeLists.txt

@@ -129,8 +129,9 @@ SET (REACTPHYSICS3D_HEADERS
     "src/engine/Entity.h"
     "src/engine/EntityManager.h"
     "src/engine/CollisionWorld.h"
-    "src/engine/ConstraintSolver.h"
+    "src/systems/ConstraintSolverSystem.h"
-    "src/engine/ContactSolver.h"
+    "src/systems/ContactSolverSystem.h"
+    "src/systems/DynamicsSystem.h"
     "src/engine/DynamicsWorld.h"
     "src/engine/EventListener.h"
     "src/engine/Island.h"
@@ -219,8 +220,9 @@ SET (REACTPHYSICS3D_SOURCES
     "src/constraint/Joint.cpp"
     "src/constraint/SliderJoint.cpp"
     "src/engine/CollisionWorld.cpp"
-    "src/engine/ConstraintSolver.cpp"
+    "src/systems/ConstraintSolverSystem.cpp"
-    "src/engine/ContactSolver.cpp"
+    "src/systems/ContactSolverSystem.cpp"
+    "src/systems/DynamicsSystem.cpp"
     "src/engine/DynamicsWorld.cpp"
     "src/engine/Island.cpp"
     "src/engine/Material.cpp"

src/body/CollisionBody.cpp

@@ -214,14 +214,14 @@ const Transform& CollisionBody::getTransform() const {
 }
 
 // Update the broad-phase state for this body (because it has moved for instance)
-void CollisionBody::updateBroadPhaseState() const {
+void CollisionBody::updateBroadPhaseState(decimal timeStep) const {
 
     // For all the proxy collision shapes of the body
     const List<Entity>& proxyShapesEntities = mWorld.mCollisionBodyComponents.getProxyShapes(mEntity);
     for (uint i=0; i < proxyShapesEntities.size(); i++) {
 
         // Update the proxy
-        mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i]);
+        mWorld.mCollisionDetection.updateProxyShape(proxyShapesEntities[i], timeStep);
     }
 }
 
@@ -390,7 +390,7 @@ void CollisionBody::setTransform(const Transform& transform) {
     mWorld.mTransformComponents.setTransform(mEntity, transform);
 
     // Update the broad-phase state of the body
-    updateBroadPhaseState();
+    updateBroadPhaseState(0);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
              "Body " + std::to_string(mEntity.id) + ": Set transform=" + transform.to_string());

src/body/CollisionBody.h

@@ -82,7 +82,7 @@ class CollisionBody {
         void removeAllCollisionShapes();
 
         /// Update the broad-phase state for this body (because it has moved for instance)
-        void updateBroadPhaseState() const;
+        void updateBroadPhaseState(decimal timeStep) const;
 
         /// Ask the broad-phase to test again the collision shapes of the body for collision
         /// (as if the body has moved).

src/body/RigidBody.h

@@ -218,7 +218,7 @@ class RigidBody : public CollisionBody {
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
-        friend class ContactSolver;
+        friend class ContactSolverSystem;
         friend class BallAndSocketJoint;
         friend class SliderJoint;
         friend class HingeJoint;

src/collision/CollisionDetection.h

@@ -292,10 +292,10 @@ class CollisionDetection {
         void removeProxyCollisionShape(ProxyShape* proxyShape);
 
         /// Update a proxy collision shape (that has moved for instance)
-        void updateProxyShape(Entity proxyShapeEntity);
+        void updateProxyShape(Entity proxyShapeEntity, decimal timeStep);
 
         /// Update all the enabled proxy-shapes
-        void updateProxyShapes();
+        void updateProxyShapes(decimal timeStep);
 
         /// Add a pair of bodies that cannot collide with each other
         void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
@@ -411,15 +411,15 @@ inline MemoryManager& CollisionDetection::getMemoryManager() const {
 }
 
 // Update a proxy collision shape (that has moved for instance)
-inline void CollisionDetection::updateProxyShape(Entity proxyShapeEntity) {
+inline void CollisionDetection::updateProxyShape(Entity proxyShapeEntity, decimal timeStep) {
 
     // Update the proxy-shape component
-    mBroadPhaseSystem.updateProxyShape(proxyShapeEntity);
+    mBroadPhaseSystem.updateProxyShape(proxyShapeEntity, timeStep);
 }
 
 // Update all the enabled proxy-shapes
-inline void CollisionDetection::updateProxyShapes() {
+inline void CollisionDetection::updateProxyShapes(decimal timeStep) {
-    mBroadPhaseSystem.updateProxyShapes();
+    mBroadPhaseSystem.updateProxyShapes(timeStep);
 }
 
 #ifdef IS_PROFILING_ACTIVE

src/collision/ContactManifold.h

@@ -124,7 +124,7 @@ class ContactManifold {
         friend class Island;
         friend class CollisionBody;
         friend class ContactManifoldSet;
-        friend class ContactSolver;
+        friend class ContactSolverSystem;
         friend class CollisionDetection;
 };
 

src/collision/ProxyShape.cpp

@@ -122,7 +122,7 @@ void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
         mBody->mWorld.mRigidBodyComponents.setIsSleeping(mBody->getEntity(), false);
     }
 
-    mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity);
+    mBody->mWorld.mCollisionDetection.updateProxyShape(mEntity, 0);
 
     int broadPhaseId = mBody->mWorld.mProxyShapesComponents.getBroadPhaseId(mEntity);
 

src/components/RigidBodyComponents.h

@@ -333,7 +333,8 @@ class RigidBodyComponents : public Components {
         // -------------------- Friendship -------------------- //
 
         friend class DynamicsWorld;
-        friend class ContactSolver;
+        friend class ContactSolverSystem;
+        friend class DynamicsSystem;
         friend class BallAndSocketJoint;
         friend class FixedJoint;
         friend class HingeJoint;

src/constraint/BallAndSocketJoint.cpp

@@ -25,7 +25,7 @@
 
 // Libraries
 #include "BallAndSocketJoint.h"
-#include "engine/ConstraintSolver.h"
+#include "systems/ConstraintSolverSystem.h"
 #include "components/RigidBodyComponents.h"
 
 using namespace reactphysics3d;

src/constraint/ContactPoint.h

@@ -136,7 +136,7 @@ class ContactPoint {
         // Friendship
         friend class ContactManifold;
         friend class ContactManifoldSet;
-        friend class ContactSolver;
+        friend class ContactSolverSystem;
         friend class CollisionDetection;
 };
 

src/constraint/FixedJoint.cpp

@@ -25,7 +25,7 @@
 
 // Libraries
 #include "FixedJoint.h"
-#include "engine/ConstraintSolver.h"
+#include "systems/ConstraintSolverSystem.h"
 #include "components/RigidBodyComponents.h"
 
 using namespace reactphysics3d;

src/constraint/HingeJoint.cpp

@@ -25,7 +25,7 @@
 
 // Libraries
 #include "HingeJoint.h"
-#include "engine/ConstraintSolver.h"
+#include "systems/ConstraintSolverSystem.h"
 #include "components/RigidBodyComponents.h"
 
 using namespace reactphysics3d;

src/constraint/Joint.h

@@ -213,7 +213,7 @@ class Joint {
 
         friend class DynamicsWorld;
         friend class Island;
-        friend class ConstraintSolver;
+        friend class ConstraintSolverSystem;
 };
 
 // Return the reference to the body 1

src/constraint/SliderJoint.cpp

@@ -25,7 +25,7 @@
 
 // Libraries
 #include "SliderJoint.h"
-#include "engine/ConstraintSolver.h"
+#include "systems/ConstraintSolverSystem.h"
 #include "components/RigidBodyComponents.h"
 
 using namespace reactphysics3d;

src/constraint/SliderJoint.h

@@ -34,7 +34,7 @@
 namespace reactphysics3d {
 
 // Declarations
-class ConstraintSolver;
+class ConstraintSolverSystem;
 
 // Structure SliderJointInfo
 /**

src/engine/DynamicsWorld.cpp

@@ -50,13 +50,14 @@ using namespace std;
 DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
               : CollisionWorld(worldSettings, logger, profiler),
                 mIslands(mMemoryManager.getSingleFrameAllocator()),
-                mContactSolver(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
+                mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
                                mProxyShapesComponents, mConfig),
-                mConstraintSolver(mIslands, mRigidBodyComponents),
+                mConstraintSolverSystem(mIslands, mRigidBodyComponents),
+                mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
                 mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
                 mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), 
                 mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
-                mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity), mTimeStep(decimal(1.0f / 60.0f)),
+                mJoints(mMemoryManager.getPoolAllocator()), mGravity(gravity),
                 mIsGravityEnabled(true),  mSleepLinearVelocity(mConfig.defaultSleepLinearVelocity),
                 mSleepAngularVelocity(mConfig.defaultSleepAngularVelocity), mTimeBeforeSleep(mConfig.defaultTimeBeforeSleep),
                 mFreeJointsIDs(mMemoryManager.getPoolAllocator()), mCurrentJointId(0) {
@@ -64,8 +65,9 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
 #ifdef IS_PROFILING_ACTIVE
 
 	// Set the profiler
-    mConstraintSolver.setProfiler(mProfiler);
+    mConstraintSolverSystem.setProfiler(mProfiler);
-    mContactSolver.setProfiler(mProfiler);
+    mContactSolverSystem.setProfiler(mProfiler);
+    mDynamicsSystem.setProfiler(mProfiler);
 
 #endif
 
@@ -113,8 +115,6 @@ void DynamicsWorld::update(decimal timeStep) {
 
     RP3D_PROFILE("DynamicsWorld::update()", mProfiler);
 
-    mTimeStep = timeStep;
-
     // Notify the event listener about the beginning of an internal tick
     if (mEventListener != nullptr) mEventListener->beginInternalTick();
 
@@ -131,27 +131,30 @@ void DynamicsWorld::update(decimal timeStep) {
     mCollisionDetection.reportContacts();
 
     // Integrate the velocities
-    integrateRigidBodiesVelocities();
+    mDynamicsSystem.integrateRigidBodiesVelocities(timeStep);
 
     // Solve the contacts and constraints
-    solveContactsAndConstraints();
+    solveContactsAndConstraints(timeStep);
 
     // Integrate the position and orientation of each body
-    integrateRigidBodiesPositions();
+    mDynamicsSystem.integrateRigidBodiesPositions(timeStep, mContactSolverSystem.isSplitImpulseActive());
 
     // Solve the position correction for constraints
     solvePositionCorrection();
 
     // Update the state (positions and velocities) of the bodies
-    updateBodiesState();
+    mDynamicsSystem.updateBodiesState();
 
-    if (mIsSleepingEnabled) updateSleepingBodies();
+    // Update the proxy-shapes components
+    mCollisionDetection.updateProxyShapes(timeStep);
+
+    if (mIsSleepingEnabled) updateSleepingBodies(timeStep);
 
     // Notify the event listener about the end of an internal tick
     if (mEventListener != nullptr) mEventListener->endInternalTick();
 
     // Reset the external force and torque applied to the bodies
-    resetBodiesForceAndTorque();
+    mDynamicsSystem.resetBodiesForceAndTorque();
 
     // Reset the islands
     mIslands.clear();
@@ -160,161 +163,16 @@ void DynamicsWorld::update(decimal timeStep) {
     mMemoryManager.resetFrameAllocator();
 }
 
-// Integrate position and orientation of the rigid bodies.
-/// The positions and orientations of the bodies are integrated using
-/// the sympletic Euler time stepping scheme.
-void DynamicsWorld::integrateRigidBodiesPositions() {
-
-    RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesPositions()", mProfiler);
-    
-    const decimal isSplitImpulseActive = mContactSolver.isSplitImpulseActive() ? decimal(1.0) : decimal(0.0);
-
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        // Get the constrained velocity
-        Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i];
-        Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i];
-
-        // Add the split impulse velocity from Contact Solver (only used
-        // to update the position)
-        newLinVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitLinearVelocities[i];
-        newAngVelocity += isSplitImpulseActive * mRigidBodyComponents.mSplitAngularVelocities[i];
-
-        // Get current position and orientation of the body
-        const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i];
-        const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation();
-
-        // Update the new constrained position and orientation of the body
-        mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * mTimeStep;
-        mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
-                                                          currentOrientation * decimal(0.5) * mTimeStep;
-    }
-}
-
-// Update the postion/orientation of the bodies
-void DynamicsWorld::updateBodiesState() {
-
-    RP3D_PROFILE("DynamicsWorld::updateBodiesState()", mProfiler);
-
-    // TODO : Make sure we compute this in a system
-
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        // Update the linear and angular velocity of the body
-        mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]);
-        mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]);
-
-        // Update the position of the center of mass of the body
-        mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i];
-
-        // Update the orientation of the body
-        const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i];
-        mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit());
-    }
-
-    // Update the transform of the body (using the new center of mass and new orientation)
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]);
-        const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i];
-        const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
-        transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
-    }
-
-    // Update the world inverse inertia tensor of the body
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
-        const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i];
-        mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
-    }
-
-    // Update the proxy-shapes components
-    mCollisionDetection.updateProxyShapes();
-}
-
-// Reset the split velocities of the bodies
-void DynamicsWorld::resetSplitVelocities() {
-
-    for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-        mRigidBodyComponents.mSplitLinearVelocities[i].setToZero();
-        mRigidBodyComponents.mSplitAngularVelocities[i].setToZero();
-    }
-}
-
-// Integrate the velocities of rigid bodies.
-/// This method only set the temporary velocities but does not update
-/// the actual velocitiy of the bodies. The velocities updated in this method
-/// might violate the constraints and will be corrected in the constraint and
-/// contact solver.
-void DynamicsWorld::integrateRigidBodiesVelocities() {
-
-    RP3D_PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()", mProfiler);
-
-    // Reset the split velocities of the bodies
-    resetSplitVelocities();
-
-    // Integration component velocities using force/torque
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
-        assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
-
-        const Vector3& linearVelocity = mRigidBodyComponents.mLinearVelocities[i];
-        const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i];
-
-        // Integrate the external force to get the new velocity of the body
-        mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + mTimeStep *
-                                                              mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
-        mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + mTimeStep *
-                                                 mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i];
-    }
-
-    // Apply gravity force
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        // If the gravity has to be applied to this rigid body
-        if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
-
-            // Integrate the gravity force
-            mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + mTimeStep *
-                                                                  mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * 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)
-    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
-
-        const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i];
-        const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i];
-        const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
-        const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
-        mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping;
-        mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping;
-    }
-}
 
 // Solve the contacts and constraints
-void DynamicsWorld::solveContactsAndConstraints() {
+void DynamicsWorld::solveContactsAndConstraints(decimal timeStep) {
 
     RP3D_PROFILE("DynamicsWorld::solveContactsAndConstraints()", mProfiler);
 
     // ---------- Solve velocity constraints for joints and contacts ---------- //
 
     // Initialize the contact solver
-    mContactSolver.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, mTimeStep);
+    mContactSolverSystem.init(mCollisionDetection.mCurrentContactManifolds, mCollisionDetection.mCurrentContactPoints, timeStep);
 
     // For each island of the world
     for (uint islandIndex = 0; islandIndex < mIslands.getNbIslands(); islandIndex++) {
@@ -323,7 +181,7 @@ void DynamicsWorld::solveContactsAndConstraints() {
         if (mIslands.joints[islandIndex].size() > 0) {
 
             // Initialize the constraint solver
-            mConstraintSolver.initializeForIsland(mTimeStep, islandIndex);
+            mConstraintSolverSystem.initializeForIsland(timeStep, islandIndex);
         }
     }
 
@@ -335,14 +193,14 @@ void DynamicsWorld::solveContactsAndConstraints() {
             // Solve the constraints
             if (mIslands.joints[islandIndex].size() > 0) {
 
-                mConstraintSolver.solveVelocityConstraints(islandIndex);
+                mConstraintSolverSystem.solveVelocityConstraints(islandIndex);
             }
         }
 
-        mContactSolver.solve();
+        mContactSolverSystem.solve();
     }
 
-    mContactSolver.storeImpulses();
+    mContactSolverSystem.storeImpulses();
 }
 
 // Solve the position error correction of the constraints
@@ -364,7 +222,7 @@ void DynamicsWorld::solvePositionCorrection() {
             for (uint i=0; i<mNbPositionSolverIterations; i++) {
 
                 // Solve the position constraints
-                mConstraintSolver.solvePositionConstraints(islandIndex);
+                mConstraintSolverSystem.solvePositionConstraints(islandIndex);
             }
         }
     }
@@ -798,7 +656,7 @@ void DynamicsWorld::createIslands() {
 // 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() {
+void DynamicsWorld::updateSleepingBodies(decimal timeStep) {
 
     RP3D_PROFILE("DynamicsWorld::updateSleepingBodies()", mProfiler);
 
@@ -831,7 +689,7 @@ void DynamicsWorld::updateSleepingBodies() {
 
                 // Increase the sleep time
                 decimal sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity);
-                mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + mTimeStep);
+                mRigidBodyComponents.setSleepTime(bodyEntity, sleepTime + timeStep);
                 sleepTime = mRigidBodyComponents.getSleepTime(bodyEntity);
                 if (sleepTime < minSleepTime) {
                     minSleepTime = sleepTime;

src/engine/DynamicsWorld.h

@@ -28,10 +28,11 @@
 
 // Libraries
 #include "CollisionWorld.h"
-#include "ConstraintSolver.h"
+#include "systems/ConstraintSolverSystem.h"
 #include "configuration.h"
 #include "utils/Logger.h"
-#include "engine/ContactSolver.h"
+#include "systems/ContactSolverSystem.h"
+#include "systems/DynamicsSystem.h"
 #include "engine/Islands.h"
 
 /// Namespace ReactPhysics3D
@@ -57,11 +58,14 @@ class DynamicsWorld : public CollisionWorld {
         /// All the islands of bodies of the current frame
         Islands mIslands;
 
-        /// Contact solver
+        /// Contact solver system
-        ContactSolver mContactSolver;
+        ContactSolverSystem mContactSolverSystem;
 
-        /// Constraint solver
+        /// Constraint solver system
-        ConstraintSolver mConstraintSolver;
+        ConstraintSolverSystem mConstraintSolverSystem;
+
+        /// Dynamics system
+        DynamicsSystem mDynamicsSystem;
 
         /// Number of iterations for the velocity solver of the Sequential Impulses technique
         uint mNbVelocitySolverIterations;
@@ -81,9 +85,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Gravity vector of the world
         Vector3 mGravity;
 
-        /// Current frame time step (in seconds)
-        decimal mTimeStep;
-
         /// True if the gravity force is on
         bool mIsGravityEnabled;
 
@@ -105,20 +106,8 @@ class DynamicsWorld : public CollisionWorld {
 
         // -------------------- Methods -------------------- //
 
-        /// Integrate the positions and orientations of rigid bodies.
-        void integrateRigidBodiesPositions();
-
-        /// Reset the external force and torque applied to the bodies
-        void resetBodiesForceAndTorque();
-
-        /// Reset the split velocities of the bodies
-        void resetSplitVelocities();
-
-        /// Integrate the velocities of rigid bodies.
-        void integrateRigidBodiesVelocities();
-
         /// Solve the contacts and constraints
-        void solveContactsAndConstraints();
+        void solveContactsAndConstraints(decimal timeStep);
 
         /// Solve the position error correction of the constraints
         void solvePositionCorrection();
@@ -129,11 +118,8 @@ class DynamicsWorld : public CollisionWorld {
         /// Compute the islands using potential contacts and joints and create the actual contacts.
         void createIslands();
 
-        /// Update the postion/orientation of the bodies
-        void updateBodiesState();
-
         /// Put bodies to sleep if needed.
-        void updateSleepingBodies();
+        void updateSleepingBodies(decimal timeStep);
 
         /// Add the joint to the list of joints of the two bodies involved in the joint
         void addJointToBody(Joint* joint);
@@ -197,9 +183,6 @@ class DynamicsWorld : public CollisionWorld {
         /// Set the gravity vector of the world
         void setGravity(Vector3& gravity);
 
-        /// Return the current time step
-        decimal getTimeStep() const;
-
         /// Return if the gravity is on
         bool isGravityEnabled() const;
 
@@ -244,16 +227,6 @@ class DynamicsWorld : public CollisionWorld {
         friend class RigidBody;
 };
 
-// Reset the external force and torque applied to the bodies
-inline void DynamicsWorld::resetBodiesForceAndTorque() {
-
-    // For each body of the world
-    for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) {
-        mRigidBodyComponents.mExternalForces[i].setToZero();
-        mRigidBodyComponents.mExternalTorques[i].setToZero();
-    }
-}
-
 // Get the number of iterations for the velocity constraint solver
 /**
  * @return The number of iterations of the velocity constraint solver
@@ -299,10 +272,10 @@ inline void DynamicsWorld::setNbIterationsPositionSolver(uint nbIterations) {
 inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
                               ContactsPositionCorrectionTechnique technique) {
     if (technique == ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS) {
-        mContactSolver.setIsSplitImpulseActive(false);
+        mContactSolverSystem.setIsSplitImpulseActive(false);
     }
     else {
-        mContactSolver.setIsSplitImpulseActive(true);
+        mContactSolverSystem.setIsSplitImpulseActive(true);
     }
 }
 
@@ -313,10 +286,10 @@ inline void DynamicsWorld::setContactsPositionCorrectionTechnique(
 inline void DynamicsWorld::setJointsPositionCorrectionTechnique(
                               JointsPositionCorrectionTechnique technique) {
     if (technique == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
-        mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
+        mConstraintSolverSystem.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
     }
     else {
-        mConstraintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
+        mConstraintSolverSystem.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
     }
 }
 
@@ -339,14 +312,6 @@ inline void DynamicsWorld::setGravity(Vector3& gravity) {
              "Dynamics World: Set gravity vector to " + gravity.to_string());
 }
 
-// Return the current time step
-/**
- * @return The current time step (in seconds)
- */
-inline decimal DynamicsWorld::getTimeStep() const {
-    return mTimeStep;
-}
-
 // Return if the gravity is enaled
 /**
  * @return True if the gravity is enabled in the world

src/systems/BroadPhaseSystem.cpp

@@ -109,7 +109,7 @@ void BroadPhaseSystem::removeProxyCollisionShape(ProxyShape* proxyShape) {
 }
 
 // Update the broad-phase state of a single proxy-shape
-void BroadPhaseSystem::updateProxyShape(Entity proxyShapeEntity) {
+void BroadPhaseSystem::updateProxyShape(Entity proxyShapeEntity, decimal timeStep) {
 
     assert(mProxyShapesComponents.mMapEntityToComponentIndex.containsKey(proxyShapeEntity));
 
@@ -117,14 +117,14 @@ void BroadPhaseSystem::updateProxyShape(Entity proxyShapeEntity) {
     uint32 index = mProxyShapesComponents.mMapEntityToComponentIndex[proxyShapeEntity];
 
     // Update the proxy-shape component
-    updateProxyShapesComponents(index, 1);
+    updateProxyShapesComponents(index, 1, timeStep);
 }
 
 // Update the broad-phase state of all the enabled proxy-shapes
-void BroadPhaseSystem::updateProxyShapes() {
+void BroadPhaseSystem::updateProxyShapes(decimal timeStep) {
 
     // Update all the enabled proxy-shape components
-    updateProxyShapesComponents(0, mProxyShapesComponents.getNbEnabledComponents());
+    updateProxyShapesComponents(0, mProxyShapesComponents.getNbEnabledComponents(), timeStep);
 }
 
 // Notify the broad-phase that a collision shape has moved and need to be updated
@@ -146,7 +146,7 @@ void BroadPhaseSystem::updateProxyShapeInternal(int broadPhaseId, const AABB& aa
 }
 
 // Update the broad-phase state of some proxy-shapes components
-void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbItems) {
+void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbItems, decimal timeStep) {
 
     assert(nbItems > 0);
     assert(startIndex < mProxyShapesComponents.getNbComponents());
@@ -158,13 +158,6 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 nbI
     nbItems = endIndex - startIndex;
     assert(nbItems >= 0);
 
-    // Get the time step if we are in a dynamics world
-    DynamicsWorld* dynamicsWorld = dynamic_cast<DynamicsWorld*>(mCollisionDetection.getWorld());
-    decimal timeStep = decimal(0.0);
-    if (dynamicsWorld != nullptr) {
-        timeStep = dynamicsWorld->getTimeStep();
-    }
-
     // For each proxy-shape component to update
     for (uint32 i = startIndex; i < startIndex + nbItems; i++) {
 

src/systems/BroadPhaseSystem.h

@@ -97,7 +97,7 @@ class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
 
 };
 
-// Class BroadPhaseAlgorithm
+// Class BroadPhaseSystem
 /**
  * This class represents the broad-phase collision detection. The
  * goal of the broad-phase collision detection is to compute the pairs of proxy shapes
@@ -143,7 +143,7 @@ class BroadPhaseSystem {
         void updateProxyShapeInternal(int broadPhaseId, const AABB& aabb, const Vector3& displacement);
 
         /// Update the broad-phase state of some proxy-shapes components
-        void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems);
+        void updateProxyShapesComponents(uint32 startIndex, uint32 nbItems, decimal timeStep);
 
     public :
 
@@ -169,10 +169,10 @@ class BroadPhaseSystem {
         void removeProxyCollisionShape(ProxyShape* proxyShape);
 
         /// Update the broad-phase state of a single proxy-shape
-        void updateProxyShape(Entity proxyShapeEntity);
+        void updateProxyShape(Entity proxyShapeEntity, decimal timeStep);
 
         /// Update the broad-phase state of all the enabled proxy-shapes
-        void updateProxyShapes();
+        void updateProxyShapes(decimal timeStep);
 
         /// Add a collision shape in the array of shapes that have moved in the last simulation step
         /// and that need to be tested again for broad-phase overlapping.

src/systems/ConstraintSolverSystem.cpp

@@ -24,14 +24,14 @@
 ********************************************************************************/
 
 // Libraries
-#include "ConstraintSolver.h"
+#include "systems/ConstraintSolverSystem.h"
 #include "utils/Profiler.h"
 #include "engine/Island.h"
 
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents)
+ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents)
                  : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents) {
 
 #ifdef IS_PROFILING_ACTIVE
@@ -43,7 +43,7 @@ ConstraintSolver::ConstraintSolver(Islands& islands, RigidBodyComponents& rigidB
 }
 
 // Initialize the constraint solver for a given island
-void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
+void ConstraintSolverSystem::initializeForIsland(decimal dt, uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::initializeForIsland()", mProfiler);
 
@@ -71,7 +71,7 @@ void ConstraintSolver::initializeForIsland(decimal dt, uint islandIndex) {
 }
 
 // Solve the velocity constraints
-void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
+void ConstraintSolverSystem::solveVelocityConstraints(uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::solveVelocityConstraints()", mProfiler);
 
@@ -86,7 +86,7 @@ void ConstraintSolver::solveVelocityConstraints(uint islandIndex) {
 }
 
 // Solve the position constraints
-void ConstraintSolver::solvePositionConstraints(uint islandIndex) {
+void ConstraintSolverSystem::solvePositionConstraints(uint islandIndex) {
 
     RP3D_PROFILE("ConstraintSolver::solvePositionConstraints()", mProfiler);
 

src/systems/ConstraintSolverSystem.h

@@ -23,8 +23,8 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef REACTPHYSICS3D_CONSTRAINT_SOLVER_H
+#ifndef REACTPHYSICS3D_CONSTRAINT_SOLVER_SYSTEM_H
-#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
+#define REACTPHYSICS3D_CONSTRAINT_SOLVER_SYSTEM_H
 
 // Libraries
 #include "configuration.h"
@@ -66,7 +66,7 @@ struct ConstraintSolverData {
 
 };
 
-// Class ConstraintSolver
+// Class ConstraintSolverSystem
 /**
  * This class represents the constraint solver that is used to solve constraints between
  * the rigid bodies. The constraint solver is based on the "Sequential Impulse" technique
@@ -135,7 +135,7 @@ struct ConstraintSolverData {
  * friction but also another twist friction constraint to prevent spin of the body around the
  * contact manifold center.
  */
-class ConstraintSolver {
+class ConstraintSolverSystem {
 
     private :
 
@@ -164,10 +164,10 @@ class ConstraintSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConstraintSolver(Islands& islands, RigidBodyComponents& rigidBodyComponents);
+        ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents);
 
         /// Destructor
-        ~ConstraintSolver() = default;
+        ~ConstraintSolverSystem() = default;
 
         /// Initialize the constraint solver for a given island
         void initializeForIsland(decimal dt, uint islandIndex);
@@ -196,7 +196,7 @@ class ConstraintSolver {
 #ifdef IS_PROFILING_ACTIVE
 
 // Set the profiler
-inline void ConstraintSolver::setProfiler(Profiler* profiler) {
+inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) {
 	mProfiler = profiler;
 }
 

src/systems/ContactSolverSystem.cpp

@@ -24,8 +24,8 @@
 ********************************************************************************/
 
 // Libraries
-#include "ContactSolver.h"
+#include "systems/ContactSolverSystem.h"
-#include "DynamicsWorld.h"
+#include "engine/DynamicsWorld.h"
 #include "body/RigidBody.h"
 #include "constraint/ContactPoint.h"
 #include "utils/Profiler.h"
@@ -38,12 +38,12 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constants initialization
-const decimal ContactSolver::BETA = decimal(0.2);
+const decimal ContactSolverSystem::BETA = decimal(0.2);
-const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
+const decimal ContactSolverSystem::BETA_SPLIT_IMPULSE = decimal(0.2);
-const decimal ContactSolver::SLOP = decimal(0.01);
+const decimal ContactSolverSystem::SLOP = decimal(0.01);
 
 // Constructor
-ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
+ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
                              RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
                              const WorldSettings& worldSettings)
               :mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr),
@@ -59,7 +59,7 @@ ContactSolver::ContactSolver(MemoryManager& memoryManager, Islands& islands, Col
 }
 
 // Initialize the contact constraints
-void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
+void ContactSolverSystem::init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep) {
 
     mAllContactManifolds = contactManifolds;
     mAllContactPoints = contactPoints;
@@ -102,7 +102,7 @@ void ContactSolver::init(List<ContactManifold>* contactManifolds, List<ContactPo
 }
 
 // Initialize the constraint solver for a given island
-void ContactSolver::initializeForIsland(uint islandIndex) {
+void ContactSolverSystem::initializeForIsland(uint islandIndex) {
 
     RP3D_PROFILE("ContactSolver::initializeForIsland()", mProfiler);
 
@@ -332,7 +332,7 @@ void ContactSolver::initializeForIsland(uint islandIndex) {
 /// For each constraint, we apply the previous impulse (from the previous step)
 /// at the beginning. With this technique, we will converge faster towards
 /// the solution of the linear system
-void ContactSolver::warmStart() {
+void ContactSolverSystem::warmStart() {
 
     RP3D_PROFILE("ContactSolver::warmStart()", mProfiler);
 
@@ -486,7 +486,7 @@ void ContactSolver::warmStart() {
 }
 
 // Solve the contacts
-void ContactSolver::solve() {
+void ContactSolverSystem::solve() {
 
     RP3D_PROFILE("ContactSolver::solve()", mProfiler);
 
@@ -764,7 +764,7 @@ void ContactSolver::solve() {
 }
 
 // Compute the collision restitution factor from the restitution factor of each body
-decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
+decimal ContactSolverSystem::computeMixedRestitutionFactor(RigidBody* body1,
                                                             RigidBody* body2) const {
     decimal restitution1 = body1->getMaterial().getBounciness();
     decimal restitution2 = body2->getMaterial().getBounciness();
@@ -774,7 +774,7 @@ decimal ContactSolver::computeMixedRestitutionFactor(RigidBody* body1,
 }
 
 // Compute the mixed friction coefficient from the friction coefficient of each body
-decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
+decimal ContactSolverSystem::computeMixedFrictionCoefficient(RigidBody *body1,
                                                               RigidBody *body2) const {
     // Use the geometric mean to compute the mixed friction coefficient
     return std::sqrt(body1->getMaterial().getFrictionCoefficient() *
@@ -782,14 +782,14 @@ decimal ContactSolver::computeMixedFrictionCoefficient(RigidBody *body1,
 }
 
 // Compute th mixed rolling resistance factor between two bodies
-inline decimal ContactSolver::computeMixedRollingResistance(RigidBody* body1,
+inline decimal ContactSolverSystem::computeMixedRollingResistance(RigidBody* body1,
                                                             RigidBody* body2) const {
     return decimal(0.5f) * (body1->getMaterial().getRollingResistance() + body2->getMaterial().getRollingResistance());
 }
 
 // Store the computed impulses to use them to
 // warm start the solver at the next iteration
-void ContactSolver::storeImpulses() {
+void ContactSolverSystem::storeImpulses() {
 
     RP3D_PROFILE("ContactSolver::storeImpulses()", mProfiler);
 
@@ -816,7 +816,7 @@ void ContactSolver::storeImpulses() {
 
 // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane
 // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
-void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
+void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity,
                                            ContactManifoldSolver& contact) const {
 
     RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler);

src/systems/ContactSolverSystem.h

@@ -23,8 +23,8 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef REACTPHYSICS3D_CONTACT_SOLVER_H
+#ifndef REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H
-#define REACTPHYSICS3D_CONTACT_SOLVER_H
+#define REACTPHYSICS3D_CONTACT_SOLVER_SYSTEM_H
 
 // Libraries
 #include "configuration.h"
@@ -48,9 +48,9 @@ class DynamicsComponents;
 class RigidBodyComponents;
 class ProxyShapeComponents;
 
-// Class Contact Solver
+// Class ContactSolverSystem
 /**
- * This class represents the contact solver that is used to solve rigid bodies contacts.
+ * This class represents the contact solver system that is used to 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).
  *
@@ -117,7 +117,7 @@ class ProxyShapeComponents;
  * friction but also another twist friction constraint to prevent spin of the body around the
  * contact manifold center.
  */
-class ContactSolver {
+class ContactSolverSystem {
 
     private:
 
@@ -360,12 +360,12 @@ class ContactSolver {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ContactSolver(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
+        ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents,
                       RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
                       const WorldSettings& worldSettings);
 
         /// Destructor
-        ~ContactSolver() = default;
+        ~ContactSolverSystem() = default;
 
         /// Initialize the contact constraints
         void init(List<ContactManifold>* contactManifolds, List<ContactPoint>* contactPoints, decimal timeStep);
@@ -395,19 +395,19 @@ class ContactSolver {
 };
 
 // Return true if the split impulses position correction technique is used for contacts
-inline bool ContactSolver::isSplitImpulseActive() const {
+inline bool ContactSolverSystem::isSplitImpulseActive() const {
     return mIsSplitImpulseActive;
 }
 
 // Activate or Deactivate the split impulses for contacts
-inline void ContactSolver::setIsSplitImpulseActive(bool isActive) {
+inline void ContactSolverSystem::setIsSplitImpulseActive(bool isActive) {
     mIsSplitImpulseActive = isActive;
 }
 
 #ifdef IS_PROFILING_ACTIVE
 
 // Set the profiler
-inline void ContactSolver::setProfiler(Profiler* profiler) {
+inline void ContactSolverSystem::setProfiler(Profiler* profiler) {
 
 	mProfiler = profiler;
 }

src/systems/DynamicsSystem.cpp

@@ -0,0 +1,190 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 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 "systems/DynamicsSystem.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+DynamicsSystem::DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity)
+              :mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled),
+               mGravity(gravity) {
+
+}
+
+// Integrate position and orientation of the rigid bodies.
+/// The positions and orientations of the bodies are integrated using
+/// the sympletic Euler time stepping scheme.
+void DynamicsSystem::integrateRigidBodiesPositions(decimal timeStep, bool isSplitImpulseActive) {
+
+    RP3D_PROFILE("DynamicsSystem::integrateRigidBodiesPositions()", mProfiler);
+
+    const decimal isSplitImpulseFactor = isSplitImpulseActive ? decimal(1.0) : decimal(0.0);
+
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        // Get the constrained velocity
+        Vector3 newLinVelocity = mRigidBodyComponents.mConstrainedLinearVelocities[i];
+        Vector3 newAngVelocity = mRigidBodyComponents.mConstrainedAngularVelocities[i];
+
+        // Add the split impulse velocity from Contact Solver (only used
+        // to update the position)
+        newLinVelocity += isSplitImpulseFactor * mRigidBodyComponents.mSplitLinearVelocities[i];
+        newAngVelocity += isSplitImpulseFactor * mRigidBodyComponents.mSplitAngularVelocities[i];
+
+        // Get current position and orientation of the body
+        const Vector3& currentPosition = mRigidBodyComponents.mCentersOfMassWorld[i];
+        const Quaternion& currentOrientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation();
+
+        // Update the new constrained position and orientation of the body
+        mRigidBodyComponents.mConstrainedPositions[i] = currentPosition + newLinVelocity * timeStep;
+        mRigidBodyComponents.mConstrainedOrientations[i] = currentOrientation + Quaternion(0, newAngVelocity) *
+                                                          currentOrientation * decimal(0.5) * timeStep;
+    }
+}
+
+// Update the postion/orientation of the bodies
+void DynamicsSystem::updateBodiesState() {
+
+    RP3D_PROFILE("DynamicsSystem::updateBodiesState()", mProfiler);
+
+    // TODO : Make sure we compute this in a system
+
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        // Update the linear and angular velocity of the body
+        mRigidBodyComponents.setLinearVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedLinearVelocities[i]);
+        mRigidBodyComponents.setAngularVelocity(mRigidBodyComponents.mBodiesEntities[i], mRigidBodyComponents.mConstrainedAngularVelocities[i]);
+
+        // Update the position of the center of mass of the body
+        mRigidBodyComponents.mCentersOfMassWorld[i] = mRigidBodyComponents.mConstrainedPositions[i];
+
+        // Update the orientation of the body
+        const Quaternion& constrainedOrientation = mRigidBodyComponents.mConstrainedOrientations[i];
+        mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).setOrientation(constrainedOrientation.getUnit());
+    }
+
+    // Update the transform of the body (using the new center of mass and new orientation)
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        Transform& transform = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]);
+        const Vector3& centerOfMassWorld = mRigidBodyComponents.mCentersOfMassWorld[i];
+        const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
+        transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
+    }
+
+    // Update the world inverse inertia tensor of the body
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
+        const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i];
+        mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
+    }
+}
+
+// Integrate the velocities of rigid bodies.
+/// This method only set the temporary velocities but does not update
+/// the actual velocitiy of the bodies. The velocities updated in this method
+/// might violate the constraints and will be corrected in the constraint and
+/// contact solver.
+void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
+
+    RP3D_PROFILE("DynamicsSystem::integrateRigidBodiesVelocities()", mProfiler);
+
+    // Reset the split velocities of the bodies
+    resetSplitVelocities();
+
+    // Integration component velocities using force/torque
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        assert(mRigidBodyComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
+        assert(mRigidBodyComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
+
+        const Vector3& linearVelocity = mRigidBodyComponents.mLinearVelocities[i];
+        const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i];
+
+        // Integrate the external force to get the new velocity of the body
+        mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
+                                                              mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
+        mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
+                                                 mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i];
+    }
+
+    // Apply gravity force
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        // If the gravity has to be applied to this rigid body
+        if (mRigidBodyComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
+
+            // Integrate the gravity force
+            mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep *
+                                                                  mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mInitMasses[i] * 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)
+    for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+
+        const decimal linDampingFactor = mRigidBodyComponents.mLinearDampings[i];
+        const decimal angDampingFactor = mRigidBodyComponents.mAngularDampings[i];
+        const decimal linearDamping = std::pow(decimal(1.0) - linDampingFactor, timeStep);
+        const decimal angularDamping = std::pow(decimal(1.0) - angDampingFactor, timeStep);
+        mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] * linearDamping;
+        mRigidBodyComponents.mConstrainedAngularVelocities[i] = mRigidBodyComponents.mConstrainedAngularVelocities[i] * angularDamping;
+    }
+}
+
+// Reset the external force and torque applied to the bodies
+void DynamicsSystem::resetBodiesForceAndTorque() {
+
+    // For each body of the world
+    for (uint32 i=0; i < mRigidBodyComponents.getNbComponents(); i++) {
+        mRigidBodyComponents.mExternalForces[i].setToZero();
+        mRigidBodyComponents.mExternalTorques[i].setToZero();
+    }
+}
+
+// Reset the split velocities of the bodies
+void DynamicsSystem::resetSplitVelocities() {
+
+    for(uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
+        mRigidBodyComponents.mSplitLinearVelocities[i].setToZero();
+        mRigidBodyComponents.mSplitAngularVelocities[i].setToZero();
+    }
+}
+

src/systems/DynamicsSystem.h

@@ -0,0 +1,111 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://www.reactphysics3d.com                 *
+* Copyright (c) 2010-2018 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_DYNAMICS_SYSTEM_H
+#define REACTPHYSICS3D_DYNAMICS_SYSTEM_H
+
+// Libraries
+#include "utils/Profiler.h"
+#include "components/RigidBodyComponents.h"
+#include "components/TransformComponents.h"
+
+namespace reactphysics3d {
+
+// Class DynamicsSystem
+/**
+ * This class is responsible to compute and update the dynamics of the bodies that are simulated
+ * using physics.
+ */
+class DynamicsSystem {
+
+    private :
+
+        // -------------------- Attributes -------------------- //
+
+        /// Reference to the rigid body components
+        RigidBodyComponents& mRigidBodyComponents;
+
+        /// Reference to the transform components
+        TransformComponents& mTransformComponents;
+
+        /// Reference to the variable to know if gravity is enabled in the world
+        bool& mIsGravityEnabled;
+
+        /// Reference to the world gravity vector
+        Vector3& mGravity;
+
+#ifdef IS_PROFILING_ACTIVE
+
+        /// Pointer to the profiler
+        Profiler* mProfiler;
+#endif
+
+    public :
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
+                       bool& isGravityEnabled, Vector3& gravity);
+
+        /// Destructor
+        ~DynamicsSystem() = default;
+
+#ifdef IS_PROFILING_ACTIVE
+
+        /// Set the profiler
+        void setProfiler(Profiler* profiler);
+
+#endif
+
+        /// Integrate the positions and orientations of rigid bodies.
+        void integrateRigidBodiesPositions(decimal timeStep, bool isSplitImpulseActive);
+
+        /// Integrate the velocities of rigid bodies.
+        void integrateRigidBodiesVelocities(decimal timeStep);
+
+        /// Update the postion/orientation of the bodies
+        void updateBodiesState();
+
+        /// Reset the external force and torque applied to the bodies
+        void resetBodiesForceAndTorque();
+
+        /// Reset the split velocities of the bodies
+        void resetSplitVelocities();
+
+};
+
+#ifdef IS_PROFILING_ACTIVE
+
+// Set the profiler
+inline void DynamicsSystem::setProfiler(Profiler* profiler) {
+    mProfiler = profiler;
+}
+
+#endif
+
+}
+
+#endif