From 29c8587c8548babcae74c509aebc2e13c17b8bee Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 18 May 2019 14:00:25 +0200 Subject: [PATCH] Add linear/angular damping into DynamicsComponents --- src/body/RigidBody.cpp | 26 +++++++++++--- src/body/RigidBody.h | 22 ------------ src/components/DynamicsComponents.cpp | 19 ++++++++-- src/components/DynamicsComponents.h | 52 +++++++++++++++++++++++++-- src/engine/DynamicsWorld.cpp | 4 +-- 5 files changed, 90 insertions(+), 33 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 906dbc70..c7f6a878 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -42,7 +42,7 @@ using namespace reactphysics3d; RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id) : CollisionBody(world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)), mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()), - mIsGravityEnabled(true), mMaterial(world.mConfig), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), + mIsGravityEnabled(true), mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) { // Compute the inverse mass @@ -195,6 +195,22 @@ inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) { mWorld.mDynamicsComponents.setExternalForce(mEntity, externalForce + force); } +// Return the linear velocity damping factor +/** + * @return The linear damping factor of this body + */ +decimal RigidBody::getLinearDamping() const { + return mWorld.mDynamicsComponents.getLinearDamping(mEntity); +} + +// Return the angular velocity damping factor +/** + * @return The angular damping factor of this body + */ +decimal RigidBody::getAngularDamping() const { + return mWorld.mDynamicsComponents.getAngularDamping(mEntity); +} + // 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. @@ -427,10 +443,10 @@ void RigidBody::enableGravity(bool isEnabled) { */ void RigidBody::setLinearDamping(decimal linearDamping) { assert(linearDamping >= decimal(0.0)); - mLinearDamping = linearDamping; + mWorld.mDynamicsComponents.setLinearDamping(mEntity, linearDamping); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(mLinearDamping)); + "Body " + std::to_string(mID) + ": Set linearDamping=" + std::to_string(linearDamping)); } // Set the angular damping factor. This is the ratio of the angular velocity @@ -440,10 +456,10 @@ void RigidBody::setLinearDamping(decimal linearDamping) { */ void RigidBody::setAngularDamping(decimal angularDamping) { assert(angularDamping >= decimal(0.0)); - mAngularDamping = angularDamping; + mWorld.mDynamicsComponents.setAngularDamping(mEntity, angularDamping); RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, - "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(mAngularDamping)); + "Body " + std::to_string(mID) + ": Set angularDamping=" + std::to_string(angularDamping)); } /// Update the transform of the body after a change of the center of mass diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 7c96bb78..c9a072b5 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -89,12 +89,6 @@ class RigidBody : public CollisionBody { /// Material properties of the rigid body Material mMaterial; - /// Linear velocity damping factor - decimal mLinearDamping; - - /// Angular velocity damping factor - decimal mAngularDamping; - /// First element of the linked list of joints involving this body JointListElement* mJointsList; @@ -286,22 +280,6 @@ inline Material& RigidBody::getMaterial() { return mMaterial; } -// Return the linear velocity damping factor -/** - * @return The linear damping factor of this body - */ -inline decimal RigidBody::getLinearDamping() const { - return mLinearDamping; -} - -// Return the angular velocity damping factor -/** - * @return The angular damping factor of this body - */ -inline decimal RigidBody::getAngularDamping() const { - return mAngularDamping; -} - // Return the first element of the linked list of joints involving this body /** * @return The first element of the linked-list of all the joints involving this body diff --git a/src/components/DynamicsComponents.cpp b/src/components/DynamicsComponents.cpp index ac9b32dc..7f3b71d8 100644 --- a/src/components/DynamicsComponents.cpp +++ b/src/components/DynamicsComponents.cpp @@ -36,7 +36,8 @@ 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(Vector3) + sizeof(Vector3) + sizeof(bool)) { + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -64,7 +65,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); Vector3* newExternalForces = reinterpret_cast(newSplitAngularVelocities + nbComponentsToAllocate); Vector3* newExternalTorques = reinterpret_cast(newExternalForces + nbComponentsToAllocate); - bool* newIsAlreadyInIsland = reinterpret_cast(newExternalTorques + nbComponentsToAllocate); + decimal* newLinearDampings = reinterpret_cast(newExternalTorques + nbComponentsToAllocate); + decimal* newAngularDampings = reinterpret_cast(newLinearDampings + nbComponentsToAllocate); + bool* newIsAlreadyInIsland = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -79,6 +82,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newSplitAngularVelocities, mSplitAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newExternalForces, mExternalForces, mNbComponents * sizeof(Vector3)); memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3)); + memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal)); + memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal)); memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); // Deallocate previous memory @@ -95,6 +100,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) { mSplitAngularVelocities = newSplitAngularVelocities; mExternalForces = newExternalForces; mExternalTorques = newExternalTorques; + mLinearDampings = newLinearDampings; + mAngularDampings = newAngularDampings; mIsAlreadyInIsland = newIsAlreadyInIsland; mNbAllocatedComponents = nbComponentsToAllocate; } @@ -115,6 +122,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const new (mSplitAngularVelocities + index) Vector3(0, 0, 0); new (mExternalForces + index) Vector3(0, 0, 0); new (mExternalTorques + index) Vector3(0, 0, 0); + mLinearDampings[index] = decimal(0.0); + mAngularDampings[index] = decimal(0.0); mIsAlreadyInIsland[index] = false; // Map the entity with the new component lookup index @@ -142,6 +151,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex) new (mSplitAngularVelocities + destIndex) Vector3(mSplitAngularVelocities[srcIndex]); new (mExternalForces + destIndex) Vector3(mExternalForces[srcIndex]); new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]); + mLinearDampings[destIndex] = mLinearDampings[srcIndex]; + mAngularDampings[destIndex] = mAngularDampings[srcIndex]; mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; // Destroy the source component @@ -171,6 +182,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { Vector3 splitAngularVelocity1(mSplitAngularVelocities[index1]); Vector3 externalForce1(mExternalForces[index1]); Vector3 externalTorque1(mExternalTorques[index1]); + decimal linearDamping1 = mLinearDampings[index1]; + decimal angularDamping1 = mAngularDampings[index1]; bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; // Destroy component 1 @@ -188,6 +201,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) { new (mSplitAngularVelocities + index2) Vector3(splitAngularVelocity1); new (mExternalForces + index2) Vector3(externalForce1); new (mExternalTorques + index2) Vector3(externalTorque1); + mLinearDampings[index2] = linearDamping1; + mAngularDampings[index2] = angularDamping1; mIsAlreadyInIsland[index2] = isAlreadyInIsland1; // Update the entity to component index mapping diff --git a/src/components/DynamicsComponents.h b/src/components/DynamicsComponents.h index 57737807..a655f578 100644 --- a/src/components/DynamicsComponents.h +++ b/src/components/DynamicsComponents.h @@ -77,6 +77,12 @@ class DynamicsComponents : public Components { /// Array with the external torque of each component Vector3* mExternalTorques; + /// Array with the linear damping factor of each component + decimal* mLinearDampings; + + /// Array with the angular damping factor of each component + decimal* mAngularDampings; + /// Array with the boolean value to know if the body has already been added into an island bool* mIsAlreadyInIsland; @@ -144,6 +150,12 @@ class DynamicsComponents : public Components { /// Return the external torque of an entity const Vector3& getExternalTorque(Entity bodyEntity) const; + /// Return the linear damping factor of an entity + decimal getLinearDamping(Entity bodyEntity) const; + + /// Return the angular damping factor of an entity + decimal getAngularDamping(Entity bodyEntity) const; + /// Return true if the entity is already in an island bool getIsAlreadyInIsland(Entity bodyEntity) const; @@ -171,11 +183,15 @@ class DynamicsComponents : public Components { /// Set the external force of an entity void setExternalTorque(Entity bodyEntity, const Vector3& externalTorque); + /// Set the linear damping factor of an entity + void setLinearDamping(Entity bodyEntity, decimal linearDamping); + + /// Set the angular damping factor of an entity + void setAngularDamping(Entity bodyEntity, decimal angularDamping); + /// Set the value to know if the entity is already in an island bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); - - // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; @@ -268,6 +284,22 @@ inline const Vector3& DynamicsComponents::getExternalTorque(Entity bodyEntity) c return mExternalTorques[mMapEntityToComponentIndex[bodyEntity]]; } +// Return the linear damping factor of an entity +inline decimal DynamicsComponents::getLinearDamping(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mLinearDampings[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the angular damping factor of an entity +inline decimal DynamicsComponents::getAngularDamping(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]]; +} + // Set the constrained linear velocity of an entity inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) { @@ -316,6 +348,22 @@ inline void DynamicsComponents::setExternalTorque(Entity bodyEntity, const Vecto mExternalTorques[mMapEntityToComponentIndex[bodyEntity]] = externalTorque; } +// Set the linear damping factor of an entity +inline void DynamicsComponents::setLinearDamping(Entity bodyEntity, decimal linearDamping) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mLinearDampings[mMapEntityToComponentIndex[bodyEntity]] = linearDamping; +} + +// Set the angular damping factor of an entity +inline void DynamicsComponents::setAngularDamping(Entity bodyEntity, decimal angularDamping) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping; +} + // 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 5f9d1700..7d09fae3 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -340,8 +340,8 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... // => e^(-x) ~ 1 - x // => v2 = v1 * (1 - c * dt) - decimal linDampingFactor = body->getLinearDamping(); - decimal angDampingFactor = body->getAngularDamping(); + decimal linDampingFactor = mDynamicsComponents.getLinearDamping(bodyEntity); + decimal angDampingFactor = mDynamicsComponents.getAngularDamping(bodyEntity); decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep); decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep); mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) * linearDamping);