Added first half of Gauss Map Optimised SAT

Assets/Scenes/PhysicsSandbox.shade

@@ -45,7 +45,7 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 0, y: 4, z: 5}
+      Position: {x: 1, y: 10, z: 3}
       Pitch: 0
       Yaw: 0
       Roll: 0
@@ -62,7 +62,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -1.45715916, y: 7, z: 0.227711335}
+      Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335}
       Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
@@ -77,7 +77,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
@@ -274,3 +274,85 @@
           Rotation Offset: {x: 0, y: 0, z: 0}
       IsActive: true
   Scripts: ~
+- EID: 7
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: 0.899999976, y: 10, z: 0}
+      Rotate: {x: -0, y: 0, z: 0.785398185}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      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: true
+          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: ~
+- EID: 8
+  Name: Default
+  IsActive: true
+  NumberOfChildren: 0
+  Components:
+    Transform Component:
+      Translate: {x: -0.5, y: 10, z: 0}
+      Rotate: {x: -0, y: 0.785398185, z: -0}
+      Scale: {x: 1, y: 1, z: 1}
+      IsActive: true
+    RigidBody Component:
+      Type: Dynamic
+      Auto Mass: false
+      Mass: 1
+      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: true
+          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: ~

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

@@ -13,6 +13,7 @@
 // Project Headers
 #include "Physics/Collision/Contacts/SHManifold.h"
 #include "Physics/Collision/Contacts/SHCollisionKey.h"
+#include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
 
 
 namespace SHADE
@@ -72,11 +73,18 @@ namespace SHADE
 
     struct FaceQuery
     {
-      bool    colliding     = false;
+      bool    colliding     = false;  // Allows for early out
       int32_t closestFace   = -1;
       float   bestDistance  = std::numeric_limits<float>::lowest();
     };
 
+    struct EdgeQuery
+    {
+      int32_t halfEdgeA     = -1;
+      int32_t halfEdgeB     = -1;
+      float   bestDistance  = std::numeric_limits<float>::lowest();
+    };
+
     /*---------------------------------------------------------------------------------*/
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
@@ -93,14 +101,19 @@ namespace SHADE
 
     // Convex VS Convex
 
+    static FaceQuery  queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+    static EdgeQuery  queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+
+    static bool       buildMinkowskiFace  (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
     static bool       isMinkowskiFace     (const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept;
+    static float      distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
 
     /*
      * TODO: 
-     * static FaceQuery queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+     * 
-     * static EdgeQuery queryEdgeDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+     * 
-     * static bool      buildMinkowskiFace  (const SHConvexPolyhedron::HalfEdge& edgeA, const SHConvexPolyhedron::HalfEdge& edgeB) noexcept;
+     * 
-     * static float     distanceBetweenEdges(const SHConvexPolyhedron::HalfEdge& edgeA, const SHConvexPolyhedron::HalfEdge& edgeB, SHConvexPolyhedron& poly) noexcept;
+     * 
      * static int32_t   findIncidentFace    (SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
      * static uint32_t  clip                [sutherland-hodgemann clipping]
      *

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

@@ -15,9 +15,9 @@
 
 // Project Headers
 #include "Math/SHMathHelpers.h"
+#include "Math/Geometry/SHPlane.h"
 #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
 
-
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -26,15 +26,22 @@ namespace SHADE
 
   bool SHCollision::ConvexVsConvex(const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
-    /*
+    const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
-     * TODO:
+    const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
-     *
-     * 1. Query face directiosn of a to b. Exit early if separation found.
-     * 2. query face directions of b to a. Exit early if separation found.
-     * 3. Query edge directions of a & b. Exit early if separation found.
-     */
 
+    const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
+    if (FACE_QUERY_A.bestDistance > 0.0f)
       return false;
+
+    const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
+    if (FACE_QUERY_B.bestDistance > 0.0f)
+      return false;
+
+    const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
+    if (EDGE_QUERY.bestDistance > 0.0f)
+      return false;
+
+    return true;
   }
 
   bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
@@ -61,4 +68,123 @@ namespace SHADE
     return false;
   }
 
