Improved stability of sphere vs convex polyhedron except for one edge case

Assets/Scenes/PhysicsSandbox.shade

@@ -4,9 +4,9 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 2.58071685, y: 0.456140399, z: 0}
+      Translate: {x: 2.72256827, y: 0.501797795, z: -0.0273017883}
-      Rotate: {x: -0, y: 0, z: 0.469853699}
+      Rotate: {x: -1.48352849, y: -4.78713309e-06, z: 0.469859391}
-      Scale: {x: 4.61071014, y: 0.999995053, z: 1}
+      Scale: {x: 4.61070776, y: 0.99999392, z: 0.999996722}
       IsActive: true
     RigidBody Component:
       Type: Static
@@ -66,7 +66,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -1.98839664, y: 7.7662077, z: 0}
+      Translate: {x: -2.49145222, y: 6.17996597, z: 0.811235607}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -112,7 +112,7 @@
   Components:
     Transform Component:
       Translate: {x: 0, y: 4.09544182, z: 0}
-      Rotate: {x: -0, y: 0, z: -0.437829614}
+      Rotate: {x: -0, y: 0, z: 0}
       Scale: {x: 4.61071014, y: 0.999995887, z: 1}
       IsActive: true
     RigidBody Component:

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

@@ -71,7 +71,7 @@ namespace SHADE
       box->scale              = createInfo.Scale;
 
       // Set halfEdge data structure for the box
-      box->halfEdgeStructure  = &boxHalfEdgeDS;
+      box->halfEdgeStructure  = &boxHalfEdgeStructure;
 
       return box;
     }
@@ -165,10 +165,10 @@ namespace SHADE
       for (int j = 0; j < NUM_VERTICES_PER_FACE; ++j)
         newFace.vertexIndices.emplace_back(FACE_VERTICES[i][j]);
 
-      boxHalfEdgeDS.AddFace(newFace);
+      boxHalfEdgeStructure.AddFace(newFace);
     }
 
-    boxHalfEdgeDS.Build();
+    boxHalfEdgeStructure.Build();
   }
 
 } // namespace SHADE

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

@@ -92,10 +92,10 @@ namespace SHADE
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
     
-    SHHalfEdgeStructure  boxHalfEdgeDS;
+    SHHalfEdgeStructure   boxHalfEdgeStructure;
 
-    Spheres       spheres;
+    Spheres               spheres;
-    Boxes         boxes;
+    Boxes                 boxes;
     // TODO: Add capsules and hulls
 
     /*---------------------------------------------------------------------------------*/

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

@@ -23,30 +23,6 @@ namespace SHADE
   /* Constructors & Destructor Definitions                                             */
   /*-----------------------------------------------------------------------------------*/
 
