Clean up

Assets/Scenes/PhysicsSandbox.shade

@@ -66,7 +66,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -0.903104782, y: 7, z: -0.782080948}
+      Translate: {x: -1.96328545, y: 7, z: 0.743723333}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true

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

@@ -97,4 +97,28 @@ namespace SHADE
   {
     return halfEdgeStructure;
   }
+
+  int32_t SHConvexPolyhedronCollisionShape::GetFaceCount() const noexcept
+  {
+    return halfEdgeStructure->GetFaceCount();
+  }
+
+  int32_t SHConvexPolyhedronCollisionShape::GetHalfEdgeCount() const noexcept
+  {
+    return halfEdgeStructure->GetHalfEdgeCount();
+  }
+
+  const SHHalfEdgeStructure::Face& SHConvexPolyhedronCollisionShape::GetFace(int index) const
+  {
+    // Assume it has already been initialised
+    return halfEdgeStructure->GetFace(index);
+  }
+
+  const SHHalfEdgeStructure::HalfEdge& SHConvexPolyhedronCollisionShape::GetHalfEdge(int index) const
+  {
+    // Assume it has already been initialised
+    return halfEdgeStructure->GetHalfEdge(index);
+  }
+
+
 } // namespace SHADE

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

@@ -58,6 +58,11 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     [[nodiscard]] const SHHalfEdgeStructure*            GetHalfEdgeStructure  ()  const noexcept;
+    [[nodiscard]] int32_t                               GetFaceCount          ()  const noexcept;
+    [[nodiscard]] const SHHalfEdgeStructure::Face&      GetFace               (int index)  const;
+    [[nodiscard]] int32_t                               GetHalfEdgeCount      ()  const noexcept;
+    [[nodiscard]] const SHHalfEdgeStructure::HalfEdge&  GetHalfEdge           (int index)  const;
+
     [[nodiscard]] virtual SHVec3 GetVertex  (int index)     const = 0;
     [[nodiscard]] virtual SHVec3 GetNormal  (int faceIndex) const = 0;
 

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

@@ -86,6 +86,7 @@ namespace SHADE
 
     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 int32_t    findVoronoiRegion (const SHSphereCollisionShape& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept;
 
     static bool isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
 

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

@@ -33,8 +33,6 @@ namespace SHADE
     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;
@@ -60,8 +58,6 @@ namespace SHADE
     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;
@@ -83,111 +79,45 @@ namespace SHADE
 
       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);
 
-    const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetHalfEdgeStructure()->GetFace(FACE_QUERY.closestFace);
+    const SHHalfEdgeStructure::Face& FACE = POLYHEDRON.GetFace(FACE_QUERY.closestFace);
 
     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
