Fixed and optimised sphere vs convex polyhedron

Improved sphere vs convex polyhedron from O(n^2) to O(n). Math is amazing.

Assets/Scenes/PhysicsSandbox.shade

@@ -4,9 +4,9 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -1.80977702, y: 3, z: 0}
+      Translate: {x: 2.58071685, y: 0.456140399, z: 0}
-      Rotate: {x: -0, y: 0, z: -0.506194055}
+      Rotate: {x: -0, y: 0, z: 0.469853699}
-      Scale: {x: 3.27252102, y: 0.999997199, z: 1}
+      Scale: {x: 4.61071014, y: 0.999995053, z: 1}
       IsActive: true
     RigidBody Component:
       Type: Static
@@ -19,7 +19,7 @@
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
-      Freeze Position Y: false
+      Freeze Position Y: true
       Freeze Position Z: false
       Freeze Rotation X: false
       Freeze Rotation Y: false
@@ -60,54 +60,13 @@
       Perspective: true
       IsActive: true
   Scripts: ~
-- EID: 2
-  Name: Default
-  IsActive: true
-  NumberOfChildren: 0
-  Components:
-    Transform Component:
-      Translate: {x: 0, y: 0, z: 0}
-      Rotate: {x: -0, y: 0, z: -0}
-      Scale: {x: 1, y: 1, z: 1}
-      IsActive: true
-    RigidBody Component:
-      Type: Static
-      Auto Mass: false
-      Mass: .inf
-      Drag: 0.00999999978
-      Angular Drag: 0.00999999978
-      Use Gravity: false
-      Gravity Scale: 1
-      Interpolate: true
-      Sleeping Enabled: true
-      Freeze Position X: false
-      Freeze Position Y: false
-      Freeze Position Z: false
-      Freeze Rotation X: false
-      Freeze Rotation Y: false
-      Freeze Rotation Z: false
-      IsActive: true
-    Collider Component:
-      DrawColliders: false
-      Colliders:
-        - Is Trigger: false
-          Collision Tag: 1
-          Type: Sphere
-          Radius: 2.5
-          Friction: 0.400000006
-          Bounciness: 0
-          Density: 1
-          Position Offset: {x: 0, y: 0, z: 0}
-          Rotation Offset: {x: 0, y: 0, z: 0}
-      IsActive: true
-  Scripts: ~
 - EID: 3
   Name: Default
   IsActive: true
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -1.97624588, y: 5, z: 0}
+      Translate: {x: -2.24715948, y: 6.47200441, z: 0}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -146,3 +105,183 @@
       Enabled: true
       forceAmount: 50
       torqueAmount: 500
+- EID: 2
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: 4.09544182, z: 0}
+      Rotate: {x: -0, y: 0, z: -0.437829614}
+      Scale: {x: 4.61071014, y: 0.999995887, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: true
+      Freeze Position Y: true
+      Freeze Position Z: true
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 500
+- EID: 4
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0, y: -1.74209237, z: 0}
+      Rotate: {x: -0, y: 0, z: -0}
+      Scale: {x: 10, y: 0.5, z: 10}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 500
+- EID: 5
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -4.80025721, y: 3, z: 0}
+      Rotate: {x: -0, y: 0, z: 1.57079601}
+      Scale: {x: 9.99975109, y: 0.499992192, z: 10}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 500
+- EID: 6
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 4.80000019, y: 3, z: 0}
+      Rotate: {x: -0, y: 0, z: 1.57079601}
+      Scale: {x: 9.99975109, y: 0.499992192, z: 10}
+      IsActive: true
+    RigidBody Component:
+      Type: Static
+      Auto Mass: false
+      Mass: .inf
+      Drag: 0.00999999978
+      Angular Drag: 0.00999999978
+      Use Gravity: true
+      Gravity Scale: 1
+      Interpolate: true
+      Sleeping Enabled: true
+      Freeze Position X: false
+      Freeze Position Y: true
+      Freeze Position Z: false
+      Freeze Rotation X: false
+      Freeze Rotation Y: false
+      Freeze Rotation Z: false
+      IsActive: true
+    Collider Component:
+      DrawColliders: false
+      Colliders:
+        - Is Trigger: false
+          Collision Tag: 1
+          Type: Box
+          Half Extents: {x: 1, y: 1, z: 1}
+          Friction: 0.400000006
+          Bounciness: 0
+          Density: 1
+          Position Offset: {x: 0, y: 0, z: 0}
+          Rotation Offset: {x: 0, y: 0, z: 0}
+      IsActive: true
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 500

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHBoxCollisionShape.cpp

@@ -160,6 +160,41 @@ namespace SHADE
       throw std::invalid_argument("Index out-of-range!");
 
     return GetVertices()[index];