+  /*-----------------------------------------------------------------------------------*/
+  /* Private Member Functions Definitions                                              */
+  /*-----------------------------------------------------------------------------------*/
+
+  SHCollision::FaceQuery SHCollision::queryFaceDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    FaceQuery faceQuery;
+
+    const int32_t NUM_FACES = A.GetFaceCount();
+    for (const int32_t i : std::views::iota(0, NUM_FACES))
+    {
+      const SHHalfEdgeStructure::Face& FACE_A = A.GetFace(i);
+      const SHVec3 NORMAL_A = A.GetNormal(i);
+
+      // Smallest penetration is point closest to face normal
+      const SHVec3  SUPPORT_POINT = B.FindSupportPoint(-NORMAL_A);
+
+      const SHVec3  VERTEX_A = A.GetVertex(FACE_A.vertexIndices[0].index);
+
+      const SHPlane FACE_PLANE { VERTEX_A, NORMAL_A };
+      const float   DISTANCE = FACE_PLANE.SignedDistance(SUPPORT_POINT);
+
+      if (DISTANCE > faceQuery.bestDistance)
+      {
+        faceQuery.bestDistance = DISTANCE;
+        faceQuery.closestFace = i;
+      }
+    }
+
+    return faceQuery;
+  }
+
+  SHCollision::EdgeQuery SHCollision::queryEdgeDirections(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    EdgeQuery edgeQuery;
+
+    const int32_t EDGE_COUNT_A = A.GetHalfEdgeCount();
+    const int32_t EDGE_COUNT_B = B.GetHalfEdgeCount();
+
+    for (int32_t i = 0; i < EDGE_COUNT_A; i += 2)
+    {
+      for (int32_t j = 0; j < EDGE_COUNT_B; j += 2)
+      {
+        const bool IS_MINKOWSKI_FACE = buildMinkowskiFace(A, B, i, j);
+        if (!IS_MINKOWSKI_FACE)
+          continue;
+
+        const float SEPARATION = distanceBetweenEdges(A, B, i, j);
+        if (SEPARATION > edgeQuery.bestDistance)
+        {
+          edgeQuery.bestDistance = SEPARATION;
+          edgeQuery.halfEdgeA = i;
+          edgeQuery.halfEdgeB = j;
+        }
+      }
+    }
+
+    return edgeQuery;
+  }
+
+  bool SHCollision::buildMinkowskiFace(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
+  {
+    // Get Half Edge from both polygons
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
+    const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_A = A.GetHalfEdge(HALF_EDGE_A.twinEdgeIndex);
+
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
+    const SHHalfEdgeStructure::HalfEdge& TWIN_EDGE_B = B.GetHalfEdge(HALF_EDGE_B.twinEdgeIndex);
+
+
+
+    // Get normals from face and twin edge face
+    const SHVec3 NA = A.GetNormal(HALF_EDGE_A.faceIndex);
+    const SHVec3 NB = A.GetNormal(TWIN_EDGE_A.faceIndex);
+    const SHVec3 NC = B.GetNormal(HALF_EDGE_B.faceIndex);
+    const SHVec3 ND = B.GetNormal(TWIN_EDGE_B.faceIndex);
+
+    return isMinkowskiFace(NA, NB, -NC, -ND);
+  }
+
+  bool SHCollision::isMinkowskiFace(const SHVec3& a, const SHVec3& b, const SHVec3& c, const SHVec3& d) noexcept
+  {
+    const SHVec3 BXA = SHVec3::Cross(b, a);
+    const SHVec3 DXC = SHVec3::Cross(d, c);
+
+    const float CBA = SHVec3::Dot(c, BXA);
+    const float DBA = SHVec3::Dot(d, BXA);
+    const float ADC = SHVec3::Dot(a, DXC);
+    const float BDC = SHVec3::Dot(b, DXC);
+
+    return CBA * DBA < 0.0f && ADC * BDC < 0.0f && CBA * BDC > 0.0f;
+  }
+
+  float SHCollision::distanceBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
+  {
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
+    const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
+
+    const SHVec3 HEAD_A = A.GetVertex(HALF_EDGE_A.headVertexIndex);
+    const SHVec3 TAIL_A = A.GetVertex(HALF_EDGE_A.tailVertexIndex);
+    const SHVec3 HEAD_B = B.GetVertex(HALF_EDGE_B.headVertexIndex);
+    const SHVec3 TAIL_B = B.GetVertex(HALF_EDGE_B.tailVertexIndex);
+
+    const SHVec3 DIR_A = SHVec3::Normalise(HEAD_A - TAIL_A);
+    const SHVec3 DIR_B = SHVec3::Normalise(HEAD_B - TAIL_B);
+
+    // Check if the edges are parallel (abs dot product is 1)
+    const float DOT_BETWEEN_EDGES = std::fabs(SHVec3::Dot(DIR_A, DIR_B));
+    if (SHMath::CompareFloat(DOT_BETWEEN_EDGES, 1.0f))
+      return std::numeric_limits<float>::lowest();
+
+    SHVec3 normal = SHVec3::Cross(DIR_A, DIR_B);
+    // Flip normal if need to ( A -> B)
+    if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
+      normal = -normal;
+
+    return SHVec3::Dot(normal, HEAD_B - HEAD_A);
+  }
+
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHBox.cpp

@@ -291,5 +291,29 @@ namespace SHADE
     return SHAABB{ CENTROID, HALF_EXTENTS };
   }
 