-  SHHalfEdgeStructure::HalfEdge::HalfEdge() noexcept
-  : tailVertexIndex { -1 }
-  , headVertexIndex { -1 }
-  , edgeIndex       { -1 }
-  , twinEdgeIndex   { -1 }
-  , faceIndex       { -1 }
-  {}
-
-  SHHalfEdgeStructure::HalfEdge::HalfEdge(const HalfEdge& rhs) noexcept
-  : tailVertexIndex { rhs.tailVertexIndex }
-  , headVertexIndex { rhs.headVertexIndex }
-  , edgeIndex       { rhs.edgeIndex }
-  , twinEdgeIndex   { rhs.twinEdgeIndex }
-  , faceIndex       { rhs.faceIndex }
-  {}
-
-  SHHalfEdgeStructure::HalfEdge::HalfEdge(HalfEdge&& rhs) noexcept
-  : tailVertexIndex { rhs.tailVertexIndex }
-  , headVertexIndex { rhs.headVertexIndex }
-  , edgeIndex       { rhs.edgeIndex }
-  , twinEdgeIndex   { rhs.twinEdgeIndex }
-  , faceIndex       { rhs.faceIndex }
-  {}
-
   SHHalfEdgeStructure::Face::Face(const Face& rhs) noexcept
   : normal { rhs.normal }
   {
@@ -63,31 +39,6 @@ namespace SHADE
   /* Operator Overload Definitions                                                     */
   /*-----------------------------------------------------------------------------------*/
 
-  SHHalfEdgeStructure::HalfEdge& SHHalfEdgeStructure::HalfEdge::operator=(const HalfEdge& rhs) noexcept
-  {
-    if (this == &rhs)
-      return *this;
-
-    tailVertexIndex = rhs.tailVertexIndex;
-    headVertexIndex = rhs.headVertexIndex;
-    edgeIndex       = rhs.edgeIndex ;
-    twinEdgeIndex   = rhs.twinEdgeIndex ;
-    faceIndex       = rhs.faceIndex ;
-
-    return *this;
-  }
-
-  SHHalfEdgeStructure::HalfEdge& SHHalfEdgeStructure::HalfEdge::operator=(HalfEdge&& rhs) noexcept
-  {
-    tailVertexIndex = rhs.tailVertexIndex;
-    headVertexIndex = rhs.headVertexIndex;
-    edgeIndex       = rhs.edgeIndex ;
-    twinEdgeIndex   = rhs.twinEdgeIndex ;
-    faceIndex       = rhs.faceIndex ;
-
-    return *this;
-  }
-
   SHHalfEdgeStructure::Face& SHHalfEdgeStructure::Face::operator=(const Face& rhs) noexcept
   {
     if (this == &rhs)
@@ -165,19 +116,19 @@ namespace SHADE
     // For each face, build half edges
     for (size_t i = 0; i < faces.size(); ++i)
     {
-      const Face& FACE = faces[i];
+      Face& face = faces[i];
 
-      if (FACE.vertexIndices.empty())
+      if (face.vertexIndices.empty())
       {
         SHLOGV_CRITICAL("Unable to build convex polyhedron, no vertices have been added to face {}!", i)
         return;
       }
 
       // Iterate through vertices and build half-edges
-      for (size_t j = 0; j < FACE.vertexIndices.size(); ++j)
+      for (size_t j = 0; j < face.vertexIndices.size(); ++j)
       {
-        const int32_t TAIL = FACE.vertexIndices[j];
+        const int32_t TAIL = face.vertexIndices[j].index;
-        const int32_t HEAD = FACE.vertexIndices[(j + 1) % FACE.vertexIndices.size()];
+        const int32_t HEAD = face.vertexIndices[(j + 1) % face.vertexIndices.size()].index;
 
         const uint64_t NEW_EDGE_ID   = BUILD_UINT64_FROM_UINT32S(TAIL, HEAD);
         const uint64_t TWIN_EDGE_ID  = BUILD_UINT64_FROM_UINT32S(HEAD, TAIL);
@@ -197,6 +148,9 @@ namespace SHADE
           // Set edge index of the newly inserted edge as the size of the map - 1
           // Since it is an unordered map, it will just be at the back
           newHalfEdge.edgeIndex       = static_cast<int32_t>(edgeMap.size()) - 1;
+
+          // Map vertex to this edge index
+          face.vertexIndices[j].edgeIndex = newHalfEdge.edgeIndex;
         }
 
         // Find twin edge if one exists
@@ -217,6 +171,12 @@ namespace SHADE
     // At this point, no duplicates should be in the map and all edges should be linked.
     for (auto& halfEdge : edgeMap | std::views::values)
       halfEdges.emplace_back(halfEdge);
+
+    // Sort based on edge indices
+    std::ranges::sort(halfEdges.begin(), halfEdges.end(), [](const HalfEdge& lhs, const HalfEdge& rhs)
+    {
+      return lhs.edgeIndex < rhs.edgeIndex;
+    });
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/CollisionShapes/SHHalfEdgeStructure.h

@@ -32,6 +32,10 @@ namespace SHADE
     /* Type Definitions                                                                */
     /*---------------------------------------------------------------------------------*/
 
+    /**
+     * @brief
+     * Encapsulates the data half edge of a face on a polyhedron.
+    */
     struct HalfEdge
     {
       /*-------------------------------------------------------------------------------*/
@@ -40,36 +44,35 @@ namespace SHADE
 
       //Head and tail forms the edge. 
       //Head <----- Tail
-      int32_t tailVertexIndex;
+      int32_t tailVertexIndex = -1;
 
       // Head is also tail of the next edge.
-      int32_t headVertexIndex;
+      int32_t headVertexIndex = -1;
 
-      int32_t edgeIndex;
+      int32_t edgeIndex       = -1;
       // Other half of the edge on a different face.
       // Important for extrapolating face normals.
-      int32_t twinEdgeIndex;
+      int32_t twinEdgeIndex   = -1;
 
       // Adjacent face of this edge.
-      int32_t faceIndex;
+      int32_t faceIndex       = -1;
-
-      /*-------------------------------------------------------------------------------*/
-      /* Constructors & Destructor                                                     */
-      /*-------------------------------------------------------------------------------*/
-
-      HalfEdge  ()                    noexcept;
-      HalfEdge  (const HalfEdge& rhs) noexcept;
-      HalfEdge  (HalfEdge&& rhs)      noexcept;
-      ~HalfEdge ()                    noexcept = default;
-
-      /*-------------------------------------------------------------------------------*/
-      /* Operator Overloads                                                            */
-      /*-------------------------------------------------------------------------------*/
-
-      HalfEdge& operator= (const HalfEdge& rhs) noexcept;
-      HalfEdge& operator= (HalfEdge&& rhs)      noexcept;
     };
 
+    struct Vertex
+    {
+    public:
+      /*-------------------------------------------------------------------------------*/
+      /* Data Members                                                                  */
+      /*-------------------------------------------------------------------------------*/
+
+      int32_t index     = -1;
+      int32_t edgeIndex = -1;  // the half-edge that this vertex is a tail of.
+    };
+
+    /**
+     * @brief
+     * Encapsulates the data of a face on a polyhedron.
+    */
     struct Face
     {
     public:
@@ -77,10 +80,8 @@ namespace SHADE
       /* Data Members                                                                  */
       /*-------------------------------------------------------------------------------*/
 
-      // TODO: Store face offset
-
       SHVec3                normal;
-      std::vector<int32_t>  vertexIndices;    // Must be in CCW order
+      std::vector<Vertex>   vertexIndices;    // Must be in CCW order
 
       /*-------------------------------------------------------------------------------*/
       /* Constructors & Destructor                                                     */
@@ -134,10 +135,6 @@ namespace SHADE
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    float radius = 0.2f;  // Default Radius is 2 cm
-
-    // Store the faces and half-edges
-
     std::vector<Face>     faces;
     std::vector<HalfEdge> halfEdges;
 

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

@@ -67,10 +67,26 @@ namespace SHADE
     [[nodiscard]] static bool ConvexVsConvex    (SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B)  noexcept;
 
   private:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    struct FaceQuery
+    {
+      bool    colliding     = false;
+      int32_t closestFace   = -1;
+      float   bestDistance  = std::numeric_limits<float>::lowest();
+    };
+
     /*---------------------------------------------------------------------------------*/
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
+    // Sphere VS Convex
+
+    static FaceQuery  findClosestFace   (const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron) noexcept;
+    static int32_t    findClosestPoint  (const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron, int32_t faceIndex) noexcept;
+
     static bool isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
 
     // TODO: buildMinkowskiFace, queryEdgeDirection, distanceBetweenTwoEdges,

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

@@ -19,16 +19,6 @@
 #include "Physics/Collision/CollisionShapes/SHCollisionShape.h"
 #include "Physics/Collision/CollisionShapes/SHBoxCollisionShape.h"
 
-// When testing against convex polyhedrons, we do not care so much as whether it is a box
-// or something else. We only need the vertices to build half edge structures for use
-// with a gauss map. Regardless, we still cast it to the type just to get vertices
-// since spheres and capsules do not implement the same method.
-
-// I did consider having another base class that encapsulates methods that convex polyhedrons
-// would implement, but for the sake of my sanity, I won't do that.
-
-// TODO
-
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -37,6 +27,21 @@ namespace SHADE
 
   bool SHCollision::SphereVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
+    const SHSphereCollisionShape&           SPHERE      = dynamic_cast<const SHSphereCollisionShape&>(A);
+    const SHConvexPolyhedronCollisionShape& POLYHEDRON  = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B);
+
+    const SHVec3  CENTER  = SPHERE.GetCenter();
+    const float   RADIUS  = SPHERE.GetWorldRadius();
+
+    const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = POLYHEDRON.GetHalfEdgeStructure();
+
+    const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
+    if (!FACE_QUERY.colliding)
+      return false;
+
+    if (FACE_QUERY.bestDistance < SHMath::EPSILON)
+      return true;
+
     return false;
   }
 
@@ -49,19 +54,197 @@ namespace SHADE
   {
     // Convert to underlying types
     // For the convex, we only need the convex polyhedron shape since the get vertex is pure virtual.
-    const SHSphereCollisionShape&           SPHERE  = dynamic_cast<const SHSphereCollisionShape&>(A);
+    const SHSphereCollisionShape&           SPHERE      = dynamic_cast<const SHSphereCollisionShape&>(A);
-    const SHConvexPolyhedronCollisionShape& CONVEX  = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B);
+    const SHConvexPolyhedronCollisionShape& POLYHEDRON  = dynamic_cast<const SHConvexPolyhedronCollisionShape&>(B);
 
     const SHVec3  CENTER  = SPHERE.GetCenter();
     const float   RADIUS  = SPHERE.GetWorldRadius();
 
+    const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = POLYHEDRON.GetHalfEdgeStructure();
+
+    const FaceQuery FACE_QUERY = findClosestFace(SPHERE, POLYHEDRON);
+    if (!FACE_QUERY.colliding)
+      return false;
+
+    uint32_t    numContacts   = 0;
+    const float PENETRATION   = RADIUS - FACE_QUERY.bestDistance;
+
+    SHContact contact;
+    contact.featurePair.key = 0;
+
+    // Check if center is inside polyhedron (below the face)
+    if (FACE_QUERY.bestDistance < SHMath::EPSILON)
+    {
+      manifold.normal = -POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
+
+      contact.penetration      = PENETRATION;
+      contact.position         = SPHERE.GetCenter();
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+      return true;
+    }
+
     // Find closest face of polygon to circle
+    const int32_t CLOSEST_POINT = findClosestPoint(SPHERE, POLYHEDRON, FACE_QUERY.closestFace);
 
-    int32_t closestFaceIndex  = -1;
+    const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetHalfEdgeStructure()->GetFace(FACE_QUERY.closestFace);
-    int32_t closestPointIndex = -1;
-    float   bestDistance      = std::numeric_limits<float>::lowest();
 
-    const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = CONVEX.GetHalfEdgeStructure();
+    const SHVec3& FACE_NORMAL   = POLYHEDRON.GetNormal(FACE_QUERY.closestFace);
+    const int32_t NUM_VERTICES  = static_cast<int32_t>(FACE.vertexIndices.size());
+
+    // Check against voronoi regions of the face to determine the type of the intersection test
+    // We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
+    // If none of these are true, the sphere is above the face but not separating
+
+    /*
+     *                   | 2
+     *       _ _ _ _ _ _ | _ _ _
+     *      /           /
+     *   | / regionD | /   regionA
+     *   |/ _ _ _ _ _|/ _ _ _
+     *  3/  regionB  /1
+     *  /           /   regionC
+     *
+     */
+
+    const SHVec3 P1 = POLYHEDRON.GetVertex(FACE.vertexIndices[CLOSEST_POINT].index);
+    const SHVec3 P1_TO_CENTER = CENTER - P1;
+
+    // To be inside either region A or B, 2 conditions must be satisfied
+    // 1. Same side as tangent
+    // 2. Same side as adjacent normal
+
+    // Check in regions A
+    {
+      const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
+      const SHVec3 P2 = POLYHEDRON.GetVertex(FACE.vertexIndices[P2_INDEX].index);
+
+      const SHVec3 TANGENT = SHVec3::Normalise(P2 - P1);
+
+      float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT);
+
+      if (projection >= 0.0f)
+      {
+        // Find closest point
+        const SHVec3 CP = P1 + projection * TANGENT;
+
+        // Check 2nd condition
+        // Get adjacent normal from twin half edge
+        const int32_t EDGE_INDEX  = FACE.vertexIndices[CLOSEST_POINT].edgeIndex;
+        const int32_t TWIN_EDGE   = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex;
+
+        const int32_t ADJ_FACE    = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex;
+        const SHVec3& ADJ_NORMAL  = POLYHEDRON.GetNormal(ADJ_FACE);
+
+        projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
+        if (projection >= 0.0f)
+        {
+          // Must be smaller than radius
+          if (projection >= RADIUS)
+            return false;
+
+          const SHVec3 CP_TO_CENTER = CENTER - CP;
+
+          manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
+
+          contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
+          contact.position = CP;
+
+          manifold.contacts[numContacts++] = contact;
+          manifold.numContacts = numContacts;
+
+          return true;
+        }
+      }
+    }
+    
+    // Check in region B
+    {
+      const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
+      const SHVec3 P3 = POLYHEDRON.GetVertex(FACE.vertexIndices[P3_INDEX].index);
+
+      const SHVec3 TANGENT = SHVec3::Normalise(P3 - P1);
+
+      float projection = SHVec3::Dot(P1_TO_CENTER, TANGENT);
+
+      if (projection >= 0.0f)
+      {
+        // Find closest point
+        const SHVec3 CP = P1 + projection * TANGENT;
+
+        // Check 2nd condition
+        // Get adjacent normal from twin half edge
+        const int32_t EDGE_INDEX  = FACE.vertexIndices[P3_INDEX].edgeIndex;
+        const int32_t TWIN_EDGE   = HALF_EDGE_STRUCTURE->GetHalfEdge(EDGE_INDEX).twinEdgeIndex;
+
+        const int32_t ADJ_FACE    = HALF_EDGE_STRUCTURE->GetHalfEdge(TWIN_EDGE).faceIndex;
+        const SHVec3& ADJ_NORMAL  = POLYHEDRON.GetNormal(ADJ_FACE);
+
+        projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
+        if (projection >= 0.0f)
+        {
+          // Must be smaller than radius
+          if (projection >= RADIUS)
+            return false;
+
+          const SHVec3 CP_TO_CENTER = CENTER - CP;
+
+          manifold.normal = -SHVec3::Normalise(CP_TO_CENTER);
+
+          contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
+          contact.position = CP;
+
+          manifold.contacts[numContacts++] = contact;
+          manifold.numContacts = numContacts;
+
+          return true;
+        }
+      }
+    }
+
+    // Either region C or D. Take whichever depth is smaller
+
+    const float C_PENETRATION = RADIUS - P1_TO_CENTER.Length();
+
+    if (std::fabs(C_PENETRATION) < RADIUS && std::fabs(C_PENETRATION) < PENETRATION)
+    {
+      manifold.normal = -SHVec3::Normalise(P1_TO_CENTER);
+
+      contact.penetration = C_PENETRATION;
+      contact.position    = P1;
+    }
+    else
+    {
+      manifold.normal = -FACE_NORMAL;
+
+      contact.penetration = PENETRATION;
+      contact.position    = CENTER - FACE_NORMAL * RADIUS;
+    }
+
+    manifold.contacts[numContacts++] = contact;
+    manifold.numContacts = numContacts;
+
+    return true;
+  }
+
+  bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  {
+    return SphereVsConvex(manifold, B, A);
+  }
+
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollision::FaceQuery SHCollision::findClosestFace(const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron) noexcept
+  {
+    FaceQuery faceQuery;
+
+    const SHVec3  CENTER  = sphere.GetCenter();
+    const float   RADIUS  = sphere.GetWorldRadius();
+
+    const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure();
 
     /*
      * Test against each face.
@@ -76,8 +259,8 @@ namespace SHADE
 
       // Build plane equation
 
-      const SHVec3  POINT   = CONVEX.GetVertex(FACE.vertexIndices[0]);  // use the first vertex to build a plane
+      const SHVec3  POINT   = polyhedron.GetVertex(FACE.vertexIndices[0].index);  // use the first vertex to build a plane
-      const SHVec3& NORMAL  = CONVEX.GetNormal(i);
+      const SHVec3& NORMAL  = polyhedron.GetNormal(i);
       const float   D       = -SHVec3::Dot(NORMAL, POINT);
 
       // Find signed distance of center to plane
@@ -87,153 +270,47 @@ namespace SHADE
       // 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 > RADIUS)
-        return false;
+        return faceQuery;
 
-      if (SIGNED_DIST > bestDistance)
+      if (SIGNED_DIST > faceQuery.bestDistance)
       {
-        bestDistance = SIGNED_DIST;
+        faceQuery.bestDistance = SIGNED_DIST;
-        closestFaceIndex = i;
+        faceQuery.closestFace = i;
       }
     }
 
-    uint32_t    numContacts   = 0;
+    faceQuery.colliding = true;
-    const float PENETRATION   = RADIUS - bestDistance;
+    return faceQuery;
-
-    // Check if center is inside polyhedron (below the face)
-    if (bestDistance < SHMath::EPSILON)
-    {
-      manifold.normal = -CONVEX.GetNormal(closestFaceIndex);
-
-      SHContact newContact;
-      newContact.penetration      = PENETRATION;
-      newContact.position         = SPHERE.GetCenter();
-      newContact.featurePair.key  = 0;
-
-      manifold.contacts[numContacts++] = newContact;
-      manifold.numContacts = numContacts;
-      return true;
-    }
-
-    // Check against voronoi regions of the face to determine the type of the intersection test
-    // We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
-    // If none of these are true, the sphere is above the face but not separating
-
-    /*
-     *                   | A
-     *       _ _ _ _ _ _ | _ _ _
-     *      /           /
-     *   | /         | /   regionA
-     *   |/ _ _ _ _ _|/ _ _ _
-     *  B/  regionB  /C
-     *  /           /   regionC
-     */
-
-    const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(closestFaceIndex);
-    const SHVec3& NORMAL = CONVEX.GetNormal(closestFaceIndex);
-
-    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;
-    const int32_t INDEX_B = closestPointIndex == 0 ? NUM_VERTICES_ON_FACE - 1 : closestPointIndex - 1;
-
-    const SHVec3 POINTS[2] =
-    {
-      CONVEX.GetVertex(FACE.vertexIndices[INDEX_A]) // A
-    , CONVEX.GetVertex(FACE.vertexIndices[INDEX_B]) // B
-    };
-
-    // To be inside either region A or B, 2 conditions must be satisfied
-    // 1. Same side as tangent
-    // 2. Same side as normal from edge to sphere
-
-    // Check in regions A & B
-    for (int i = 0; i < 2; ++i)
-    {
-      const SHVec3 TANGENT = SHVec3::Normalise(POINTS[i] - C);
-
-      float projection = SHVec3::Dot(C_TO_CENTER, TANGENT);
-      if (projection >= 0.0f)
-      {
-        // Check 2nd condition
-        // Find closest point
-        const SHVec3 CP = C + projection * TANGENT;
-        const SHVec3 CP_TO_CENTER = SHVec3::Normalise(C - CP);
-
-        projection = SHVec3::Dot(C_TO_CENTER, CP_TO_CENTER);
-        if (projection >= 0.0f)
-        {
-          // Sphere Within region
-          manifold.normal = -CP_TO_CENTER;
-
-          SHContact newContact;
-          newContact.penetration      = RADIUS - projection;
-          newContact.position         = CP;
-          newContact.featurePair.key  = 0;
-
-          manifold.contacts[numContacts++] = newContact;
-          manifold.numContacts = numContacts;
-
-          return true;
-        }
-      }
-    }
-
-    // Check region C (closest point)
-    {
-      if (C_TO_CENTER.LengthSquared() < RADIUS * RADIUS)
-      {
-        manifold.normal = -SHVec3::Normalise(C_TO_CENTER);
-
-        SHContact newContact;
-        newContact.penetration      = PENETRATION;
-        newContact.position         = C;
-        newContact.featurePair.key  = 0;
-
-        manifold.contacts[numContacts++] = newContact;
-        manifold.numContacts = numContacts;
-
-        return true;
-      }
-    }
-
-    // Region D
-    if (PENETRATION <= RADIUS)
-    {
-      manifold.normal = -NORMAL;
-
-      SHContact newContact;
-      newContact.penetration      = PENETRATION;
-      newContact.position         = SPHERE.GetCenter() - NORMAL * RADIUS;
-      newContact.featurePair.key  = 0;
-
-      manifold.contacts[numContacts++] = newContact;
-      manifold.numContacts = numContacts;
-
-      return true;
-    }
-
-    return false;
   }
 
