Added edge vs edge contacts for convex polyhedron collisions

Assets/Scenes/PhysicsSandbox.shade

@@ -45,9 +45,9 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 1, y: 10, z: 3}
+      Position: {x: -0.5, y: 10, z: -3}
       Pitch: 0
-      Yaw: 0
+      Yaw: 180
       Roll: 0
       Width: 1920
       Height: 1080
@@ -84,7 +84,7 @@
       Freeze Rotation Z: false
       IsActive: true
     Collider Component:
-      DrawColliders: false
+      DrawColliders: true
       Colliders:
         - Is Trigger: false
           Collision Tag: 1
@@ -95,7 +95,7 @@
           Density: 1
           Position Offset: {x: 0, y: 0, z: 0}
           Rotation Offset: {x: 0, y: 0, z: 0}
-        - Is Trigger: false
+        - Is Trigger: true
           Collision Tag: 1
           Type: Sphere
           Radius: 0.5
@@ -280,9 +280,9 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.899999976, y: 10, z: 0}
+      Translate: {x: 0.524352431, y: 11.1989822, z: 0.0463808179}
-      Rotate: {x: -0, y: 0, z: 0.785398185}
+      Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
-      Scale: {x: 1, y: 1, z: 1}
+      Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
       IsActive: true
     RigidBody Component:
       Type: Dynamic
@@ -295,7 +295,7 @@
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
-      Freeze Position Y: true
+      Freeze Position Y: false
       Freeze Position Z: false
       Freeze Rotation X: false
       Freeze Rotation Y: false
@@ -304,7 +304,7 @@
     Collider Component:
       DrawColliders: false
       Colliders:
-        - Is Trigger: true
+        - Is Trigger: false
           Collision Tag: 1
           Type: Box
           Half Extents: {x: 1, y: 1, z: 1}
@@ -322,13 +322,13 @@
   Components:
     Transform Component:
       Translate: {x: -0.5, y: 10, z: 0}
-      Rotate: {x: -0, y: 0.785398185, z: -0}
+      Rotate: {x: -0, y: 0, z: -0}
       Scale: {x: 1, y: 1, z: 1}
       IsActive: true
     RigidBody Component:
-      Type: Dynamic
+      Type: Static
       Auto Mass: false
-      Mass: 1
+      Mass: .inf
       Drag: 0.00999999978
       Angular Drag: 0.00999999978
       Use Gravity: true
@@ -345,7 +345,7 @@
     Collider Component:
       DrawColliders: false
       Colliders:
-        - Is Trigger: true
+        - Is Trigger: false
           Collision Tag: 1
           Type: Box
           Half Extents: {x: 1, y: 1, z: 1}

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

@@ -101,12 +101,13 @@ namespace SHADE
 
     // Convex VS Convex
 
-    static FaceQuery  queryFaceDirections (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
+    static FaceQuery  queryFaceDirections           (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept;
-    static EdgeQuery  queryEdgeDirections (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       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 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;
+    static float      distanceBetweenEdges          (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
+    static SHVec3     findClosestPointBetweenEdges  (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB, SHVec3& normal) noexcept;
 
     /*
      * TODO: 

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

@@ -17,6 +17,7 @@
 #include "Math/SHMathHelpers.h"
 #include "Math/Geometry/SHPlane.h"
 #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
+#include "Tools/Utilities/SHUtilities.h"
 
 namespace SHADE
 {
@@ -65,6 +66,55 @@ namespace SHADE
      * 
      */
 
+    const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
+    const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
+
+    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;
+
+    const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B;
+
+    // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
+    // each edge and use that as the contact point.
+    // Artificially increase the depth of this collision by a small tolerance to tend towards picking a face query in the next frame.
+    if (EDGE_QUERY.bestDistance > BEST_FACE_QUERY.bestDistance)
+    {
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
+
+      // In this scenario, we only have one contact
+
+      SHContactFeatures featurePair;
+      featurePair.typeA   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      featurePair.indexA  = HALF_EDGE_A.tailVertexIndex;
+      featurePair.typeB   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      featurePair.indexB  = HALF_EDGE_B.tailVertexIndex;
+
+      SHContact contact;
+      contact.featurePair = featurePair;
+
+      contact.position    = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB, manifold.normal);
+      contact.penetration = EDGE_QUERY.bestDistance;
+
+      manifold.contacts[0] = contact;
+      manifold.numContacts = 1;
+
+      return true;
+    }
+
+    // Use a bias to favour a normal in the direction of A -> B
+
+
+
     return false;
   }
 
@@ -187,4 +237,54 @@ namespace SHADE
     return SHVec3::Dot(normal, HEAD_B - HEAD_A);
   }
 
+  SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB, SHVec3& normal) 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 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
+    const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
+
+    normal = SHVec3::Cross(VB, VA);
+    // Flip normal if need to ( A -> B)
+    if (SHVec3::Dot(normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
+      normal = -normal;
+
+    
+    /*
+     * Find a1, a2, b1, b2, c1, c2 to solve the simultaneous equation
+     * C' = TAIL_B - TAIL_A
+     * U  = VA / VB
+     *
+     * a = VBxUx + VByUy + VBzUz
+     * b = -VAxUx - VAyUy - VAzUz
+     * c = (Cx'Ux + Cy'Uy + Cz'Uz)
+     */
+
+    const SHVec3 C = TAIL_B - TAIL_A;
+
+    const float A1 =  VB.x * VA.x + VB.y * VA.y + VB.z * VA.z;
+    const float A2 =  VB.x * VB.x + VB.y * VB.y + VB.z * VB.z;
+    const float B1 = -VA.x * VA.x + -VA.y * VA.y + -VA.z * VA.z;
+    const float B2 = -VA.x * VB.x + -VA.y * VB.y + -VA.z * VB.z;
+    const float C1 = C.x * VA.x + C.y + VA.y + C.z + VA.z;
+    const float C2 = C.x * VB.x + C.y + VB.y + C.z + VB.z;
+
+    /*
+     * R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 )
+     * S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1)
+     */
+    
+    const float R = -C2 / (B2 - (A2 * (C1 + B1)) / A1);
+
+    // Just take a point from A since it's A -> B
+    return TAIL_A + R * VA;
+  }
+
+
 } // namespace SHADE