From aa4935f39695a0bf0a3192e13fcec1ffbb19cd18 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 17 May 2019 17:39:30 +0200 Subject: [PATCH] Add external force/torque in DynamicsComponents --- src/body/RigidBody.cpp | 85 +++++++++++++++++++++++++-- src/body/RigidBody.h | 82 -------------------------- src/components/DynamicsComponents.cpp | 20 ++++++- src/components/DynamicsComponents.h | 50 ++++++++++++++++ src/engine/DynamicsWorld.cpp | 7 +-- src/engine/DynamicsWorld.h | 7 +-- 6 files changed, 154 insertions(+), 97 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 43eea1ab..906dbc70 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -114,8 +114,38 @@ void RigidBody::setType(BodyType type) { askForBroadPhaseCollisionCheck(); // Reset the force and torque on the body - mExternalForce.setToZero(); - mExternalTorque.setToZero(); + mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero()); + 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(); - mExternalTorque.setToZero(); + mWorld.mDynamicsComponents.setExternalForce(mEntity, Vector3::zero()); + mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero()); } } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 49e087a4..7c96bb78 100644 --- a/src/body/RigidBody.h +++ b/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 diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index 9a1cbe28..ac9b32dc 100644 --- a/src/components/DynamicsComponents.cpp +++ b/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(newConstrainedLinearVelocities + nbComponentsToAllocate); Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newSplitAngularVelocities + nbComponentsToAllocate); + Vector3* newExternalForces = reinterpret_cast(newSplitAngularVelocities + nbComponentsToAllocate); + Vector3* newExternalTorques = reinterpret_cast(newExternalForces + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(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(); } diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index adb23497..57737807 100644 --- a/src/components/DynamicsComponents.h +++ b/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 { diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index dd9903f6..5f9d1700 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/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) { diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index a1fa3717..45f8f9dd 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -261,10 +261,9 @@ class DynamicsWorld : public CollisionWorld { inline void DynamicsWorld::resetBodiesForceAndTorque() { // For each body of the world - List::Iterator it; - for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - (*it)->mExternalForce.setToZero(); - (*it)->mExternalTorque.setToZero(); + for (uint32 i=0; i < mDynamicsComponents.getNbComponents(); i++) { + mDynamicsComponents.mExternalForces[i].setToZero(); + mDynamicsComponents.mExternalTorques[i].setToZero(); } }