Reverted changes to inertia tensors

Created a new branch since I reverted to an older commit but kept some new updates

Assets/Scenes/PhysicsSandbox.shade

@@ -45,9 +45,9 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: -0.5, y: 10, z: -3}
+      Position: {x: 0, y: 0, z: 7}
       Pitch: 0
-      Yaw: 180
+      Yaw: 0
       Roll: 0
       Width: 1920
       Height: 1080
@@ -71,7 +71,7 @@
       Auto Mass: false
       Mass: 10
       Drag: 0.00999999978
-      Angular Drag: 0.00999999978
+      Angular Drag: 0
       Use Gravity: true
       Gravity Scale: 1
       Interpolate: true
@@ -95,15 +95,6 @@
           Density: 1
           Position Offset: {x: 0, y: 0, z: 0}
           Rotation Offset: {x: 0, y: 0, z: 0}
-        - Is Trigger: true
-          Collision Tag: 1
-          Type: Sphere
-          Radius: 0.5
-          Friction: 0.400000006
-          Bounciness: 0
-          Density: 1
-          Position Offset: {x: 0.75, y: 0.5, z: 0}
-          Rotation Offset: {x: 0, y: 0, z: 0}
       IsActive: true
   Scripts:
     - Type: PhysicsTestObj
@@ -280,7 +271,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.524352431, y: 13.5, z: 0.0463808179}
+      Translate: {x: 1, y: 2, z: 3}
       Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
       Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
       IsActive: true
@@ -321,7 +312,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -0.5, y: 10, z: 0}
+      Translate: {x: 0, y: 0, z: 3}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true

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

SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h

@@ -31,22 +31,16 @@ namespace SHADE
     /* Type Definit                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    enum class Type : uint8_t
-    {
-      VERTEX  = 0
-    , FACE    = 1
-    };
-
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
     struct
     {
-      uint8_t indexA;
+      uint8_t inI;
-      uint8_t indexB;
+      uint8_t outI;
-      uint8_t typeA;
+      uint8_t inR;
-      uint8_t typeB;
+      uint8_t outR;
     };
 
     uint32_t key = std::numeric_limits<uint32_t>::max();

SHADE_Engine/src/Physics/Collision/Narrowphase/SHCollision.h

@@ -82,6 +82,7 @@ namespace SHADE
     {
       int32_t halfEdgeA     = -1;
       int32_t halfEdgeB     = -1;
+      int32_t axis          = -1;
       float   bestDistance  = std::numeric_limits<float>::lowest();
     };
 

SHADE_Engine/src/Physics/Collision/Narrowphase/SHConvexVsConvex.cpp

@@ -48,7 +48,8 @@ namespace SHADE
 
   bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
-    static constexpr float TOLERANCE  = 0.1f + SHPHYSICS_LINEAR_SLOP;
+    static constexpr float ABSOLUTE_TOLERANCE = 0.01f;
+    static constexpr float RELATIVE_TOLERANCE = 0.95f;
 
     const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
     const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
@@ -72,7 +73,7 @@ namespace SHADE
     const SHConvexPolyhedron* incidentPoly  = nullptr;
 
     FaceQuery minFaceQuery;
-    if (FACE_QUERY_A.bestDistance + TOLERANCE > FACE_QUERY_B.bestDistance)
+    if (FACE_QUERY_A.bestDistance + ABSOLUTE_TOLERANCE > FACE_QUERY_B.bestDistance * RELATIVE_TOLERANCE)
     {
       minFaceQuery  = FACE_QUERY_A;
       referencePoly = &POLY_A;
@@ -91,7 +92,7 @@ namespace SHADE
 
     // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
     // each edge and use that as the contact point.
-    if (EDGE_QUERY.bestDistance > minFaceQuery.bestDistance + TOLERANCE)
+    if (EDGE_QUERY.bestDistance * RELATIVE_TOLERANCE > minFaceQuery.bestDistance + ABSOLUTE_TOLERANCE)
     {
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
@@ -111,11 +112,7 @@ namespace SHADE
 
       // In this scenario, we only have one contact
       SHContact contact;
-      contact.featurePair.typeA   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      contact.featurePair.key = EDGE_QUERY.axis;
-      contact.featurePair.indexA  = HALF_EDGE_A.tailVertexIndex;
-      contact.featurePair.typeB   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
-      contact.featurePair.indexB  = HALF_EDGE_B.tailVertexIndex;
-
       contact.position        = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
       contact.penetration     = EDGE_QUERY.bestDistance;
 
@@ -188,6 +185,7 @@ namespace SHADE
     const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount();
     const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount();
 
+    int32_t axis = -1;
     for (int32_t i = 0; i < EDGE_COUNT_A; i += 2)
     {
       for (int32_t j = 0; j < EDGE_COUNT_B; j += 2)
@@ -196,12 +194,15 @@ namespace SHADE
         if (!IS_MINKOWSKI_FACE)
           continue;
 
+        ++axis;
+
         const float SEPARATION = distanceBetweenEdges(A, B, i, j);
         if (SEPARATION > edgeQuery.bestDistance)
         {
           edgeQuery.bestDistance = SEPARATION;
           edgeQuery.halfEdgeA = i;
           edgeQuery.halfEdgeB = j;
+          edgeQuery.axis      = axis;
         }
       }
     }
@@ -321,10 +322,10 @@ namespace SHADE
      * R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 )
      */
 
