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

Add external force/torque in DynamicsComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-17 17:39:30 +02:00
parent 81303fbaeb
commit aa4935f396
6 changed files with 154 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/body/RigidBody.cpp
View File

@ -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());
}
}

82
src/body/RigidBody.h
View File

@ -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

20
src/components/DynamicsComponents.cpp
View File

@ -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();
}

50
src/components/DynamicsComponents.h
View File

@ -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 {

7
src/engine/DynamicsWorld.cpp
View File

@ -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) {

7
src/engine/DynamicsWorld.h
View File

@ -261,10 +261,9 @@ class DynamicsWorld : public CollisionWorld {
inline void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
List<RigidBody*>::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();
}
}