-  bool SHCollision::ConvexVsSphere(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
+  int32_t SHCollision::findClosestPoint(const SHSphereCollisionShape& sphere, const SHConvexPolyhedronCollisionShape& polyhedron, int32_t faceIndex) noexcept
   {
-    return SphereVsConvex(manifold, B, A);
+    // Find closest point on face
+    int32_t closestPointIndex = -1;
+
+    const SHVec3  CENTER  = sphere.GetCenter();
+    const float   RADIUS  = sphere.GetWorldRadius();
+
+    const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure();
+
+    
+    const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(faceIndex);
+    const int32_t NUM_VERITICES = static_cast<int32_t>(FACE.vertexIndices.size());
+
+    float smallestDist = std::numeric_limits<float>::max();
+    for (int32_t i = 0; i < NUM_VERITICES; ++i)
+    {
+      const SHVec3 POINT = polyhedron.GetVertex(FACE.vertexIndices[i].index);
+      const float DIST = SHVec3::DistanceSquared(CENTER, POINT);
+
+      if (DIST < smallestDist)
+      {
+        smallestDist = DIST;
+        closestPointIndex = i;
+      }
+    }
+
+    return closestPointIndex;
   }
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/SHPhysicsMaterial.h

@@ -19,6 +19,10 @@ namespace SHADE
   /* Type Definitions                                                                  */
   /*-----------------------------------------------------------------------------------*/
 
+  /**
+   * @brief
+   * Encapsulates the data of a physics material for physics simulations.
+  */
   class SH_API SHPhysicsMaterial
   {
   public: