mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move isGravityEnabled and inertia tensors in DynamicsComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-20 07:12:09 +02:00
parent ed4f76f7c6
commit a11d884ce1
5 changed files with 184 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/body/RigidBody.cpp
View File

@ -42,8 +42,7 @@ using namespace reactphysics3d;
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
: CollisionBody(world, entity, id), mArrayIndex(0),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mMaterial(world.mConfig),
mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
mMaterial(world.mConfig), mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
// Compute the inverse mass
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
@ -92,14 +91,14 @@ void RigidBody::setType(BodyType type) {
// Reset the inverse mass and inverse inertia tensor to zero
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0));
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
}
else { // If it is a dynamic body
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
if (mIsInertiaTensorSetByUser) {
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
}
}
@ -118,6 +117,27 @@ void RigidBody::setType(BodyType type) {
mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
}
// Get the inverse local inertia tensor of the body (in body coordinates)
const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
return mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
}
// Return the inverse of the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
/**
* @return The 3x3 inverse inertia tensor matrix of the body in world-space
* coordinates
*/
Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// Compute and return the inertia tensor in world coordinates
return mWorld.mDynamicsComponents.getInertiaTensorWorldInverse(mEntity);
}
// Method that return the mass of the body
/**
* @return The mass (in kilograms) of the body
@ -171,7 +191,7 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@ -234,7 +254,7 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@ -337,7 +357,8 @@ void RigidBody::updateInertiaTensorInverseWorld() {
// TODO : Make sure we do this in a system
Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix();
mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
const Matrix3x3& inverseInertiaLocalTensor = mWorld.mDynamicsComponents.getInertiaTensorLocalInverse(mEntity);
mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose());
}
// Add a collision shape to the body.
@ -437,11 +458,11 @@ void RigidBody::removeCollisionShape(ProxyShape* proxyShape) {
* @param isEnabled True if you want the gravity to be applied to this body
*/
void RigidBody::enableGravity(bool isEnabled) {
mIsGravityEnabled = isEnabled;
mWorld.mDynamicsComponents.setIsGravityEnabled(mEntity, isEnabled);
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set isGravityEnabled=" +
(mIsGravityEnabled ? "true" : "false"));
(isEnabled ? "true" : "false"));
}
// Set the linear damping factor. This is the ratio of the linear velocity
@ -568,8 +589,8 @@ void RigidBody::recomputeMassInformation() {
mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0));
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0));
if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
if (!mIsInertiaTensorSetByUser) mWorld.mDynamicsComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero();
@ -646,7 +667,7 @@ void RigidBody::recomputeMassInformation() {
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
mWorld.mDynamicsComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
}
// Update the world inverse inertia tensor
@ -675,6 +696,14 @@ Vector3 RigidBody::getAngularVelocity() const {
return mWorld.mDynamicsComponents.getAngularVelocity(mEntity);
}
// Return true if the gravity needs to be applied to this rigid body
/**
* @return True if the gravity is applied to the body
*/
bool RigidBody::isGravityEnabled() const {
return mWorld.mDynamicsComponents.getIsGravityEnabled(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

38
src/body/RigidBody.h
View File

@ -71,15 +71,6 @@ class RigidBody : public CollisionBody {
/// by the user with respect to the center of mass of the body
Matrix3x3 mUserInertiaTensorLocalInverse;
/// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse;
/// Inverse of the world inertia tensor of the body
Matrix3x3 mInertiaTensorInverseWorld;
/// True if the gravity needs to be applied to this rigid body
bool mIsGravityEnabled;
/// Material properties of the rigid body
Material mMaterial;
@ -229,35 +220,6 @@ class RigidBody : public CollisionBody {
friend class FixedJoint;
};
// Get the inverse local inertia tensor of the body (in body coordinates)
inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
return mInertiaTensorLocalInverse;
}
// Return the inverse of the inertia tensor in world coordinates.
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
/**
* @return The 3x3 inverse inertia tensor matrix of the body in world-space
* coordinates
*/
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// Compute and return the inertia tensor in world coordinates
return mInertiaTensorInverseWorld;
}
// Return true if the gravity needs to be applied to this rigid body
/**
* @return True if the gravity is applied to the body
*/
inline bool RigidBody::isGravityEnabled() const {
return mIsGravityEnabled;
}
// Return a reference to the material properties of the rigid body
/**
* @return A reference to the material of the body

28
src/components/DynamicsComponents.cpp
View File

@ -37,7 +37,8 @@ using namespace reactphysics3d;
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(decimal) +
sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool)) {
sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(Matrix3x3) +
sizeof(bool) + sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -69,7 +70,10 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
bool* newIsGravityEnabled = reinterpret_cast<bool*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
@ -88,6 +92,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
memcpy(newIsGravityEnabled, mIsGravityEnabled, mNbComponents * sizeof(bool));
memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
// Deallocate previous memory
@ -108,6 +115,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
mAngularDampings = newAngularDampings;
mInitMasses = newInitMasses;
mInverseMasses = newInverseMasses;
mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
mIsGravityEnabled = newIsGravityEnabled;
mIsAlreadyInIsland = newIsAlreadyInIsland;
mNbAllocatedComponents = nbComponentsToAllocate;
}
@ -132,6 +142,9 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
mAngularDampings[index] = decimal(0.0);
mInitMasses[index] = decimal(1.0);
mInverseMasses[index] = decimal(1.0);
new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
mIsGravityEnabled[index] = true;
mIsAlreadyInIsland[index] = false;
// Map the entity with the new component lookup index
@ -163,6 +176,9 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
mAngularDampings[destIndex] = mAngularDampings[srcIndex];
mInitMasses[destIndex] = mInitMasses[srcIndex];
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
mInverseInertiaTensorsLocal[destIndex] = mInverseInertiaTensorsLocal[srcIndex];
mInverseInertiaTensorsWorld[destIndex] = mInverseInertiaTensorsWorld[srcIndex];
mIsGravityEnabled[destIndex] = mIsGravityEnabled[srcIndex];
mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
// Destroy the source component
@ -196,6 +212,9 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
decimal angularDamping1 = mAngularDampings[index1];
decimal initMass1 = mInitMasses[index1];
decimal inverseMass1 = mInverseMasses[index1];
Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
bool isGravityEnabled1 = mIsGravityEnabled[index1];
bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
// Destroy component 1
@ -217,6 +236,9 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
mAngularDampings[index2] = angularDamping1;
mInitMasses[index2] = initMass1;
mInverseMasses[index2] = inverseMass1;
mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
mIsGravityEnabled[index2] = isGravityEnabled1;
mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
// Update the entity to component index mapping
@ -245,4 +267,6 @@ void DynamicsComponents::destroyComponent(uint32 index) {
mSplitAngularVelocities[index].~Vector3();
mExternalForces[index].~Vector3();
mExternalTorques[index].~Vector3();
mInverseInertiaTensorsLocal[index].~Matrix3x3();
mInverseInertiaTensorsWorld[index].~Matrix3x3();
}

76
src/components/DynamicsComponents.h
View File

@ -30,6 +30,7 @@
#include "mathematics/Transform.h"
#include "engine/Entity.h"
#include "components/Components.h"
#include "mathematics/Matrix3x3.h"
#include "containers/Map.h"
// ReactPhysics3D namespace
@ -89,6 +90,15 @@ class DynamicsComponents : public Components {
/// Array with the inverse mass of each component
decimal* mInverseMasses;
/// Array with the inverse of the inertia tensor of each component
Matrix3x3* mInverseInertiaTensorsLocal;
/// Array with the inverse of the world inertia tensor of each component
Matrix3x3* mInverseInertiaTensorsWorld;
/// True if the gravity needs to be applied to this component
bool* mIsGravityEnabled;
/// Array with the boolean value to know if the body has already been added into an island
bool* mIsAlreadyInIsland;
@ -168,6 +178,15 @@ class DynamicsComponents : public Components {
/// Return the mass inverse of an entity
decimal getMassInverse(Entity bodyEntity) const;
/// Return the inverse local inertia tensor of an entity
const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
/// Return the inverse world inertia tensor of an entity
const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
/// Return true if gravity is enabled for this entity
bool getIsGravityEnabled(Entity bodyEntity) const;
/// Return true if the entity is already in an island
bool getIsAlreadyInIsland(Entity bodyEntity) const;
@ -207,6 +226,15 @@ class DynamicsComponents : public Components {
/// Set the inverse mass of an entity
void setMassInverse(Entity bodyEntity, decimal inverseMass);
/// Set the inverse local inertia tensor of an entity
void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
/// Set the inverse world inertia tensor of an entity
void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
/// Set the value to know if the gravity is enabled for this entity
bool setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled);
/// Set the value to know if the entity is already in an island
bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
@ -334,6 +362,22 @@ inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const {
return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the inverse local inertia tensor of an entity
inline const Matrix3x3& DynamicsComponents::getInertiaTensorLocalInverse(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the inverse world inertia tensor of an entity
inline const Matrix3x3& DynamicsComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
}
// Set the constrained linear velocity of an entity
inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
@ -414,6 +458,30 @@ inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal invers
mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass;
}
// Set the inverse local inertia tensor of an entity
inline void DynamicsComponents::setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse;
}
// Set the inverse world inertia tensor of an entity
inline void DynamicsComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
}
// Return true if gravity is enabled for this entity
inline bool DynamicsComponents::getIsGravityEnabled(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]];
}
// Return true if the entity is already in an island
inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
@ -422,6 +490,14 @@ inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {
return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]];
}
// Set the value to know if the gravity is enabled for this entity
inline bool DynamicsComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mIsGravityEnabled[mMapEntityToComponentIndex[bodyEntity]] = isGravityEnabled;
}
/// Set the value to know if the entity is already in an island
inline bool DynamicsComponents::setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland) {

88
src/engine/DynamicsWorld.cpp
View File

@ -295,60 +295,52 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
// Reset the split velocities of the bodies
resetSplitVelocities();
// TODO : We should loop over non-sleeping dynamic components here and not over islands
// Integration component velocities using force/torque
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
// For each island of the world
for (uint i=0; i < mIslands.getNbIslands(); i++) {
assert(mDynamicsComponents.mSplitLinearVelocities[i] == Vector3(0, 0, 0));
assert(mDynamicsComponents.mSplitAngularVelocities[i] == Vector3(0, 0, 0));
// For each body of the island
for (uint b=0; b < mIslands.bodyEntities[i].size(); b++) {
// Integrate the external force to get the new velocity of the body
mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mLinearVelocities[i] + mTimeStep *
mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.mExternalForces[i];
mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mAngularVelocities[i] +
mTimeStep * mDynamicsComponents.mInverseInertiaTensorsWorld[i] * mDynamicsComponents.mExternalTorques[i];
}
const Entity bodyEntity = mIslands.bodyEntities[i][b];
// Apply gravity force
for (uint32 i=0; i < mDynamicsComponents.getNbEnabledComponents(); i++) {
// If the gravity has to be applied to this rigid body
if (mDynamicsComponents.mIsGravityEnabled[i] && mIsGravityEnabled) {
RigidBody* body = static_cast<RigidBody*>(mBodyComponents.getBody(bodyEntity));
const uint indexBody = body->mArrayIndex;
assert(mDynamicsComponents.getSplitLinearVelocity(bodyEntity) == Vector3(0, 0, 0));
assert(mDynamicsComponents.getSplitAngularVelocity(bodyEntity) == Vector3(0, 0, 0));
assert(indexBody < mRigidBodies.size());
// Integrate the external force to get the new velocity of the body
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getLinearVelocity(bodyEntity) +
mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) * mDynamicsComponents.getExternalForce(bodyEntity));
mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getAngularVelocity(bodyEntity) +
mTimeStep * body->getInertiaTensorInverseWorld() * mDynamicsComponents.getExternalTorque(bodyEntity));
// If the gravity has to be applied to this rigid body
if (body->isGravityEnabled() && mIsGravityEnabled) {
// Integrate the gravity force
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) +
mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) *
body->getMass() * 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)
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);
mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, mDynamicsComponents.getConstrainedAngularVelocity(bodyEntity) * angularDamping);
// Integrate the gravity force
mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] + mTimeStep *
mDynamicsComponents.mInverseMasses[i] * mDynamicsComponents.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 < mDynamicsComponents.getNbEnabledComponents(); i++) {
const decimal linDampingFactor = mDynamicsComponents.mLinearDampings[i];
const decimal angDampingFactor = mDynamicsComponents.mAngularDampings[i];
const decimal linearDamping = pow(decimal(1.0) - linDampingFactor, mTimeStep);
const decimal angularDamping = pow(decimal(1.0) - angDampingFactor, mTimeStep);
mDynamicsComponents.mConstrainedLinearVelocities[i] = mDynamicsComponents.mConstrainedLinearVelocities[i] * linearDamping;
mDynamicsComponents.mConstrainedAngularVelocities[i] = mDynamicsComponents.mConstrainedAngularVelocities[i] * angularDamping;
}
}
// Solve the contacts and constraints