Add DynamicsComponents class

CMakeLists.txt

@@ -143,6 +143,7 @@ SET (REACTPHYSICS3D_HEADERS
     "src/components/BodyComponents.h"
     "src/components/TransformComponents.h"
     "src/components/ProxyShapeComponents.h"
+    "src/components/DynamicsComponents.h"
     "src/collision/CollisionCallback.h"
     "src/collision/OverlapCallback.h"
     "src/mathematics/mathematics.h"
@@ -235,6 +236,7 @@ SET (REACTPHYSICS3D_SOURCES
     "src/components/BodyComponents.cpp"
     "src/components/TransformComponents.cpp"
     "src/components/ProxyShapeComponents.cpp"
+    "src/components/DynamicsComponents.cpp"
     "src/collision/CollisionCallback.cpp"
     "src/mathematics/mathematics_functions.cpp"
     "src/mathematics/Matrix2x2.cpp"

src/body/RigidBody.cpp

@@ -83,8 +83,8 @@ void RigidBody::setType(BodyType type) {
     if (mType == BodyType::STATIC) {
 
         // Reset the velocity to zero
-        mLinearVelocity.setToZero();
+        mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero());
-        mAngularVelocity.setToZero();
+        mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero());
     }
 
     // If it is a static or a kinematic body
@@ -189,7 +189,10 @@ void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
     mCenterOfMassWorld = mWorld.mTransformComponents.getTransform(mEntity) * mCenterOfMassLocal;
 
     // Update the linear velocity of the center of mass
-    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    Vector3 linearVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
+    const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
+    linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
              "Body " + std::to_string(mID) + ": Set centerOfMassLocal=" + centerOfMassLocal.to_string());
@@ -422,15 +425,15 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
     if (mType == BodyType::STATIC) return;
 
     // Update the linear velocity of the current body state
-    mLinearVelocity = linearVelocity;
+    mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
     // If the linear velocity is not zero, awake the body
-    if (mLinearVelocity.lengthSquare() > decimal(0.0)) {
+    if (linearVelocity.lengthSquare() > decimal(0.0)) {
         setIsSleeping(false);
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set linearVelocity=" + mLinearVelocity.to_string());
+             "Body " + std::to_string(mID) + ": Set linearVelocity=" + linearVelocity.to_string());
 }
 
 // Set the angular velocity.
@@ -439,19 +442,21 @@ void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
 */
 void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
 
+    // TODO : Make sure this method is not called from the internal physics engine
+
     // If it is a static body, we do nothing
     if (mType == BodyType::STATIC) return;
 
     // Set the angular velocity
-    mAngularVelocity = angularVelocity;
+    mWorld.mDynamicsComponents.setAngularVelocity(mEntity, angularVelocity);
 
     // If the velocity is not zero, awake the body
-    if (mAngularVelocity.lengthSquare() > decimal(0.0)) {
+    if (angularVelocity.lengthSquare() > decimal(0.0)) {
         setIsSleeping(false);
     }
 
     RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
-             "Body " + std::to_string(mID) + ": Set angularVelocity=" + mAngularVelocity.to_string());
+             "Body " + std::to_string(mID) + ": Set angularVelocity=" + angularVelocity.to_string());
 }
 
 // Set the current position and orientation