+  SHVec3 SHBox::FindSupportPoint(const SHVec3& direction) const noexcept
+  {
+    float bestDistance = std::numeric_limits<float>::lowest();
+    
+
+    SHVec3 vertices[NUM_VERTICES];
+    GetCorners(vertices);
+
+    // No reason to put the center really..
+    SHVec3 bestPoint = Center;
+    for (auto& vertex : vertices)
+    {
+      const float PROJECTION = SHVec3::Dot(vertex, direction);
+
+      if (PROJECTION > bestDistance)
+      {
+        bestDistance = PROJECTION;
+        bestPoint = vertex;
+      }
+    }
+
+    return bestPoint;
+  }
+
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Collision/Shapes/SHBox.h

@@ -114,6 +114,7 @@ namespace SHADE
     [[nodiscard]] SHRaycastResult Raycast           (const SHRay& ray)        const noexcept override;
     [[nodiscard]] SHMatrix        GetTRS            ()                        const noexcept override;
     [[nodiscard]] SHAABB          ComputeAABB       ()                        const noexcept override;
+    [[nodiscard]] SHVec3          FindSupportPoint  (const SHVec3& direction) const noexcept override;
 
   private:
     /*---------------------------------------------------------------------------------*/

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

@@ -150,11 +150,11 @@ namespace SHADE
     const int32_t FACE_VERTICES[NUM_FACES][NUM_VERTICES_PER_FACE]
     {
       { 0, 1, 2, 3 }
-    , { 1, 5, 6, 2 }
+    , { 5, 6, 2, 1 }
     , { 5, 4, 7, 6 }
-    , { 4, 0, 3, 7 }
+    , { 0, 3, 7, 4 }
-    , { 3, 2, 6, 7 }
+    , { 2, 6, 7, 3 }
-    , { 4, 5, 1, 0 }
+    , { 5, 1, 0, 4 }
     };
 
     for (int i = 0; i < NUM_FACES; ++i)

SHADE_Engine/src/Physics/Collision/Shapes/SHConvexPolyhedron.h

@@ -63,9 +63,25 @@ namespace SHADE
     [[nodiscard]] int32_t                               GetHalfEdgeCount      ()  const noexcept;
     [[nodiscard]] const SHHalfEdgeStructure::HalfEdge&  GetHalfEdge           (int index)  const;
 
+    // Virtual Methods
+
     [[nodiscard]] virtual SHVec3 GetVertex        (int index)     const = 0;
     [[nodiscard]] virtual SHVec3 GetNormal        (int faceIndex) const = 0;
 
+    /*---------------------------------------------------------------------------------*/
+    /* Member Functions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    /**
+     * @brief
+     * Finds the most extreme point on the polygon in a given direction.
+     * @param direction
+     * The direction to find the support point in.
+     * @return
+     * The most extreme vertex in the given direction.
+     */
+    [[nodiscard]] virtual SHVec3 FindSupportPoint  (const SHVec3& direction) const noexcept = 0;
+
   protected:
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */