Add external force/torque in DynamicsComponents

src/body/RigidBody.cpp

@@ -114,8 +114,38 @@ void RigidBody::setType(BodyType type) {
     askForBroadPhaseCollisionCheck();
 
     // Reset the force and torque on the body
-    mExternalForce.setToZero();
+    mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero());
-    mExternalTorque.setToZero();
+    mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
+}
+
+// Apply an external force to the body at a given point (in world-space coordinates).
+/// If the point is not at the center of mass of the body, it will also
+/// generate some torque and therefore, change the angular velocity of 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 forces and that this sum will be
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The force to apply on the body
+ * @param point The point where the force is applied (in world-space coordinates)
+ */
+inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != BodyType::DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the force
+    const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity);
+    mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force);
+
+    // Add the torque
+    const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
+    mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + (point - mCenterOfMassWorld).cross(force));
 }
 
 // Set the local inertia tensor of the body (in local-space coordinates)
@@ -142,6 +172,29 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
              "Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
 }
 
+// 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
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param force The external force to apply on the center of mass of the body
+ */
+inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != BodyType::DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the force
+    const Vector3& externalForce = mWorld.mDynamicsComponents.getExternalForce(mEntity);
+    mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force);
+}
+
 // Set the inverse local inertia tensor of the body (in local-space coordinates)
 /// If the inverse inertia tensor is set with this method, it will not be computed
 /// using the collision shapes of the body.
@@ -598,16 +651,40 @@ Vector3 RigidBody::getAngularVelocity() const {
     return mWorld.mDynamicsComponents.getAngularVelocity(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
+/// reset to zero at the end of each call of the DynamicsWorld::update() method.
+/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
+/**
+ * @param torque The external torque to apply on the body
+ */
+inline void RigidBody::applyTorque(const Vector3& torque) {
+
+    // If it is not a dynamic body, we do nothing
+    if (mType != BodyType::DYNAMIC) return;
+
+    // Awake the body if it was sleeping
+    if (mIsSleeping) {
+        setIsSleeping(false);
+    }
+
+    // Add the torque
+    const Vector3& externalTorque = mWorld.mDynamicsComponents.getExternalTorque(mEntity);
+    mWorld.mDynamicsComponents.setExternalTorque(mEntity, externalTorque + torque);
+}
+
 // Set the variable to know whether or not the body is sleeping
 void RigidBody::setIsSleeping(bool isSleeping) {
 
     CollisionBody::setIsSleeping(isSleeping);
 
     if (isSleeping) {
+
         mWorld.mDynamicsComponents.setLinearVelocity(mEntity, Vector3::zero());
         mWorld.mDynamicsComponents.setAngularVelocity(mEntity, Vector3::zero());
-        mExternalForce.setToZero();
+        mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero());
-        mExternalTorque.setToZero();
+        mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
     }
 }
 

src/body/RigidBody.h

@@ -70,18 +70,6 @@ class RigidBody : public CollisionBody {
         /// Center of mass of the body in world-space coordinates
         Vector3 mCenterOfMassWorld;
 
-        /// Linear velocity of the body
-        //Vector3 mLinearVelocity;
-
-        /// Angular velocity of the body
-        //Vector3 mAngularVelocity;
-
-        /// Current external force on the body
-        Vector3 mExternalForce;
-
-        /// Current external torque on the body
-        Vector3 mExternalTorque;
-
         /// Inverse Local inertia tensor of the body (in local-space) set
         /// by the user with respect to the center of mass of the body
         Matrix3x3 mUserInertiaTensorLocalInverse;
@@ -330,76 +318,6 @@ inline JointListElement* RigidBody::getJointsList() {
     return mJointsList;
 }
 
-// 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
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param force The external force to apply on the center of mass of the body
- */
-inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
-
-    // If it is not a dynamic body, we do nothing
-    if (mType != BodyType::DYNAMIC) return;
-
-    // Awake the body if it was sleeping
-    if (mIsSleeping) {
-        setIsSleeping(false);
-    }
-
-    // Add the force
-    mExternalForce += force;
-}
-
-// Apply an external force to the body at a given point (in world-space coordinates).
-/// If the point is not at the center of mass of the body, it will also
-/// generate some torque and therefore, change the angular velocity of 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 forces and that this sum will be
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param force The force to apply on the body
- * @param point The point where the force is applied (in world-space coordinates)
- */
-inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
-
-    // If it is not a dynamic body, we do nothing
-    if (mType != BodyType::DYNAMIC) return;
-
-    // Awake the body if it was sleeping
-    if (mIsSleeping) {
-        setIsSleeping(false);
-    }
-
-    // Add the force and torque
-    mExternalForce += force;
-    mExternalTorque += (point - mCenterOfMassWorld).cross(force);
-}
-
-// 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
-/// reset to zero at the end of each call of the DynamicsWorld::update() method.
-/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
-/**
- * @param torque The external torque to apply on the body
- */
-inline void RigidBody::applyTorque(const Vector3& torque) {
-
-    // If it is not a dynamic body, we do nothing
-    if (mType != BodyType::DYNAMIC) return;
-
-    // Awake the body if it was sleeping
-    if (mIsSleeping) {
-        setIsSleeping(false);
-    }
-
-    // Add the torque
-    mExternalTorque += torque;
-}
-
 }
 
  #endif

src/components/DynamicsComponents.cpp

@@ -36,7 +36,7 @@ using namespace reactphysics3d;
 // Constructor
 DynamicsComponents::DynamicsComponents(MemoryAllocator& allocator)
                     :Components(allocator, sizeof(Entity) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
-                                           sizeof(Vector3) + sizeof(Vector3) + sizeof(bool)) {
+                                           sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool)) {
 
     // Allocate memory for the components data
     allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -62,7 +62,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
     Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
     Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
-    bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newSplitAngularVelocities + nbComponentsToAllocate);
+    Vector3* newExternalForces = reinterpret_cast<Vector3*>(newSplitAngularVelocities + nbComponentsToAllocate);
+    Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
+    bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newExternalTorques + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -75,6 +77,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
         memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
         memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3));
+        memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3));
+        memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3));
         memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
 
         // Deallocate previous memory
@@ -89,6 +93,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
     mConstrainedAngularVelocities = newConstrainedAngularVelocities;
     mSplitLinearVelocities = newSplitLinearVelocities;
     mSplitAngularVelocities = newSplitAngularVelocities;
+    mExternalForces = newExternalForces;
+    mExternalTorques = newExternalTorques;
     mIsAlreadyInIsland = newIsAlreadyInIsland;
     mNbAllocatedComponents = nbComponentsToAllocate;
 }
@@ -107,6 +113,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
     new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
     new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
     new (mSplitAngularVelocities + index) Vector3(0, 0, 0);
+    new (mExternalForces + index) Vector3(0, 0, 0);
+    new (mExternalTorques + index) Vector3(0, 0, 0);
     mIsAlreadyInIsland[index] = false;
 
     // Map the entity with the new component lookup index
@@ -132,6 +140,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
     new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
     new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
     new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]);
+    new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]);
+    new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]);
     mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
 
     // Destroy the source component
@@ -159,6 +169,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
     Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
     Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]);
+    Vector3 externalForce1(mExternalForces[index1]);
+    Vector3 externalTorque1(mExternalTorques[index1]);
     bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
 
     // Destroy component 1
@@ -174,6 +186,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
     new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
     new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
     new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1);
+    new (mExternalForces + index2) Vector3(externalForce1);
+    new (mExternalTorques + index2) Vector3(externalTorque1);
     mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
 
     // Update the entity to component index mapping
@@ -200,4 +214,6 @@ void DynamicsComponents::destroyComponent(uint32 index) {
     mConstrainedAngularVelocities[index].~Vector3();
     mSplitLinearVelocities[index].~Vector3();
     mSplitAngularVelocities[index].~Vector3();
+    mExternalForces[index].~Vector3();
+    mExternalTorques[index].~Vector3();
 }

src/components/DynamicsComponents.h