+
+    //// Rotate half extents
+    //const SHVec3 ROTATED_EXTENTS = SHVec3::Rotate(Extents, Orientation);
+    //const SHVec3 CENTER { Center };
+
+    ///*
+    // * Front: -Z
+    // *
+    // * 3 _____ 2
+    // *  |     |
+    // *  |     |
+    // *  |_____|
+    // * 0       1
+    // *
+    // * Back: +Z
+    // *
+    // * 7 _____ 6
+    // *  |     |
+    // *  |     |
+    // *  |_____|
+    // * 4       5
+    // *
+    // */
+
+    //switch (index)
+    //{
+    //  case 0: return CENTER + SHVec3 { -ROTATED_EXTENTS.x, -ROTATED_EXTENTS.y, -ROTATED_EXTENTS.z }
+    //  case 1: return CENTER + SHVec3 { 
+    //  case 2: return CENTER + SHVec3 { 
+    //  case 3: return CENTER + SHVec3 { 
+    //  case 4: return CENTER + SHVec3 { 
+    //  case 5: return CENTER + SHVec3 { 
+    //  case 6: return CENTER + SHVec3 { 
+    //  case 7: return CENTER + SHVec3 { 
+    //}
   }
 
   SHVec3 SHBoxCollisionShape::GetNormal(int faceIndex) const

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp

@@ -126,9 +126,9 @@ namespace SHADE
      *
      * Faces:
      *
-     * Front:   0 (0,1,2,3) Normal:  Z
+     * Front:   0 (0,1,2,3) Normal: -Z
      * Right:   1 (1,5,6,2) Normal:  X
-     * Back:    2 (5,4,7,6) Normal: -Z
+     * Back:    2 (5,4,7,6) Normal:  Z
      * Left:    3 (4,0,3,7) Normal: -X
      * Top:     4 (3,2,6,7) Normal:  Y
      * Bottom:  5 (4,5,1,0) Normal: -Y

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

@@ -52,8 +52,8 @@ namespace SHADE
     const SHSphereCollisionShape&           SPHERE  = dynamic_cast<const SHSphereCollisionShape&>(A);
     const SHConvexPolyhedronCollisionShape& CONVEX  = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B);
 
-    // Ensure a gap between A & B
+    const SHVec3 CENTER       = SPHERE.GetCenter();
-    const float TOTAL_RADIUS  = SPHERE.GetWorldRadius() + CONVEX.RADIUS;
+    const float TOTAL_RADIUS  = SPHERE.GetWorldRadius() + SHConvexPolyhedronCollisionShape::RADIUS;
 
     // Find closest face of polygon to circle
 
@@ -64,56 +64,49 @@ namespace SHADE
     const SHHalfEdgeDS* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure();
 
     /*
-     * Test against each face
+     * Test against each face.
      *
-     * TODO:
+     * 1. For each face, build a plane in point-normal form.
-     * This check is now O(n^2) because we find the closest point.
+     * 2. Find the signed distance from plane to center of sphere.
-     * It can be optimised to O(n) by utilising the following steps:
+     * 3. Save best distance and face.
-     * 1. Rotate sphere into polyhedron's space
-     * 2. Build a plane equation from the face in point-normal form. We need the plane's offset from the origin.
-     * 3. Compute distance to the face.
      */
     for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i)
     {
       const SHHalfEdgeDS::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i);
 
-      // TODO: Remove and optimise
+      // Build plane equation
-      // Find the closest point on the face to the sphere
-      const int32_t NUM_VERTICES = static_cast<int32_t>(FACE.vertexIndices.size());
-      for (int32_t j = 0; j < NUM_VERTICES; ++j)
-      {
-        // Get vector from center to a vertex on the face
-        const SHVec3 A_TO_B = SPHERE.GetCenter() - CONVEX.GetVertex(FACE.vertexIndices[j]);
 
-        const float PROJECTION = SHVec3::Dot(A_TO_B, FACE.normal);
+      const SHVec3  POINT   = CONVEX.GetVertex(FACE.vertexIndices[0]);  // use the first vertex to build a plane
+      const SHVec3& NORMAL  = CONVEX.GetNormal(i);
+      const float   D       = -SHVec3::Dot(NORMAL, POINT);
 
-        // Early out
+      // Find signed distance of center to plane
-        if (PROJECTION > TOTAL_RADIUS)
+      const float SIGNED_DIST = SHVec3::Dot(NORMAL, CENTER) + D;
+
+      // Early out:
+      // If face is facing away from center, signed dist is negative.
+      // Therefore signed distance is only positive when sphere is in front of the face.
+      if (SIGNED_DIST > TOTAL_RADIUS)
         return false;
 
-        if (PROJECTION > bestDistance)
+      if (SIGNED_DIST > bestDistance)
       {
-          bestDistance      = PROJECTION;
+        bestDistance = SIGNED_DIST;
         closestFaceIndex = i;
-          closestPointIndex = j;
-        }
       }
     }
 
     uint32_t    numContacts   = 0;
