From 1b885c887872ce9bb54ae158182291fdd1dc641b Mon Sep 17 00:00:00 2001
From: Diren D Bharwani <diren.bharwani@gmail.com>
Date: Thu, 12 Jan 2023 23:23:04 +0800
Subject: [PATCH] Fixed a potential infinite loop in AABB Tree

This behaviour should've thrown an exception wtf???
---
 .../Broadphase/SHDynamicAABBTree.cpp          |  2 +-
 .../Shapes/SHCollisionShapeLibrary.cpp        |  4 +--
 .../src/Physics/Dynamics/SHRigidBody.cpp      | 35 ++++++++-----------
 3 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp b/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp
index dc87d706..7177e517 100644
--- a/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp
+++ b/SHADE_Engine/src/Physics/Collision/Broadphase/SHDynamicAABBTree.cpp
@@ -337,7 +337,7 @@ namespace SHADE
   {
     SHASSERT(index >= 0 && index < capacity, "Trying to free an invalid AABB Tree node!")
 
-    nodes[index].next     = NULL_NODE;
+    nodes[index].next     = freeList;
     nodes[index].height   = NULL_NODE;
 
     // Put it back on the free list
diff --git a/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp b/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp
index c70728f3..dde4c8f7 100644
--- a/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp
+++ b/SHADE_Engine/src/Physics/Collision/Shapes/SHCollisionShapeLibrary.cpp
@@ -127,9 +127,9 @@ namespace SHADE
      * Faces:
      *
      * Front:   0 (0,1,2,3) Normal: -Z
-     * Right:   1 (1,5,6,2) Normal:  X
+     * Right:   1 (5,6,2,1) Normal:  X
      * Back:    2 (5,4,7,6) Normal:  Z
-     * Left:    3 (4,0,3,7) Normal: -X
+     * Left:    3 (0,3,7,4) Normal: -X
      * Top:     4 (3,2,6,7) Normal:  Y
      * Bottom:  5 (4,5,1,0) Normal: -Y
      *
diff --git a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp
index 57c2d90e..b690d487 100644
--- a/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp
+++ b/SHADE_Engine/src/Physics/Dynamics/SHRigidBody.cpp
@@ -644,29 +644,24 @@ namespace SHADE
     const SHMatrix RT = SHMatrix::Transpose(R);
 
     /*
-      Compute world inertia as a Vector
-      
-      | a b c || x |   | ax + by + cz |
-      | d e f || y | = | dx + ey + fz |
-      | g h i || z |   | gx + hy + iz |
-    */
+     *  Compute world inertia as a Vector
+     *  
+     *  | a b c || x |   | ax + by + cz |
+     *  | d e f || y | = | dx + ey + fz |
+     *  | g h i || z |   | gx + hy + iz |
+     */
     
-    SHMatrix tmp = SHMatrix
-    {
-      SHVec4 { localInvInertia.x, 0.0f, 0.0f, 0.0f }
-    , SHVec4 { 0.0f, localInvInertia.y, 0.0f, 0.0f }
-    , SHVec4 { 0.0f, 0.0f, localInvInertia.z, 0.0f }
-    , SHVec4 { 0.0f, 0.0f, 0.0, 1.0f }
-    };
+    SHMatrix localInertiaTensor = SHMatrix::Identity;
 
-    tmp *= RT;
+    // Set the diagonals
+    localInertiaTensor.m[0][0] = localInvInertia.x;
+    localInertiaTensor.m[1][1] = localInvInertia.y;
+    localInertiaTensor.m[2][2] = localInvInertia.z;
 
-    worldInvInertia = SHVec3
-    {
-      R.m[0][0] * tmp.m[0][0] + RT.m[0][1] * tmp.m[1][1] + RT.m[0][2] * tmp.m[2][2]
-    , R.m[1][0] * tmp.m[0][0] + RT.m[1][1] * tmp.m[1][1] + RT.m[1][2] * tmp.m[2][2]
-    , R.m[2][0] * tmp.m[0][0] + RT.m[2][1] * tmp.m[1][1] + RT.m[2][2] * tmp.m[2][2]
-    };
+    localInertiaTensor *= RT;
+    const SHVec3 DIAGONALS { localInertiaTensor.m[0][0], localInertiaTensor.m[1][1], localInertiaTensor.m[2][2] };
+
+    worldInvInertia = R * DIAGONALS;
 
     // Compute world centroid
     worldCentroid = (R * localCentroid) + motionState.position;