Assets/Scenes/PhysicsSandbox.shade

@@ -280,7 +280,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.524352431, y: 11.1989822, z: 0.0463808179}
+      Translate: {x: 0.524352431, y: 11.5, z: 0.0463808179}
       Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
       Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
       IsActive: true

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

@@ -107,7 +107,7 @@ namespace SHADE
     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;
-    static SHVec3     findClosestPointBetweenEdges  (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB, SHVec3& normal) noexcept;
+    static SHVec3     findClosestPointBetweenEdges  (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept;
 
     /*
      * TODO: 

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

@@ -83,6 +83,8 @@ namespace SHADE
 
     const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B;
 
+    uint32_t numContacts = 0;
+
     // 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.
@@ -91,22 +93,31 @@ namespace SHADE
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
 
+      const SHVec3 HEAD_A = POLY_A.GetVertex(HALF_EDGE_A.headVertexIndex);
+      const SHVec3 TAIL_A = POLY_A.GetVertex(HALF_EDGE_A.tailVertexIndex);
+      const SHVec3 HEAD_B = POLY_B.GetVertex(HALF_EDGE_B.headVertexIndex);
+      const SHVec3 TAIL_B = POLY_B.GetVertex(HALF_EDGE_B.tailVertexIndex);
+
+      const SHVec3 VA = SHVec3::Normalise(HEAD_A - TAIL_A);
+      const SHVec3 VB = SHVec3::Normalise(HEAD_B - TAIL_B);
+
+      manifold.normal = SHVec3::Cross(VB, VA);
+      // Flip normal if need to ( A -> B)
+      if (SHVec3::Dot(manifold.normal, HEAD_A - A.GetWorldCentroid()) < 0.0f)
+        manifold.normal = -manifold.normal;
+
       // 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.featurePair.typeA   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      contact.featurePair.indexA  = HALF_EDGE_A.tailVertexIndex;
+      contact.featurePair.typeB   = SHUtilities::ConvertEnum(SHContactFeatures::Type::VERTEX);
+      contact.featurePair.indexB  = HALF_EDGE_B.tailVertexIndex;
 
-      contact.position    = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB, manifold.normal);
+      contact.position    = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
       contact.penetration = EDGE_QUERY.bestDistance;
 
-      manifold.contacts[0] = contact;
+      manifold.contacts[numContacts++] = contact;
-      manifold.numContacts = 1;
+      manifold.numContacts = numContacts;
 
       return true;
     }
@@ -237,8 +248,22 @@ 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
+  SHVec3 SHCollision::findClosestPointBetweenEdges(const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB) noexcept
   {
+    /*
+     * The two edges can be parameterised in the form p + tv
+     *
+     * LA(r) = A + rVA
+     * LB(s) = B + sVB
+     *
+     * The vector between the closest points is the cross product of VA and VB.
+     * Since the cross product is orthogonal to VA and VB, we can generalise this as:
+     * (LB(s) - LA(r)) /dot VA = 0
+     * (LB(s) - LA(r)) /dot VB = 0
+     *
+     * Where LB(s) - LA(r) is the same vector as VB X VA.
+     */
+
     const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(edgeA);
     const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(edgeB);
 
@@ -250,37 +275,35 @@ namespace SHADE
     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
+     * C' = TAIL_B - TAIL_A
-     * b = -VAxUx - VAyUy - VAzUz
+     *
-     * c = (Cx'Ux + Cy'Uy + Cz'Uz)
+     * a =  VB /dot U
+     * b = -VA /dot U
+     * c =  C' /dot U
+     *
+     * U is either VA or VB
      */
 
     const SHVec3 C = TAIL_B - TAIL_A;
 
-    const float A1 =  VB.x * VA.x + VB.y * VA.y + VB.z * VA.z;
+    const float VB_DOT_VA = SHVec3::Dot(VB, VA);
-    const float A2 =  VB.x * VB.x + VB.y * VB.y + VB.z * VB.z;
+    const float VB_DOT_VB = SHVec3::Dot(VB, VB);
-    const float B1 = -VA.x * VA.x + -VA.y * VA.y + -VA.z * VA.z;
+    const float AV_DOT_VA = SHVec3::Dot(-VA, VA);
-    const float B2 = -VA.x * VB.x + -VA.y * VB.y + -VA.z * VB.z;
+    const float AV_DOT_VB = SHVec3::Dot(-VA, VB);
-    const float C1 = C.x * VA.x + C.y + VA.y + C.z + VA.z;
+    const float C_DOT_VA  = SHVec3::Dot(C, VA);
-    const float C2 = C.x * VB.x + C.y + VB.y + C.z + VB.z;
+    const float C_DOT_VB  = SHVec3::Dot(C, VB);
 
     /*
      * R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 )
      * S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1)
+     *
+     * We only need to solve for R
      */
     
-    const float R = -C2 / (B2 - (A2 * (C1 + B1)) / A1);
+    const float R = -C_DOT_VB / (AV_DOT_VB - (VB_DOT_VB * (C_DOT_VA + AV_DOT_VA)) / VB_DOT_VA);
 
     // Just take a point from A since it's A -> B
     return TAIL_A + R * VA;