Generalised the Parallel Axis Theorem for computing inertia tensors

Assets/Scenes/PhysicsSandbox.shade

@@ -77,7 +77,7 @@
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
-      Freeze Position Y: true
+      Freeze Position Y: false
       Freeze Position Z: false
       Freeze Rotation X: false
       Freeze Rotation Y: false

SHADE_Engine/src/Math/SHMatrix.cpp

@@ -34,6 +34,14 @@ namespace SHADE
     0.0f, 0.0f, 0.0f, 1.0f
   };
 
+  const SHMatrix SHMatrix::Zero
+  {
+    SHVec4::Zero
+  , SHVec4::Zero
+  , SHVec4::Zero
+  , SHVec4::Zero
+  };
+
   /*-----------------------------------------------------------------------------------*/
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/

SHADE_Engine/src/Math/SHMatrix.h

@@ -45,6 +45,7 @@ namespace SHADE
     static constexpr  size_t  NUM_COLS  = 4U;
 
     static const SHMatrix     Identity;
+    static const SHMatrix     Zero;
 
     /*---------------------------------------------------------------------------------*/
     /* Constructors & Destructor                                                       */

SHADE_Engine/src/Math/Vector/SHVec3.cpp

@@ -372,6 +372,30 @@ namespace SHADE
     return lhs.Cross(rhs);
   }
 
+  SHMatrix SHVec3::OuterProduct(const SHVec3& lhs, const SHVec3& rhs) noexcept
+  {
+    /*
+     *  Outer product is a matrix multiplication u * vT
+     *  3x1 * 1x3 = 3x3
+     *
+     *  | u1 | | v1 v2 v3 |   | u1v1 u1v2 u1v3 |
+     *  | u2 |              = | u2v1 u2v2 u2v3 |
+     *  | u3 |                | u3v1 u3v2 u3v3 |
+     */
+
+    SHMatrix u  = SHMatrix::Zero;
+    SHMatrix vT = SHMatrix::Zero;
+
+    for (int i = 0; i < SIZE; ++i)
+    {
+      u.m[0][i]   = lhs[i];
+      vT.m[i][0]  = rhs[i];
+    }
+
+    return u * vT;
+  }
+
+
   SHVec3 SHVec3::Project(const SHVec3& v, const SHVec3& u) noexcept
   {
     SHVec3 result;

SHADE_Engine/src/Math/Vector/SHVec3.h

@@ -128,6 +128,7 @@ namespace SHADE
     [[nodiscard]] static float    Angle           (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
     [[nodiscard]] static float    Dot             (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
     [[nodiscard]] static SHVec3   Cross           (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
+    [[nodiscard]] static SHMatrix OuterProduct    (const SHVec3& lhs, const SHVec3& rhs)                                            noexcept;
     [[nodiscard]] static SHVec3   Project         (const SHVec3& v, const SHVec3& u)                                                noexcept;
     [[nodiscard]] static SHVec3   Reflect         (const SHVec3& v, const SHVec3& normal)                                           noexcept;
     [[nodiscard]] static SHVec3   Rotate          (const SHVec3& v, const SHVec3& axis, float angleInRad)                           noexcept;

SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp

@@ -14,6 +14,7 @@
 #include "SHRigidBody.h"
 
 // Project Headers
+#include <complex.h>
 #include <numeric>
 
 #include "Physics/Collision/SHCollider.h"
@@ -664,26 +665,24 @@ namespace SHADE
     const float CUSTOM_MASS       = 1.0f / invMass;
     const bool  AUTO_MASS         = IsAutoMassEnabled();
     const bool  INCLUDE_TRIGGERS  = IsTriggerInMassData();
-    const auto& SHAPES            = collider->GetCollisionShapes();
+    
 
     // Compute Total mass and store individual masses if custom mass is being used.
     // Compute local centroid at the same time
 
     // Zero matrix;
-    SHMatrix tmpLocalTensor;
+    SHMatrix J = SHMatrix::Zero;
-    tmpLocalTensor -= SHMatrix::Identity;
 
     float totalMass = 0.0f;
-    std::vector<float> trueMass;
+    std::vector<float> trueMass;  // We store the true masses here for calculating the ratio with custom masses.
 
+    const auto& SHAPES = collider->GetCollisionShapes();
     for (auto* shape : SHAPES)
     {
       // We skip triggers by default
       if (shape->IsTrigger() && !INCLUDE_TRIGGERS)
         continue;
 
-      shape->ComputeTransforms();
-
       // p = m/v, therefore m = pv. This is the true mass of the shape.
       const float MASS = shape->GetDensity() * shape->GetVolume();
       totalMass += MASS;
@@ -705,6 +704,7 @@ namespace SHADE
     const SHMatrix R  = SHMatrix::Rotate(motionState.orientation);
     const SHMatrix RT = SHMatrix::Transpose(R);
 
+    // We need the world centroid to compute the offset of the collider from the body's centroid
     worldCentroid = (R * localCentroid) + motionState.position;
 
     for (size_t i = 0; i < SHAPES.size(); ++i)
@@ -721,31 +721,23 @@ namespace SHADE
         actualMass *= CUSTOM_MASS / totalMass;
 
       // Convert inertia tensor into local-space of the body
+      // R * I * RT = R * (I * RT)
       SHMatrix I  = SHAPE->GetInertiaTensor( actualMass ) * RT;
       I = R * I;
 
       // Parallel Axis Theorem
-      const SHVec3  O   = SHAPE->GetPosition() - worldCentroid;
+      // J = I + m((R /dot R)E_3 - R /outerProduct R)
-      const float   O2  = O.LengthSquared();
+      const SHVec3    R       = SHAPE->GetPosition() - worldCentroid;
+      const float     R_MAG2  = R.LengthSquared();
+      const SHMatrix  R_OX_R  = SHVec3::OuterProduct(R, R);
 
-      SHMatrix offsetMatrix;
+      J += I + actualMass * (SHMatrix::Identity * R_MAG2 - R_OX_R);
-      offsetMatrix -= SHMatrix::Identity;
-      offsetMatrix.m[0][0] = offsetMatrix.m[1][1] = offsetMatrix.m[2][2] = O2;
-
-      const SHVec3 NOX = O * -O.x;
-      const SHVec3 NOY = O * -O.y;
-      const SHVec3 NOZ = O * -O.z;
-
-      offsetMatrix += SHMatrix{ NOX, NOY, NOZ, SHVec4::Zero };
-      offsetMatrix *= actualMass;
-
-      tmpLocalTensor += I + offsetMatrix;
     }
 
     // Set diagonals then invert
-    localInvInertia.m[0][0] = tmpLocalTensor.m[0][0];
+    localInvInertia.m[0][0] = J.m[0][0];
-    localInvInertia.m[1][1] = tmpLocalTensor.m[1][1];
+    localInvInertia.m[1][1] = J.m[1][1];
-    localInvInertia.m[2][2] = tmpLocalTensor.m[2][2];
+    localInvInertia.m[2][2] = J.m[2][2];
 
     localInvInertia = SHMatrix::Inverse(localInvInertia);
   }