-
-    // Rotate the normal into the world space
-    const SHVec3& BEST_NORMAL = CONVEX.GetNormal(closestFaceIndex);
     const float PENETRATION   = TOTAL_RADIUS - bestDistance;
 
     // Check if center is inside polyhedron (below the face)
     if (bestDistance < SHMath::EPSILON)
     {
-      manifold.normal = -BEST_NORMAL;
+      manifold.normal = CONVEX.GetNormal(closestFaceIndex);
 
       SHContact newContact;
       newContact.penetration      = PENETRATION;
-      newContact.position         = SPHERE.GetCenter() - BEST_NORMAL * PENETRATION;
+      newContact.position         = SPHERE.GetCenter();
       newContact.featurePair.key  = 0;
 
       manifold.contacts[numContacts++] = newContact;
@@ -135,10 +128,25 @@ namespace SHADE
      *  /           /   regionC
      */
 
-    const SHHalfEdgeDS::Face& CLOSEST_FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex);
+    const SHHalfEdgeDS::Face& FACE  = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex);
-    const int32_t NUM_VERTICES_ON_FACE = static_cast<int32_t>(CLOSEST_FACE.vertexIndices.size());
+    const SHVec3& NORMAL            = CONVEX.GetNormal(closestFaceIndex);
 
-    const SHVec3 C            = CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[closestPointIndex]);
+    const int32_t NUM_VERTICES_ON_FACE = static_cast<int32_t>(FACE.vertexIndices.size());
+
+    // Find closest point on face
+    closestPointIndex = 0;
+    float smallestDist = SHVec3::Dot(CENTER - CONVEX.GetVertex(FACE.vertexIndices[0]), NORMAL);
+
+    for (int32_t i = 1; i < NUM_VERTICES_ON_FACE; ++i)
+    {
+      const SHVec3 POINT = CONVEX.GetVertex(FACE.vertexIndices[i]);
+      const float PROJECTION = SHVec3::Dot(CENTER - POINT, NORMAL);
+
+      if (PROJECTION < smallestDist)
+        closestPointIndex = i;
+    }
+
+    const SHVec3 C            = CONVEX.GetVertex(FACE.vertexIndices[closestPointIndex]);
     const SHVec3 C_TO_CENTER  = SPHERE.GetCenter() - C;
 
     const int32_t INDEX_A = (closestPointIndex + 1) % NUM_VERTICES_ON_FACE;
@@ -146,8 +154,8 @@ namespace SHADE
 
     const SHVec3 POINTS[2] =
     {
-      CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_A]) // A
+      CONVEX.GetVertex(FACE.vertexIndices[INDEX_A]) // A
-    , CONVEX.GetVertex(CLOSEST_FACE.vertexIndices[INDEX_B]) // B
+    , CONVEX.GetVertex(FACE.vertexIndices[INDEX_B]) // B
     };
 
     // To be inside either region A or B, 2 conditions must be satisfied
@@ -170,7 +178,7 @@ namespace SHADE
         projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER);
         if (projection >= 0.0f)
         {
-          // Sphere Within region A
+          // Sphere Within region
           manifold.normal = CP_TO_CENTER;
 
           SHContact newContact;
@@ -190,7 +198,7 @@ namespace SHADE
     {
       if (C_TO_CENTER.LengthSquared() < TOTAL_RADIUS * TOTAL_RADIUS)
       {
-        manifold.normal = SHVec3::Normalise(C_TO_CENTER);
+        manifold.normal = -SHVec3::Normalise(C_TO_CENTER);
 
         SHContact newContact;
         newContact.penetration      = PENETRATION;
@@ -207,11 +215,11 @@ namespace SHADE
     // Region D
     if (PENETRATION <= TOTAL_RADIUS)
     {
-      manifold.normal = -BEST_NORMAL;
+      manifold.normal = -NORMAL;
 
       SHContact newContact;
       newContact.penetration      = PENETRATION;
-      newContact.position         = SPHERE.GetCenter() - BEST_NORMAL * TOTAL_RADIUS;
+      newContact.position         = SPHERE.GetCenter() - NORMAL * TOTAL_RADIUS;
       newContact.featurePair.key  = 0;
 
       manifold.contacts[numContacts++] = newContact;

SHADE_Engine/src/Physics/Dynamics/SHContactManager.cpp

@@ -63,7 +63,6 @@ namespace SHADE
   const SHContactManager::ContactPoints& SHContactManager::GetContactPoints() const noexcept
   {
     static ContactPoints contactPoints;
-
     contactPoints.clear();
 
     for (auto& manifold : manifolds | std::views::values)