Add initMass and massInverse to DynamicsComponents

src/body/RigidBody.cpp

@@ -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)) {
+    if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
-        mMassInverse = decimal(1.0) / mInitMass;
+        mWorld.mDynamicsComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mDynamicsComponents.getInitMass(mEntity));
     }
     else {
-        mInitMass = decimal(1.0);
+        mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(1.0));
-        mMassInverse = 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);
+    mWorld.mDynamicsComponents.setInitMass(mEntity, decimal(0.0));
-    mMassInverse = 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)) {
+    if (mWorld.mDynamicsComponents.getInitMass(mEntity) > decimal(0.0)) {
-        mMassInverse = decimal(1.0) / mInitMass;
+        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;

src/body/RigidBody.h

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

src/components/DynamicsComponents.cpp

@@ -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(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) +
-                                           sizeof(Vector3) + sizeof(bool)) {
+                                           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

src/components/DynamicsComponents.h

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

src/constraint/BallAndSocketJoint.cpp

@@ -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;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    const decimal inverseMassBody2 = constraintSolverData.dynamicsComponents.getMassInverse(mBody2Entity);
 
     // Recompute the inverse inertia tensors
     mI1 = mBody1->getInertiaTensorInverseWorld();

src/constraint/FixedJoint.cpp

@@ -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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    const decimal inverseMassBody2 = mBody2->mMassInverse;
+    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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    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) +

src/constraint/HingeJoint.cpp

@@ -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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    const decimal inverseMassBody2 = mBody2->mMassInverse;
+    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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    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) +

src/constraint/SliderJoint.cpp

@@ -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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    const decimal inverseMassBody2 = mBody2->mMassInverse;
+    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 inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    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;
+    const decimal inverseMassBody1 = constraintSolverData.dynamicsComponents.getMassInverse(mBody1Entity);
-    decimal inverseMassBody2 = mBody2->mMassInverse;
+    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) ?

src/engine/ContactSolver.cpp

@@ -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].massInverseBody1 = mDynamicsComponents.getMassInverse(body1->getEntity());
-        mContactConstraints[mNbContactManifolds].massInverseBody2 = body2->mMassInverse;
+        mContactConstraints[mNbContactManifolds].massInverseBody2 = mDynamicsComponents.getMassInverse(body2->getEntity());
         mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.getNbContactPoints();
         mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2);
         mContactConstraints[mNbContactManifolds].rollingResistanceFactor = computeMixedRollingResistance(body1, body2);

src/engine/DynamicsWorld.cpp

@@ -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() +
+            mDynamicsComponents.setConstrainedLinearVelocity(bodyEntity, mDynamicsComponents.getLinearVelocity(bodyEntity) +
-                                        mTimeStep * body->mMassInverse * mDynamicsComponents.getExternalForce(bodyEntity));
+                                        mTimeStep * mDynamicsComponents.getMassInverse(bodyEntity) * mDynamicsComponents.getExternalForce(bodyEntity));
-            mDynamicsComponents.setConstrainedAngularVelocity(bodyEntity, body->getAngularVelocity() +
+            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);
             }