@@ -71,6 +71,12 @@ class DynamicsComponents : public Components {
         /// Array with the split angular velocity of each component
         Vector3* mSplitAngularVelocities;
 
+        /// Array with the external force of each component
+        Vector3* mExternalForces;
+
+        /// Array with the external torque of each component
+        Vector3* mExternalTorques;
+
         /// Array with the boolean value to know if the body has already been added into an island
         bool* mIsAlreadyInIsland;
 
@@ -132,6 +138,12 @@ class DynamicsComponents : public Components {
         /// Return the split angular velocity of an entity
         const Vector3& getSplitAngularVelocity(Entity bodyEntity) const;
 
+        /// Return the external force of an entity
+        const Vector3& getExternalForce(Entity bodyEntity) const;
+
+        /// Return the external torque of an entity
+        const Vector3& getExternalTorque(Entity bodyEntity) const;
+
         /// Return true if the entity is already in an island
         bool getIsAlreadyInIsland(Entity bodyEntity) const;
 
@@ -153,6 +165,12 @@ class DynamicsComponents : public Components {
         /// Set the split angular velocity of an entity
         void setSplitAngularVelocity(Entity bodyEntity, const Vector3& splitAngularVelocity);
 
+        /// Set the external force of an entity
+        void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
+
+        /// Set the external force of an entity
+        void setExternalTorque(Entity bodyEntity, const Vector3& externalTorque);
+
         /// Set the value to know if the entity is already in an island
         bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
 
@@ -234,6 +252,22 @@ inline const Vector3& DynamicsComponents::getSplitAngularVelocity(Entity bodyEnt
    return mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]];
 }
 
+// Return the external force of an entity
+inline const Vector3& DynamicsComponents::getExternalForce(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mExternalForces[mMapEntityToComponentIndex[bodyEntity]];
+}
+
+// Return the external torque of an entity
+inline const Vector3& DynamicsComponents::getExternalTorque(Entity bodyEntity) const {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   return mExternalTorques[mMapEntityToComponentIndex[bodyEntity]];
+}
+
 // Set the constrained linear velocity of an entity
 inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
 
@@ -266,6 +300,22 @@ inline void DynamicsComponents::setSplitAngularVelocity(Entity bodyEntity, const
    mSplitAngularVelocities[mMapEntityToComponentIndex[bodyEntity]] = splitAngularVelocity;
 }
 
+// Set the external force of an entity
+inline void DynamicsComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mExternalForces[mMapEntityToComponentIndex[bodyEntity]] = externalForce;
+}
+
+// Set the external force of an entity
+inline void DynamicsComponents::setExternalTorque(Entity bodyEntity, const Vector3& externalTorque) {
+
+   assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
+
+   mExternalTorques[mMapEntityToComponentIndex[bodyEntity]] = externalTorque;
+}
+
 // Return true if the entity is already in an island
 inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
 

src/engine/DynamicsWorld.cpp

@@ -234,9 +234,6 @@ void DynamicsWorld::updateBodiesState() {
             // Update the orientation of the body
             mTransformComponents.getTransform(bodyEntity).setOrientation(mConstrainedOrientations[index].getUnit());
 
-            // TODO : REMOVE THIS
-            assert(mConstrainedOrientations[index].lengthSquare() < 1.5 * 1.5);
-
             // Update the transform of the body (using the new center of mass and new orientation)
             body->updateTransformWithCenterOfMass();
 
@@ -318,9 +315,9 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
 
             // Integrate the external force to get the new velocity of the body
             mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, body->getLinearVelocity() +
-                                        mTimeStep * body->mMassInverse * body->mExternalForce);
+                                        mTimeStep * body->mMassInverse * mDynamicsComponents.getExternalForce(bodyEntity));
             mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, body->getAngularVelocity() +
-                                        mTimeStep * body->getInertiaTensorInverseWorld() * body->mExternalTorque);
+                                        mTimeStep * body->getInertiaTensorInverseWorld() * mDynamicsComponents.getExternalTorque(bodyEntity));
 
             // If the gravity has to be applied to this rigid body
             if (body->isGravityEnabled() && mIsGravityEnabled) {

src/engine/DynamicsWorld.h

@@ -261,10 +261,9 @@ class DynamicsWorld : public CollisionWorld {
 inline void DynamicsWorld::resetBodiesForceAndTorque() {
 
     // For each body of the world
-    List<RigidBody*>::Iterator it;
+    for (uint32 i=0; i < mDynamicsComponents.getNbComponents(); i++) {
-    for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
+        mDynamicsComponents.mExternalForces[i].setToZero();
-        (*it)->mExternalForce.setToZero();
+        mDynamicsComponents.mExternalTorques[i].setToZero();
-        (*it)->mExternalTorque.setToZero();
     }
 }