+    // Get points and build tangents
-    // We have 3 voronoi regions to check: cp -> prev, cp -> next and cp -> center
+    const int32_t P2_INDEX = (CLOSEST_POINT + 1) % NUM_VERTICES;
-    // If none of these are true, the sphere is above the face but not separating
+    const int32_t P3_INDEX = CLOSEST_POINT == 0 ? NUM_VERTICES - 1 : CLOSEST_POINT - 1;
-
-    /*
-     *                   | 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);
-
-    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 the cross product (tangent x normal)
-      //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 = SHVec3::Cross(tangent, FACE_NORMAL);
-
-      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);
 
-    tangent = SHVec3::Normalise(P3 - P1);
+    const SHVec3 TANGENT_1 = SHVec3::Normalise(P2 - P1);
+    const SHVec3 TANGENT_2 = SHVec3::Normalise(P3 - P1);
 
-    projection = SHVec3::Dot(P1_TO_CENTER, tangent);
+    // Get the voronoi region it belongs in
-
+    const int32_t REGION = findVoronoiRegion(SPHERE, P1, FACE_NORMAL, TANGENT_1, TANGENT_2);
-    if (projection >= 0.0f)
+    if (REGION == 0)
-    {
-      // Find closest point
-      const SHVec3 CP = P1 + projection * tangent;
-
-      // Check 2nd condition
-      // Get adjacent normal from the cross product (normal x tangent)
-      //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);
-      const SHVec3 ADJ_NORMAL = SHVec3::Cross(FACE_NORMAL, tangent);
-
-
-      projection = SHVec3::Dot(P1_TO_CENTER, ADJ_NORMAL);
-      if (projection >= 0.0f)
-      {
-        // Must be smaller than radius
-        if (projection >= RADIUS)
       return false;
 
+    // Create contact information based on region
+
+    const SHVec3 P1_TO_CENTER = CENTER - P1;
+    switch (REGION)
+    {
+      case 1: // Region A
+      case 2: // Region B
+      {
+        // Find closest point
+        const SHVec3& TANGENT     = REGION == 1 ? TANGENT_1 : TANGENT_2;
+        const SHVec3 CP           = P1 + TANGENT * SHVec3::Dot(P1_TO_CENTER, TANGENT);
         const SHVec3 CP_TO_CENTER = CENTER - CP;
 
         manifold.normal     = -SHVec3::Normalise(CP_TO_CENTER);
@@ -195,35 +125,29 @@ namespace SHADE
         contact.penetration = RADIUS - SHVec3::Dot(CP_TO_CENTER, -manifold.normal);
         contact.position    = CP;
 
-        manifold.contacts[numContacts++] = contact;
+        break;
-        manifold.numContacts = numContacts;
-
-        return true;
       }
-    }
+      case 3: // Region C
-
-    // Region C has a negative dot product with any of the tangents.
-
-    projection = SHVec3::Dot(P1_TO_CENTER, tangent);
-    if (projection < 0)
       {
         manifold.normal     = -SHVec3::Normalise(P1_TO_CENTER);
 
         contact.penetration = RADIUS - P1_TO_CENTER.Length();
         contact.position    = P1;
 
-      manifold.contacts[numContacts++] = contact;
+        break;
-      manifold.numContacts = numContacts;
-
-      return true;
       }
-
+      case 4: // Region D
-    // Region D
+      {
         manifold.normal     = -FACE_NORMAL;
 
         contact.penetration = PENETRATION;
         contact.position    = CENTER - FACE_NORMAL * RADIUS;
 
+        break;
+      }
+      default: return false;  // Should never happen
+    }
+
     manifold.contacts[numContacts++] = contact;
     manifold.numContacts = numContacts;
 
@@ -246,8 +170,6 @@ namespace SHADE
     const SHVec3  CENTER  = sphere.GetCenter();
     const float   RADIUS  = sphere.GetWorldRadius();
 
-    const SHHalfEdgeStructure* HALF_EDGE_STRUCTURE = polyhedron.GetHalfEdgeStructure();
-
     /*
      * Test against each face.
      *
@@ -255,9 +177,9 @@ namespace SHADE
      * 2. Find the signed distance from plane to center of sphere.
      * 3. Save best distance and face.
      */
-    for (int32_t i = 0; i < HALF_EDGE_STRUCTURE->GetFaceCount(); ++i)
+    for (int32_t i = 0; i < polyhedron.GetFaceCount(); ++i)
     {
-      const SHHalfEdgeStructure::Face& FACE = HALF_EDGE_STRUCTURE->GetFace(i);
+      const SHHalfEdgeStructure::Face& FACE = polyhedron.GetFace(i);
 
       // Build plane equation
 
@@ -291,12 +213,8 @@ namespace SHADE
     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 = polyhedron.GetFace(faceIndex);
-
-    
-    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();
@@ -315,4 +233,67 @@ namespace SHADE
     return closestPointIndex;
   }
 
+  int32_t SHCollision::findVoronoiRegion(const SHSphereCollisionShape& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept
+  {
+    static constexpr int NUM_TANGENTS = 2;
+
+    // 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& CENTER  = sphere.GetCenter();
+    const float   RADIUS  = sphere.GetWorldRadius();
+
+    const SHVec3 TANGENTS         [NUM_TANGENTS] { tangent1, tangent2 };
+    const SHVec3 ADJACENT_NORMALS [NUM_TANGENTS] { SHVec3::Cross(tangent1, faceNormal), SHVec3::Cross(faceNormal, tangent2) };
+
+    const SHVec3 FACE_TO_CENTER = CENTER - faceVertex;
+
+    // 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 Region A & B
+    for (int i = 0; i < NUM_TANGENTS; ++i)
+    {
+      float projection = SHVec3::Dot(FACE_TO_CENTER, TANGENTS[i]);
+      if (projection >= 0.0f)
+      {
+        // Find closest point
+        const SHVec3 CLOSEST_POINT = faceVertex + projection * TANGENTS[i];
+
+        projection = SHVec3::Dot(FACE_TO_CENTER, ADJACENT_NORMALS[i]);
+        if (projection >= 0.0f)
+        {
+          if (projection > RADIUS)
+            return 0;
+
+          // Region 1 or 2 ( A or B)
+          return i + 1;
+        }
+      }
+    }
+
+    // Check Region C
+    // Face to vertex is in the opposite direction of any tangent.
+    const float PROJECTION = SHVec3::Dot(FACE_TO_CENTER, tangent1);
+    if (PROJECTION < 0)
+      return 3;
+
+    // Belongs in region D by default
+    return 4;
+  }
+
+
 } // namespace SHADE