-    const float A2_OVER_A1  = VB_DOT_VB / VB_DOT_VA;
+    const float A2_OVER_A1  = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA;
     const float NUMERATOR   = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
     const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
-    const float R = NUMERATOR / DENOMINATOR;
+    const float R = DENOMINATOR == 0.0f ? NUMERATOR : NUMERATOR / DENOMINATOR;
 
     // Just take a point from A since it's A -> B
     return TAIL_A + R * VA;

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSphereVsConvex.cpp

@@ -179,7 +179,13 @@ namespace SHADE
 
   bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
-    return SphereVsConvex(manifold, B, A);
+    if (SphereVsConvex(manifold, B, A))
+    {
+      manifold.normal = -manifold.normal;
+      return true;
+    };
+
+    return false;
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -326,7 +332,13 @@ namespace SHADE
     // Face to vertex is in the opposite direction of any tangent.
     const float PROJECTION = SHVec3::Dot(FACE_TO_CENTER, tangent1);
     if (PROJECTION < 0)
+    {
+      const float DISTANCE_SQUARED = SHVec3::DistanceSquared(faceVertex, CENTER);
+      if (DISTANCE_SQUARED > RADIUS * RADIUS)
+        return 0;
+
       return 3;
+    }
 
     // Belongs in region D by default
     return 4;

SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -238,6 +238,8 @@ namespace SHADE
 
   void SHContactManager::updateManifold(SHManifold& manifold, const SHManifold& oldManifold) noexcept
   {
+    static const float SQRT_ONE_THIRD = std::sqrtf(1.0f / 3.0f);
+
     // Early out since exiting a collision does not require an update beyond updating the state
     if (manifold.state == SHCollisionState::EXIT)
       return;
@@ -250,7 +252,7 @@ namespace SHADE
     const SHVec3& OLD_TANGENT_1 = oldManifold.tangents[1];
 
     // Compute tangents
-    if (std::fabs(NORMAL.x) >= SHMath::EULER_CONSTANT)
+    if (std::fabs(NORMAL.x) >= SQRT_ONE_THIRD)
       tangent0 = SHVec3{ NORMAL.y, -NORMAL.x, 0.0f };
     else
       tangent0 = SHVec3{ 0.0f, NORMAL.z, -NORMAL.y };

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -187,14 +187,7 @@ namespace SHADE
          * restituion bias  = restitution * (relative velocity /dot normal)
          */
 
-        const SHVec3  RV_A = vA + SHVec3::Cross(wA, contact.rA);
-        const SHVec3  RV_B = vB + SHVec3::Cross(wB, contact.rB);
-        const float   RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal);
-
         const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP);
-        const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
-
-        contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
 
         // Warm starting
         // Compute impulses
@@ -208,6 +201,14 @@ namespace SHADE
 
         vB += impulse * constraint.invMassB * LINEAR_LOCK_B;
         wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, impulse) * ANGULAR_LOCK_B;
+
+        const SHVec3  RV_A = vA + SHVec3::Cross(wA, contact.rA);
+        const SHVec3  RV_B = vB + SHVec3::Cross(wB, contact.rB);
+        const float   RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal);
+
+        const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
+
+        contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
       }
 
       velocityStateA.LinearVelocity   = vA;

SHADE_Engine/src/Physics/SHPhysicsConstants.h

@@ -26,7 +26,7 @@ namespace SHADE
    * @brief
    * Linear Collision & Constraint tolerance.
   */
-  static constexpr float SHPHYSICS_LINEAR_SLOP              = 0.005f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
+  static constexpr float SHPHYSICS_LINEAR_SLOP              = 0.05f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
 
   /**
    * @brief