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

Add initMass and massInverse to DynamicsComponents

Browse Source
This commit is contained in:
Daniel Chappuis 2019-05-18 21:52:51 +02:00
parent 29c8587c85
commit ed4f76f7c6
10 changed files with 145 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/body/RigidBody.cpp
View File

@ -40,13 +40,13 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, Entity entity, bodyindex id)
: CollisionBody(world, entity, id), mArrayIndex(0), mInitMass(decimal(1.0)),
: CollisionBody(world, entity, id), mArrayIndex(0),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mMaterial(world.mConfig),
mJointsList(nullptr), mIsCenterOfMassSetByUser(false), mIsInertiaTensorSetByUser(false) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
@ -91,12 +91,12 @@ void RigidBody::setType(BodyType type) {
if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
// Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0);
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0));
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
if (mIsInertiaTensorSetByUser) {
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
@ -118,6 +118,14 @@ void RigidBody::setType(BodyType type) {
mWorld.mDynamicsComponents.setExternalTorque(mEntity, Vector3::zero());
}
// Method that return the mass of the body
/**
* @return The mass (in kilograms) of the body
*/
decimal RigidBody::getMass() const {
return mWorld.mDynamicsComponents.getInitMass(mEntity);
}
// 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.
@ -274,14 +282,14 @@ void RigidBody::setMass(decimal mass) {
if (mType != BodyType::DYNAMIC) return;
mInitMass = mass;
mWorld.mDynamicsComponents.setInitMass(mEntity, mass);
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
}
else {
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(1.0));
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0));
}
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
@ -558,8 +566,8 @@ void RigidBody::setTransform(const Transform& transform) {
// the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0));
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(0.0));
if (!mIsInertiaTensorSetByUser) mInertiaTensorLocalInverse.setToZero();
if (!mIsInertiaTensorSetByUser) mInertiaTensorInverseWorld.setToZero();
if (!mIsCenterOfMassSetByUser) mCenterOfMassLocal.setToZero();
@ -580,15 +588,15 @@ void RigidBody::recomputeMassInformation() {
const List<Entity>& proxyShapesEntities = mWorld.mBodyComponents.getProxyShapes(mEntity);
for (uint i=0; i < proxyShapesEntities.size(); i++) {
ProxyShape* proxyShape = mWorld.mProxyShapesComponents.getProxyShape(proxyShapesEntities[i]);
mInitMass += proxyShape->getMass();
mWorld.mDynamicsComponents.setInitMass(mEntity, mWorld.mDynamicsComponents.getInitMass(mEntity) + proxyShape->getMass());
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal += proxyShape->getLocalToBodyTransform().getPosition() * proxyShape->getMass();
}
}
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
}
else {
mCenterOfMassWorld = transform.getPosition();
@ -599,7 +607,7 @@ void RigidBody::recomputeMassInformation() {
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
if (!mIsCenterOfMassSetByUser) {
mCenterOfMassLocal *= mMassInverse;
mCenterOfMassLocal *= mWorld.mDynamicsComponents.getMassInverse(mEntity);
}
mCenterOfMassWorld = transform * mCenterOfMassLocal;

14
src/body/RigidBody.h
View File

@ -60,9 +60,6 @@ class RigidBody : public CollisionBody {
// -------------------- Attributes -------------------- //
/// Intial mass of the body
decimal mInitMass;
/// Center of mass of the body in local-space coordinates.
/// The center of mass can therefore be different from the body origin
Vector3 mCenterOfMassLocal;
@ -80,9 +77,6 @@ class RigidBody : public CollisionBody {
/// Inverse of the world inertia tensor of the body
Matrix3x3 mInertiaTensorInverseWorld;
/// Inverse of the mass of the body
decimal mMassInverse;
/// True if the gravity needs to be applied to this rigid body
bool mIsGravityEnabled;
@ -235,14 +229,6 @@ class RigidBody : public CollisionBody {
friend class FixedJoint;
};
// Method that return the mass of the body
/**
* @return The mass (in kilograms) of the body
*/
inline decimal RigidBody::getMass() const {
return mInitMass;
}
// Get the inverse local inertia tensor of the body (in body coordinates)
inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
return mInertiaTensorLocalInverse;

20
src/components/DynamicsComponents.cpp
View File

@ -36,8 +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(Vector3) +
sizeof(Vector3) + sizeof(bool)) {
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) +
sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool)) {
// Allocate memory for the components data
allocate(INIT_NB_ALLOCATED_COMPONENTS);
@ -67,7 +67,9 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
Vector3* newExternalTorques = reinterpret_cast<Vector3*>(newExternalForces + nbComponentsToAllocate);
decimal* newLinearDampings = reinterpret_cast<decimal*>(newExternalTorques + nbComponentsToAllocate);
decimal* newAngularDampings = reinterpret_cast<decimal*>(newLinearDampings + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
bool* newIsAlreadyInIsland = reinterpret_cast<bool*>(newInverseMasses + nbComponentsToAllocate);
// If there was already components before
if (mNbComponents > 0) {
@ -84,6 +86,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newExternalTorques, mExternalTorques, mNbComponents * sizeof(Vector3));
memcpy(newLinearDampings, mLinearDampings, mNbComponents * sizeof(decimal));
memcpy(newAngularDampings, mAngularDampings, mNbComponents * sizeof(decimal));
memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool));
// Deallocate previous memory
@ -102,6 +106,8 @@ void DynamicsComponents::allocate(uint32 nbComponentsToAllocate) {
mExternalTorques = newExternalTorques;
mLinearDampings = newLinearDampings;
mAngularDampings = newAngularDampings;
mInitMasses = newInitMasses;
mInverseMasses = newInverseMasses;
mIsAlreadyInIsland = newIsAlreadyInIsland;
mNbAllocatedComponents = nbComponentsToAllocate;
}
@ -124,6 +130,8 @@ void DynamicsComponents::addComponent(Entity bodyEntity, bool isSleeping, const
new (mExternalTorques + index) Vector3(0, 0, 0);
mLinearDampings[index] = decimal(0.0);
mAngularDampings[index] = decimal(0.0);
mInitMasses[index] = decimal(1.0);
mInverseMasses[index] = decimal(1.0);
mIsAlreadyInIsland[index] = false;
// Map the entity with the new component lookup index
@ -153,6 +161,8 @@ void DynamicsComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex)
new (mExternalTorques + destIndex) Vector3(mExternalTorques[srcIndex]);
mLinearDampings[destIndex] = mLinearDampings[srcIndex];
mAngularDampings[destIndex] = mAngularDampings[srcIndex];
mInitMasses[destIndex] = mInitMasses[srcIndex];
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
// Destroy the source component
@ -184,6 +194,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
Vector3 externalTorque1(mExternalTorques[index1]);
decimal linearDamping1 = mLinearDampings[index1];
decimal angularDamping1 = mAngularDampings[index1];
decimal initMass1 = mInitMasses[index1];
decimal inverseMass1 = mInverseMasses[index1];
bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1];
// Destroy component 1
@ -203,6 +215,8 @@ void DynamicsComponents::swapComponents(uint32 index1, uint32 index2) {
new (mExternalTorques + index2) Vector3(externalTorque1);
mLinearDampings[index2] = linearDamping1;
mAngularDampings[index2] = angularDamping1;
mInitMasses[index2] = initMass1;
mInverseMasses[index2] = inverseMass1;
mIsAlreadyInIsland[index2] = isAlreadyInIsland1;
// Update the entity to component index mapping

50
src/components/DynamicsComponents.h
View File

@ -83,6 +83,12 @@ class DynamicsComponents : public Components {
/// Array with the angular damping factor of each component
decimal* mAngularDampings;
/// Array with the initial mass of each component
decimal* mInitMasses;
/// Array with the inverse mass of each component
decimal* mInverseMasses;
/// Array with the boolean value to know if the body has already been added into an island
bool* mIsAlreadyInIsland;
@ -156,6 +162,12 @@ class DynamicsComponents : public Components {
/// Return the angular damping factor of an entity
decimal getAngularDamping(Entity bodyEntity) const;
/// Return the initial mass of an entity
decimal getInitMass(Entity bodyEntity) const;
/// Return the mass inverse of an entity
decimal getMassInverse(Entity bodyEntity) const;
/// Return true if the entity is already in an island
bool getIsAlreadyInIsland(Entity bodyEntity) const;
@ -189,6 +201,12 @@ class DynamicsComponents : public Components {
/// Set the angular damping factor of an entity
void setAngularDamping(Entity bodyEntity, decimal angularDamping);
/// Set the initial mass of an entity
void setInitMass(Entity bodyEntity, decimal initMass);
/// Set the inverse mass of an entity
void setMassInverse(Entity bodyEntity, decimal inverseMass);
/// Set the value to know if the entity is already in an island
bool setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland);
@ -300,6 +318,22 @@ inline decimal DynamicsComponents::getAngularDamping(Entity bodyEntity) const {
return mAngularDampings[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the initial mass of an entity
inline decimal DynamicsComponents::getInitMass(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mInitMasses[mMapEntityToComponentIndex[bodyEntity]];
}
// Return the inverse mass of an entity
inline decimal DynamicsComponents::getMassInverse(Entity bodyEntity) const {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mInverseMasses[mMapEntityToComponentIndex[bodyEntity]];
}
// Set the constrained linear velocity of an entity
inline void DynamicsComponents::setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity) {
@ -364,6 +398,22 @@ inline void DynamicsComponents::setAngularDamping(Entity bodyEntity, decimal ang
mAngularDampings[mMapEntityToComponentIndex[bodyEntity]] = angularDamping;
}
// Set the initial mass of an entity
inline void DynamicsComponents::setInitMass(Entity bodyEntity, decimal initMass) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mInitMasses[mMapEntityToComponentIndex[bodyEntity]] = initMass;
}
// Set the mass inverse of an entity
inline void DynamicsComponents::setMassInverse(Entity bodyEntity, decimal inverseMass) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mInverseMasses[mMapEntityToComponentIndex[bodyEntity]] = inverseMass;
}
// Return true if the entity is already in an island
inline bool DynamicsComponents::getIsAlreadyInIsland(Entity bodyEntity) const {

16
src/constraint/BallAndSocketJoint.cpp
View File

@ -68,7 +68,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix)
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -113,14 +115,14 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World);
// Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1;
v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World);
// Apply the impulse to the body to the body 2
v2 += mBody2->mMassInverse * mImpulse;
v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * mImpulse;
w2 += mI2 * angularImpulseBody2;
}
@ -148,14 +150,14 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World);
// Apply the impulse to the body 1
v1 += mBody1->mMassInverse * linearImpulseBody1;
v1 += constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity) * linearImpulseBody1;
w1 += mI1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World);
// Apply the impulse to the body 2
v2 += mBody2->mMassInverse * deltaLambda;
v2 += constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity) * deltaLambda;
w2 += mI2 * angularImpulseBody2;
}
@ -173,8 +175,8 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();

18
src/constraint/FixedJoint.cpp
View File

@ -83,7 +83,9 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -140,8 +142,8 @@ void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -mImpulseTranslation;
@ -178,8 +180,8 @@ void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// --------------- Translation Constraints --------------- //
@ -239,8 +241,8 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -257,7 +259,7 @@ void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +

20
src/constraint/HingeJoint.cpp
View File

@ -117,7 +117,9 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
@ -209,8 +211,8 @@ void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -mB2CrossA1 * mImpulseRotation.x - mC2CrossA1 * mImpulseRotation.y;
@ -268,8 +270,8 @@ void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintS
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// --------------- Translation Constraints --------------- //
@ -418,8 +420,8 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inverse inertia tensors
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -455,7 +457,9 @@ void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintS
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = mBody1->mMassInverse + mBody2->mMassInverse;
const decimal body1InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal body2InverseMass = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +

28
src/constraint/SliderJoint.cpp
View File

@ -128,7 +128,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1->getEntity());
const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2->getEntity());
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
@ -174,7 +176,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
mInverseMassMatrixLimit = sumInverseMass +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
@ -197,7 +199,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
if (mIsMotorEnabled) {
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
mInverseMassMatrixMotor = mBody1->mMassInverse + mBody2->mMassInverse;
mInverseMassMatrixMotor = sumInverseMass;
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
}
@ -227,8 +229,8 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mBody1->mMassInverse;
const decimal inverseMassBody2 = mBody2->mMassInverse;
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
@ -289,8 +291,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
Vector3& w2 = constraintSolverData.dynamicsComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// --------------- Translation Constraints --------------- //
@ -450,8 +452,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
Quaternion& q2 = constraintSolverData.orientations[mIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mBody1->mMassInverse;
decimal inverseMassBody2 = mBody2->mMassInverse;
const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
// Recompute the inertia tensor of bodies
mI1 = mBody1->getInertiaTensorInverseWorld();
@ -490,7 +492,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// constraints (2x2 matrix)
decimal sumInverseMass = mBody1->mMassInverse + mBody2->mMassInverse;
const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
decimal sumInverseMass = body1MassInverse + body2MassInverse;
Vector3 I1R1PlusUCrossN1 = mI1 * mR1PlusUCrossN1;
Vector3 I1R1PlusUCrossN2 = mI1 * mR1PlusUCrossN2;
Vector3 I2R2CrossN1 = mI2 * mR2CrossN1;
@ -610,7 +614,9 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
if (mIsLowerLimitViolated || mIsUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mInverseMassMatrixLimit = mBody1->mMassInverse + mBody2->mMassInverse +
const decimal body1MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
const decimal body2MassInverse = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
mInverseMassMatrixLimit = body1MassInverse + body2MassInverse +
mR1PlusUCrossSliderAxis.dot(mI1 * mR1PlusUCrossSliderAxis) +
mR2CrossSliderAxis.dot(mI2 * mR2CrossSliderAxis);
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?

4
src/engine/ContactSolver.cpp
View File

@ -143,8 +143,8 @@ void ContactSolver::initializeForIsland(uint islandIndex) {
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mDynamicsComponents.getEntityIndex(body2->getEntity());
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
mContactConstraints[mNbContactManifolds].massInverseBody1 = body1->mMassInverse;
mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
mContactConstraints[mNbContactManifolds].massInverseBody1 = mDynamicsComponents.getMassInverse(body1->getEntity());
mContactConstraints[mNbContactManifolds].massInverseBody2 = mDynamicsComponents.getMassInverse(body2->getEntity());
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints();
mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);

9
src/engine/DynamicsWorld.cpp
View File

@ -314,16 +314,17 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
assert(indexBody < mRigidBodies.size());
// Integrate the external force to get the new velocity of the body
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, body->getLinearVelocity() +
mTimeStep * body->mMassInverse * mDynamicsComponents.getExternalForce(bodyEntity));
mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, body->getAngularVelocity() +
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 * body->mMassInverse *
mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getConstrainedLinearVelocity(bodyEntity) +
mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) *
body->getMass() * mGravity);
}