From fba338eaef197eb2e0abeae46de86a52cb9ed62f Mon Sep 17 00:00:00 2001
From: Diren D Bharwani <diren.bharwani@gmail.com>
Date: Fri, 30 Dec 2022 01:14:40 +0800
Subject: [PATCH] Fixed half edge builder and built box polyhedron

---
 .../SHCollisionShapeLibrary.cpp               | 51 ++++++++++++++++---
 .../CollisionShapes/SHConvexPolyhedron.cpp    |  8 ++-
 2 files changed, 52 insertions(+), 7 deletions(-)

diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp
index 6d2fdc06..ad5e3c44 100644
--- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp
+++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHCollisionShapeLibrary.cpp
@@ -70,6 +70,9 @@ namespace SHADE
       box->Orientation      = createInfo.Orientation;
       box->scale            = createInfo.Scale;
 
+      // Set convex polyhedron for the box
+      box->polyhedron       = &boxPolyhedron;
+
       return box;
     }
 
@@ -109,6 +112,9 @@ namespace SHADE
 
   void SHCollisionShapeLibrary::createBoxPolyhedron() noexcept
   {
+    static constexpr int NUM_VERTICES_PER_FACE  = 4;
+    static constexpr int NUM_FACES              = 6;
+
     /*
      * Vertices (Front/Back Face):
      *
@@ -120,16 +126,49 @@ namespace SHADE
      *
      * Faces:
      *
-     * Front:   0 (0,1,2,3)
-     * Right:   1 (1,5,6,2)
-     * Back:    2 (5,4,7,6)
-     * Left:    3 (4,0,3,7)
-     * Top:     4 (3,2,6,7)
-     * Bottom:  5 (4,5,1,0)
+     * Front:   0 (0,1,2,3) Normal:  Z
+     * Right:   1 (1,5,6,2) Normal:  X
+     * 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
      *
      */
 
     // Create face data
+
+    const SHVec3 FACE_NORMALS[NUM_FACES]
+    {
+      -SHVec3::UnitZ
+    ,  SHVec3::UnitX
+    ,  SHVec3::UnitZ
+    , -SHVec3::UnitX
+    ,  SHVec3::UnitY
+    , -SHVec3::UnitY
+    };
+
+    const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE]
+    {
+      { 0, 1, 2, 3 }
+    , { 1, 5, 6, 2 }
+    , { 5, 4, 7, 6 }
+    , { 4, 0, 3, 7 }
+    , { 3, 2, 6, 7 }
+    , { 4, 5, 1, 0 }
+    };
+
+    for (int i = 0; i < NUM_FACES; ++i)
+    {
+      SHConvexPolyhedron::Face newFace;
+      newFace.normal = FACE_NORMALS[i];
+
+      for (int j = 0; j < NUM_VERTICES_PER_FACE; ++j)
+        newFace.vertexIndices.emplace_back(FACE_VERTICES[i][j]);
+
+      boxPolyhedron.AddFace(newFace);
+    }
+
+    boxPolyhedron.BuildPolyhedron();
   }
 
 } // namespace SHADE
\ No newline at end of file
diff --git a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedron.cpp b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedron.cpp
index cb5fad76..ca049ebb 100644
--- a/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedron.cpp
+++ b/SHADE_Engine/src/Physics/Collision/CollisionShapes/SHConvexPolyhedron.cpp
@@ -167,11 +167,17 @@ namespace SHADE
     {
       const Face& FACE = faces[i];
 
+      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)
       {
         const int32_t TAIL = FACE.vertexIndices[j];
-        const int32_t HEAD = (TAIL + 1) > FACE.vertexIndices.back() ? FACE.vertexIndices.front() : TAIL + 1;  // Wrap around
+        const int32_t HEAD = j + 1 < FACE.vertexIndices.size() ? FACE.vertexIndices[j + 1] : FACE.vertexIndices[0];
 
         const uint64_t NEW_EDGE_ID   = BUILD_UINT64_FROM_UINT32S(TAIL, HEAD);
         const uint64_t TWIN_EDGE_ID  = BUILD_UINT64_FROM_UINT32S(HEAD, TAIL);