Refactor the getter/setter for inertia tensor of a RigidBody

src/body/RigidBody.cpp

@@ -91,14 +91,13 @@ void RigidBody::setType(BodyType type) {
 
         // Reset the inverse mass and inverse inertia tensor to zero
         mMassInverse = decimal(0.0);
-        mInertiaTensorLocal.setToZero();
         mInertiaTensorLocalInverse.setToZero();
         mInertiaTensorInverseWorld.setToZero();
 
     }
     else {  // If it is a dynamic body
         mMassInverse = decimal(1.0) / mInitMass;
-        mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+        mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
 
         // Update the world inverse inertia tensor
         updateInertiaTensorInverseWorld();
@@ -126,12 +125,26 @@ void RigidBody::setType(BodyType type) {
  */
 void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
 
+    mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
+
     if (mType != BodyType::DYNAMIC) return;
 
-    mInertiaTensorLocal = inertiaTensorLocal;
+    // Compute the inverse local inertia tensor
+    mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
+
+    // Update the world inverse inertia tensor
+    updateInertiaTensorInverseWorld();
+}
+
+// Set the inverse local inertia tensor of the body (in body coordinates)
+void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal) {
+
+    mUserInertiaTensorLocalInverse = inverseInertiaTensorLocal;
+
+    if (mType != BodyType::DYNAMIC) return;
 
     // Compute the inverse local inertia tensor
-    mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+    mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();
@@ -346,12 +359,13 @@ void RigidBody::recomputeMassInformation() {
 
     mInitMass = decimal(0.0);
     mMassInverse = decimal(0.0);
-    mInertiaTensorLocal.setToZero();
     mInertiaTensorLocalInverse.setToZero();
     mInertiaTensorInverseWorld.setToZero();
     mCenterOfMassLocal.setToZero();
+    Matrix3x3 inertiaTensorLocal;
+    inertiaTensorLocal.setToZero();
 
-    // If it is STATIC or KINEMATIC body
+    // If it is a STATIC or a KINEMATIC body
     if (mType == BodyType::STATIC || mType == BodyType::KINEMATIC) {
         mCenterOfMassWorld = mTransform.getPosition();
         return;
@@ -403,11 +417,11 @@ void RigidBody::recomputeMassInformation() {
         offsetMatrix[2] += offset * (-offset.z);
         offsetMatrix *= shape->getMass();
 
-        mInertiaTensorLocal += inertiaTensor + offsetMatrix;
+        inertiaTensorLocal += inertiaTensor + offsetMatrix;
     }
 
     // Compute the local inverse inertia tensor
-    mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
+    mInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
 
     // Update the world inverse inertia tensor
     updateInertiaTensorInverseWorld();

src/body/RigidBody.h

@@ -81,9 +81,9 @@ class RigidBody : public CollisionBody {
         /// Current external torque on the body
         Vector3 mExternalTorque;
 
-        /// Local inertia tensor of the body (in local-space) with respect to the
+        /// Inverse Local inertia tensor of the body (in local-space) set
-        /// center of mass of the body
+        /// by the user with respect to the center of mass of the body
-        Matrix3x3 mInertiaTensorLocal;
+        Matrix3x3 mUserInertiaTensorLocalInverse;
 
         /// Inverse of the inertia tensor of the body
         Matrix3x3 mInertiaTensorLocalInverse;
@@ -163,24 +163,24 @@ class RigidBody : public CollisionBody {
         /// Set the variable to know whether or not the body is sleeping
         virtual void setIsSleeping(bool isSleeping) override;
 
-        /// Return the local inertia tensor of the body (in body coordinates)
-        const Matrix3x3& getInertiaTensorLocal() const;
-
         /// Set the local inertia tensor of the body (in body coordinates)
         void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
 
+        /// Set the inverse local inertia tensor of the body (in body coordinates)
+        void setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTensorLocal);
+
+        /// Get the inverse local inertia tensor of the body (in body coordinates)
+        const Matrix3x3& getInverseInertiaTensorLocal() const;
+
+        /// Return the inverse of the inertia tensor in world coordinates.
+        Matrix3x3 getInertiaTensorInverseWorld() const;
+
         /// Set the local center of mass of the body (in local-space coordinates)
         void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
 
         /// Set the mass of the rigid body
         void setMass(decimal mass);
 
-        /// Return the inertia tensor in world coordinates.
-        Matrix3x3 getInertiaTensorWorld() const;
-
-        /// Return the inverse of the inertia tensor in world coordinates.
-        Matrix3x3 getInertiaTensorInverseWorld() const;
-
         /// Return true if the gravity needs to be applied to this rigid body
         bool isGravityEnabled() const;
 
@@ -273,28 +273,9 @@ inline Vector3 RigidBody::getAngularVelocity() const {
     return mAngularVelocity;
 }
 
-// Return the local inertia tensor of the body (in local-space coordinates)
+// Get the inverse local inertia tensor of the body (in body coordinates)
-/**
+inline const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
- * @return The 3x3 inertia tensor matrix of the body (in local-space coordinates)
+    return mInertiaTensorLocalInverse;
- */
-inline const Matrix3x3& RigidBody::getInertiaTensorLocal() const {
-    return mInertiaTensorLocal;
-}
-
-// Return the inertia tensor in world coordinates.
-/// The inertia tensor I_w in world coordinates is computed
-/// with the local inertia tensor I_b in body coordinates
-/// by I_w = R * I_b * R^T
-/// where R is the rotation matrix (and R^T its transpose) of
-/// the current orientation quaternion of the body
-/**
- * @return The 3x3 inertia tensor matrix of the body in world-space coordinates
- */
-inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
-
-    // Compute and return the inertia tensor in world coordinates
-    return mTransform.getOrientation().getMatrix() * mInertiaTensorLocal *
-           mTransform.getOrientation().getMatrix().getTranspose();
 }
 
 // Return the inverse of the inertia tensor in world coordinates.

src/engine/ContactSolver.cpp

@@ -259,8 +259,17 @@ void ContactSolver::initializeForIsland(Island* island) {
         bool isBody2DynamicType = body2->getType() == BodyType::DYNAMIC;
         mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
         if (mContactConstraints[mNbContactManifolds].rollingResistanceFactor > 0 && (isBody1DynamicType || isBody2DynamicType)) {
+
             mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2;
-            mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
+            decimal det = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getDeterminant();
+
+            // If the matrix is not inversible
+            if (approxEqual(det, decimal(0.0))) {
+               mContactConstraints[mNbContactManifolds].inverseRollingResistance.setToZero();
+            }
+            else {
+               mContactConstraints[mNbContactManifolds].inverseRollingResistance = mContactConstraints[mNbContactManifolds].inverseRollingResistance.getInverse();
+            }
         }
 
         mContactConstraints[mNbContactManifolds].normal.normalize();