@@ -470,7 +475,10 @@ void RigidBody::setTransform(const Transform& transform) {
     mCenterOfMassWorld = transform * mCenterOfMassLocal;
 
     // Update the linear velocity of the center of mass
-    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
+    const Vector3& angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
+    linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -573,7 +581,39 @@ void RigidBody::recomputeMassInformation() {
     updateInertiaTensorInverseWorld();
 
     // Update the linear velocity of the center of mass
-    mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    Vector3 linearVelocity = mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
+    Vector3 angularVelocity = mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
+    linearVelocity += angularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
+    mWorld.mDynamicsComponents.setLinearVelocity(mEntity, linearVelocity);
+}
+
+// Return the linear velocity
+/**
+ * @return The linear velocity vector of the body
+ */
+Vector3 RigidBody::getLinearVelocity() const {
+    return mWorld.mDynamicsComponents.getLinearVelocity(mEntity);
+}
+
+// Return the angular velocity of the body
+/**
+ * @return The angular velocity vector of the body
+ */
+Vector3 RigidBody::getAngularVelocity() const {
+    return mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
+}
+
+// Set the variable to know whether or not the body is sleeping
+void RigidBody::setIsSleeping(bool isSleeping) {
+
+    if (isSleeping) {
+        mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero());
+        mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero());
+        mExternalForce.setToZero();
+        mExternalTorque.setToZero();
+    }
+
+    CollisionBody::setIsSleeping(isSleeping);
 }
 
 #ifdef IS_PROFILING_ACTIVE

src/body/RigidBody.h

@@ -70,10 +70,10 @@ class RigidBody : public CollisionBody {
         Vector3 mCenterOfMassWorld;
 
         /// Linear velocity of the body
-        Vector3 mLinearVelocity;
+        //Vector3 mLinearVelocity;
 
         /// Angular velocity of the body
-        Vector3 mAngularVelocity;
+        //Vector3 mAngularVelocity;
 
         /// Current external force on the body
         Vector3 mExternalForce;
@@ -260,22 +260,6 @@ inline decimal RigidBody::getMass() const {
     return mInitMass;
 }
 
-// Return the linear velocity
-/**
- * @return The linear velocity vector of the body
- */
-inline Vector3 RigidBody::getLinearVelocity() const {
-    return mLinearVelocity;
-}
-
-// Return the angular velocity of the body
-/**
- * @return The angular velocity vector of the body
- */
-inline Vector3 RigidBody::getAngularVelocity() const {
-    return mAngularVelocity;
-}
-
 // Get the inverse local inertia tensor of the body (in body coordinates)
 inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
     return mInertiaTensorLocalInverse;
@@ -345,19 +329,6 @@ inline JointListElement* RigidBody::getJointsList() {
     return mJointsList;
 }
 
-// Set the variable to know whether or not the body is sleeping
-inline void RigidBody::setIsSleeping(bool isSleeping) {
-
-    if (isSleeping) {
-        mLinearVelocity.setToZero();
-        mAngularVelocity.setToZero();
-        mExternalForce.setToZero();
-        mExternalTorque.setToZero();
-    }
-
-    CollisionBody::setIsSleeping(isSleeping);
-}
-
 // Apply an external force to the body at its center of mass.
 /// 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 forces and that this sum will be

src/collision/CollisionDetection.cpp

@@ -48,11 +48,12 @@ using namespace std;
 
 
 // Constructor
-CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents &transformComponents,
+CollisionDetection::CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents, TransformComponents& transformComponents,
-                                       MemoryManager& memoryManager)
+                                       DynamicsComponents& dynamicsComponents, MemoryManager& memoryManager)
                    : mMemoryManager(memoryManager), mProxyShapesComponents(proxyShapesComponents), mTransformComponents(transformComponents),
                      mCollisionDispatch(mMemoryManager.getPoolAllocator()), mWorld(world),
-                     mOverlappingPairs(mMemoryManager.getPoolAllocator()), mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents),
+                     mOverlappingPairs(mMemoryManager.getPoolAllocator()),
+                     mBroadPhaseSystem(*this, mProxyShapesComponents, mTransformComponents, dynamicsComponents),
                      mNoCollisionPairs(mMemoryManager.getPoolAllocator()), mIsCollisionShapesAdded(false),
                      mNarrowPhaseInput(mMemoryManager.getSingleFrameAllocator()) {
 

src/collision/CollisionDetection.h

@@ -154,7 +154,8 @@ class CollisionDetection {
 
         /// Constructor
         CollisionDetection(CollisionWorld* world, ProxyShapeComponents& proxyShapesComponents,
-                           TransformComponents& transformComponents, MemoryManager& memoryManager);
+                           TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents,
+                           MemoryManager& memoryManager);
 
         /// Destructor
         ~CollisionDetection() = default;

src/components/Components.h

@@ -107,9 +107,12 @@ class Components {
         /// Remove a component
         void removeComponent(Entity entity);
 
-        // Notify if a given entity is disabled (sleeping or inactive) or not
+        /// Notify if a given entity is disabled (sleeping or inactive) or not
         void setIsEntityDisabled(Entity entity, bool isDisabled);
 
+        /// Return true if there is a component for a given entity
+        bool hasComponent(Entity entity) const;
+
         /// Return the number of components
         uint32 getNbComponents() const;
 
@@ -117,6 +120,11 @@ class Components {
         uint32 getNbEnabledComponents() const;
 };
 
+// Return true if there is a component for a given entity
+inline bool Components::hasComponent(Entity entity) const {
+    return mMapEntityToComponentIndex.containsKey(entity);
+}
+
 // Return the number of components
 inline uint32 Components::getNbComponents() const {
     return mNbComponents;

src/components/DynamicsComponents.cpp

@@ -0,0 +1,159 @@
+/********************************************************************************
+* 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 "DynamicsComponents.h"
+#include "engine/EntityManager.h"
+#include <cassert>
+#include <random>
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+
+// Constructor
+DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
+                    :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof (Vector3)) {
+
+    // Allocate memory for the components data
+    allocate(INIT_NB_ALLOCATED_COMPONENTS);
+}
+
+// Allocate memory for a given number of components
+void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
+
+    assert(nbComponentsToAllocate > mNbAllocatedComponents);
+
+    // Size for the data of a single component (in bytes)
+    const size_t totalSizeBytes = nbComponentsToAllocate * mComponentDataSize;
+
+    // Allocate memory
+    void* newBuffer = mMemoryAllocator.allocate(totalSizeBytes);
+    assert(newBuffer != nullptr);
+
+    // New pointers to components data
+    Entity* newEntities = static_cast<Entity*>(newBuffer);
+    Vector3* newLinearVelocities = reinterpret_cast<Vector3*>(newEntities + nbComponentsToAllocate);
+    Vector3* newAngularVelocities = reinterpret_cast<Vector3*>(newLinearVelocities + nbComponentsToAllocate);
+
+    // If there was already components before
+    if (mNbComponents > 0) {
+
+        // Copy component data from the previous buffer to the new one
+        memcpy(newLinearVelocities, mLinearVelocities, mNbComponents * sizeof(Transform));
+        memcpy(newAngularVelocities, mAngularVelocities, mNbComponents * sizeof(Entity));
+
+        // Deallocate previous memory
+        mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
+    }
+
+    mBuffer = newBuffer;
+    mBodies = newEntities;
+    mLinearVelocities = newLinearVelocities;
+    mAngularVelocities = newAngularVelocities;
+    mNbAllocatedComponents = nbComponentsToAllocate;
+}
+
+// Add a component
+void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component) {
+
+    // Prepare to add new component (allocate memory if necessary and compute insertion index)
+    uint32 index = prepareAddComponent(isSleeping);
+
+    // Insert the new component data
+    new (mBodies + index) Entity(bodyEntity);
+    new (mLinearVelocities + index) Vector3(component.linearVelocity);
+    new (mAngularVelocities + index) Vector3(component.angularVelocity);
+
+    // Map the entity with the new component lookup index
+    mMapEntityToComponentIndex.add(Pair<Entity, uint32>(bodyEntity, index));
+
+    mNbComponents++;
+
+    assert(mDisabledStartIndex <= mNbComponents);
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
+}
+
+// Move a component from a source to a destination index in the components array
+// The destination location must contain a constructed object
+void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) {
+
+    const Entity entity = mBodies[srcIndex];
+
+    // Copy the data of the source component to the destination location
+    new (mBodies + destIndex) Entity(mBodies[srcIndex]);
+    new (mLinearVelocities + destIndex) Vector3(mLinearVelocities[srcIndex]);
+    new (mAngularVelocities + destIndex) Vector3(mAngularVelocities[srcIndex]);
+
+    // Destroy the source component
+    destroyComponent(srcIndex);
+
+    assert(!mMapEntityToComponentIndex.containsKey(entity));
+
+    // Update the entity to component index mapping
+    mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity, destIndex));
+
+    assert(mMapEntityToComponentIndex[mBodies[destIndex]] == destIndex);
+}
+
+// Swap two components in the array
+void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
+
+    // Copy component 1 data
+    Entity entity1(mBodies[index1]);
+    Vector3 linearVelocity1(mLinearVelocities[index1]);
+    Vector3 angularVelocity1(mAngularVelocities[index1]);
+
+    // Destroy component 1
+    destroyComponent(index1);
+
+    moveComponentToIndex(index2, index1);
+
+    // Reconstruct component 1 at component 2 location
+    new (mBodies + index2) Entity(entity1);
+    new (mLinearVelocities + index2) Vector3(linearVelocity1);
+    new (mAngularVelocities + index2) Vector3(angularVelocity1);
+
+    // Update the entity to component index mapping
+    mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
+
+    assert(mMapEntityToComponentIndex[mBodies[index1]] == index1);
+    assert(mMapEntityToComponentIndex[mBodies[index2]] == index2);
+    assert(mNbComponents == static_cast<uint32>(mMapEntityToComponentIndex.size()));
+}
+
+// Destroy a component at a given index
+void DynamicsComponents::destroyComponent(uint32 index) {
+
+    Components::destroyComponent(index);
+
+    assert(mMapEntityToComponentIndex[mBodies[index]] == index);
+
+    mMapEntityToComponentIndex.remove(mBodies[index]);
+
+    mBodies[index].~Entity();
+    mLinearVelocities[index].~Vector3();
+    mAngularVelocities[index].~Vector3();
+}

src/components/DynamicsComponents.h

@@ -0,0 +1,153 @@
+/********************************************************************************
+* 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_COMPONENTS_H
+#define REACTPHYSICS3D_DYNAMICS_COMPONENTS_H
+
+// Libraries
+#include "mathematics/Transform.h"
+#include "engine/Entity.h"
+#include "components/Components.h"
+#include "containers/Map.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Class declarations
+class MemoryAllocator;
+class EntityManager;
+
+// Class DynamicsComponents
+/**
+ * This class represent the component of the ECS that contains the variables concerning dynamics
+ * like velocities.
+ */
+class DynamicsComponents : public Components {
+
+    private:
+
+        // -------------------- Attributes -------------------- //
+
+        /// Array of body entities of each component
+        Entity* mBodies;
+
+        /// Array with the linear velocity of each component
+        Vector3* mLinearVelocities;
+
+        /// Array with the angular velocity of each component
+        Vector3* mAngularVelocities;
+
+        // -------------------- Methods -------------------- //
+
+        /// Allocate memory for a given number of components
+        virtual void allocate(uint32 nbComponentsToAllocate) override;
+
+        /// Destroy a component at a given index
+        virtual void destroyComponent(uint32 index) override;
+
+        /// Move a component from a source to a destination index in the components array
+        virtual void moveComponentToIndex(uint32 srcIndex, uint32 destIndex) override;
+
+        /// Swap two components in the array
+        virtual void swapComponents(uint32 index1, uint32 index2) override;
+
+    public:
+
+        /// Structure for the data of a transform component
+        struct DynamicsComponent {
+
+            const Vector3& linearVelocity;
+            const Vector3& angularVelocity;
+
+            /// Constructor
+            DynamicsComponent(const Vector3& linearVelocity, const Vector3& angularVelocity)
+                : linearVelocity(linearVelocity), angularVelocity(angularVelocity) {
+
+            }
+        };
+
+        // -------------------- Methods -------------------- //
+
+        /// Constructor
+        DynamicsComponents(MemoryAllocator& allocator);
+
+        /// Destructor
+        virtual ~DynamicsComponents() override = default;
+
+        /// Add a component
+        void addComponent(Entity bodyEntity, bool isSleeping, const DynamicsComponent& component);
+
+        /// Return the linear velocity of an entity
+        Vector3& getLinearVelocity(Entity bodyEntity) const;
+
+        /// Return the angular velocity of an entity
+        Vector3& getAngularVelocity(Entity bodyEntity) const;
+
+        /// Set the linear velocity of an entity
+        void setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity);
+
+        /// Set the angular velocity of an entity
+        void setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity);
+
+        // -------------------- Friendship -------------------- //
+
+        friend class BroadPhaseSystem;
+};
+
+// Return the linear velocity of an entity
+inline Vector3& DynamicsComponents::getLinearVelocity(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the angular velocity of an entity
+inline Vector3& DynamicsComponents::getAngularVelocity(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Set the linear velocity of an entity
+inline void DynamicsComponents::setLinearVelocity(Entity bodyEntity, const Vector3& linearVelocity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mLinearVelocities[mMapEntityToComponentIndex[bodyEntity]] = linearVelocity;
+}
+
+// Set the angular velocity of an entity
+inline void DynamicsComponents::setAngularVelocity(Entity bodyEntity, const Vector3& angularVelocity) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = angularVelocity;
+}
+
+}
+
+#endif

src/engine/CollisionWorld.cpp

@@ -39,7 +39,8 @@ uint CollisionWorld::mNbWorlds = 0;
 CollisionWorld::CollisionWorld(const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
                : mConfig(worldSettings), mEntityManager(mMemoryManager.getPoolAllocator()),
                  mBodyComponents(mMemoryManager.getBaseAllocator()), mTransformComponents(mMemoryManager.getBaseAllocator()),
-                 mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mMemoryManager),
+                 mProxyShapesComponents(mMemoryManager.getBaseAllocator()), mDynamicsComponents(mMemoryManager.getBaseAllocator()),
+                 mCollisionDetection(this, mProxyShapesComponents, mTransformComponents, mDynamicsComponents, mMemoryManager),
                  mBodies(mMemoryManager.getPoolAllocator()), mCurrentBodyId(0), mFreeBodiesIds(mMemoryManager.getPoolAllocator()),
                  mEventListener(nullptr), mName(worldSettings.worldName),
                  mIsProfilerCreatedByUser(profiler != nullptr),

src/engine/CollisionWorld.h

@@ -36,6 +36,7 @@
 #include "components/BodyComponents.h"
 #include "components/TransformComponents.h"
 #include "components/ProxyShapeComponents.h"
+#include "components/DynamicsComponents.h"
 
 /// Namespace reactphysics3d
 namespace reactphysics3d {
@@ -82,6 +83,9 @@ class CollisionWorld {
         /// Proxy-Shapes Components
         ProxyShapeComponents mProxyShapesComponents;
 
+        /// Dynamics components of the bodies (linear, angular velocities)
+        DynamicsComponents mDynamicsComponents;
+
         /// Reference to the collision detection
         CollisionDetection mCollisionDetection;
 

src/engine/DynamicsWorld.cpp

@@ -214,8 +214,8 @@ void DynamicsWorld::updateBodiesState() {
             uint index = bodies[b]->mArrayIndex;
 
             // Update the linear and angular velocity of the body
-            bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
+            mDynamicsComponents.setLinearVelocity(bodies[b]->getEntity(), mConstrainedLinearVelocities[index]);
-            bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index];
+            mDynamicsComponents.setAngularVelocity(bodies[b]->getEntity(), mConstrainedAngularVelocities[index]);
 
             // Update the position of the center of mass of the body
             bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
@@ -429,6 +429,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
     assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
 
     mTransformComponents.addComponent(entity, false, TransformComponents::TransformComponent(transform));
+    mDynamicsComponents.addComponent(entity, false, DynamicsComponents::DynamicsComponent(Vector3::zero(), Vector3::zero()));
 
     // Create the rigid body
     RigidBody* rigidBody = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,

src/engine/DynamicsWorld.h

@@ -32,6 +32,7 @@
 #include "configuration.h"
 #include "utils/Logger.h"
 #include "engine/ContactSolver.h"
+#include "components/DynamicsComponents.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -216,6 +217,9 @@ 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;
 
@@ -359,6 +363,14 @@ 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

@@ -29,16 +29,17 @@
 #include "utils/Profiler.h"
 #include "collision/RaycastInfo.h"
 #include "memory/MemoryManager.h"
-#include "engine/CollisionWorld.h"
+#include "engine/DynamicsWorld.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
 // Constructor
 BroadPhaseSystem::BroadPhaseSystem(CollisionDetection& collisionDetection, ProxyShapeComponents& proxyShapesComponents,
-                                   TransformComponents& transformComponents)
+                                   TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents)
                     :mDynamicAABBTree(collisionDetection.getMemoryManager().getPoolAllocator(), DYNAMIC_TREE_AABB_GAP),
                      mProxyShapesComponents(proxyShapesComponents), mTransformsComponents(transformComponents),
+                     mDynamicsComponents(dynamicsComponents),
                      mMovedShapes(collisionDetection.getMemoryManager().getPoolAllocator()),
                      mPotentialPairs(collisionDetection.getMemoryManager().getPoolAllocator()),
                      mCollisionDetection(collisionDetection) {
@@ -157,6 +158,13 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end
     startIndex = std::min(startIndex, mProxyShapesComponents.getNbEnabledComponents());
     endIndex = std::min(endIndex, mProxyShapesComponents.getNbEnabledComponents());
 
+    // 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 < endIndex; i++) {
 
@@ -166,9 +174,17 @@ void BroadPhaseSystem::updateProxyShapesComponents(uint32 startIndex, uint32 end
             const Entity& bodyEntity = mProxyShapesComponents.mBodiesEntities[i];
             const Transform& transform = mTransformsComponents.getTransform(bodyEntity);
 
+            Vector3 displacement(0, 0, 0);
+
+            // If there is a dynamics component for the current entity
+            if (mDynamicsComponents.hasComponent(bodyEntity)) {
+
+                // Get the linear velocity from the dynamics component
+                const Vector3& linearVelocity = mDynamicsComponents.getLinearVelocity(bodyEntity);
+
                 // TODO : Use body linear velocity and compute displacement
-            const Vector3 displacement = Vector3::zero();
+                displacement = timeStep * linearVelocity;
-            //const Vector3 displacement = world.mTimeStep * mLinearVelocity;
+            }
 
             // Recompute the world-space AABB of the collision shape
             AABB aabb;

src/systems/BroadPhaseSystem.h

@@ -32,6 +32,7 @@
 #include "containers/Set.h"
 #include "components/ProxyShapeComponents.h"
 #include "components/TransformComponents.h"
+#include "components/DynamicsComponents.h"
 
 /// Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -148,6 +149,9 @@ class BroadPhaseSystem {
         /// Reference to the transform components
         TransformComponents& mTransformsComponents;
 
+        /// Reference to the dynamics components
+        DynamicsComponents& mDynamicsComponents;
+
         /// Set with the broad-phase IDs of all collision shapes that have moved (or have been
         /// created) during the last simulation step. Those are the shapes that need to be tested
         /// for overlapping in the next simulation step.
@@ -179,7 +183,7 @@ class BroadPhaseSystem {
 
         /// Constructor
         BroadPhaseSystem(CollisionDetection& collisionDetection, ProxyShapeComponents& proxyShapesComponents,
-                         TransformComponents &transformComponents);
+                         TransformComponents& transformComponents, DynamicsComponents& dynamicsComponents);
 
         /// Destructor
         ~BroadPhaseSystem() = default;