Fixed convex-convex face detection

Minor bugs with contact point detection. Will test more before pushing into main

Assets/Scenes/PhysicsSandbox.shade

@@ -267,9 +267,9 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 1, y: 2, z: 3}
+      Translate: {x: 1.81218028, y: 2, z: 3}
-      Rotate: {x: 0, y: 0.785398185, z: 0.785397708}
+      Rotate: {x: 0.785398006, y: 0.785398483, z: 0.785398304}
-      Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456}
+      Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
       IsActive: true
     RigidBody Component:
       Type: Dynamic

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

@@ -124,7 +124,7 @@ namespace SHADE
     static SHVec3                   findClosestPointBetweenEdges  (const SHConvexPolyhedron& A, const SHConvexPolyhedron& B, int32_t edgeA, int32_t edgeB)    noexcept;
     static int32_t                  findIncidentFace              (const SHConvexPolyhedron& poly, const SHVec3& normal)                                      noexcept;
     static std::vector<ClipVertex>  clipPolygonWithPlane          (const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx)  noexcept;
-    static std::vector<ClipVertex>  reduceContacts                (const std::vector<ClipVertex>& in)                                                         noexcept;
+    static std::vector<int32_t>     reduceContacts                (const std::vector<SHContact>& in, const SHVec3& faceNormal)                                noexcept;
 
   };
 } // namespace SHADE

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

@@ -189,8 +189,78 @@ namespace SHADE
     }
 
     // From the final set of clipped points, only keep the points that are below the reference plane.
+    const SHPlane REFERENCE_PLANE{ referencePoly->GetVertex(REFERENCE_FACE.vertexIndices.front().index), REFERENCE_NORMAL };
 
-    return false;
+    std::vector<SHContact> contacts;
+    for (int32_t i = 0; i < numClipIn; ++i)
+    {
+      const SHVec3 POS = clipIn[i].position;
+      const float DIST = REFERENCE_PLANE.SignedDistance(POS);
+      if (DIST <= 0.0f)
+      {
+        SHContact contact;
+        contact.position = POS;
+        contact.penetration = -DIST;
+
+        if (flipNormal)
+        {
+          contact.featurePair.inI = clipIn[i].featurePair.inR;
+          contact.featurePair.inR = clipIn[i].featurePair.inI;
+          contact.featurePair.outI = clipIn[i].featurePair.outR;
+          contact.featurePair.outR = clipIn[i].featurePair.outI;
+        }
+        else
+        {
+          contact.featurePair.key = clipIn[i].featurePair.key;
+        }
+
+        contacts.emplace_back(contact);
+        ++numContacts;
+      }
+    }
+
+    // Reduce contact manifold if more than 4 points
+    if (numContacts > 4)
+    {
+      const auto INDICES_TO_KEEP = reduceContacts(contacts, REFERENCE_NORMAL);
+      std::vector<SHContact> reducedContacts;
+
+      const int32_t NUM_REDUCED = static_cast<int32_t>(INDICES_TO_KEEP.size());
+      for (int32_t i = 0; i < NUM_REDUCED; ++i)
+        reducedContacts.emplace_back(contacts[INDICES_TO_KEEP[i]]);
+
+      contacts.clear();
+      // Copy contacts to main container
+      for (auto& contact : reducedContacts)
+        contacts.emplace_back(contact);
+    }
+
+    // Remove potential duplicate contact points
+    // No way about this being an n^2 loop
+    static constexpr float THRESHOLD = SHPHYSICS_SAME_CONTACT_DISTANCE * SHPHYSICS_SAME_CONTACT_DISTANCE;
+    for (auto i = contacts.begin(); i != contacts.end(); ++i)
+    {
+      for (auto j = i + 1; j != contacts.end();)
+      {
+        const float D2 = SHVec3::DistanceSquared(i->position, j->position);
+        if (D2 < THRESHOLD)
+          j = contacts.erase(j);
+        else
+          ++j;
+      }
+    }
+
+    // Copy final contacts into the manifold
+    numContacts = static_cast<int32_t>(contacts.size());
+    for (int32_t i = 0; i < numContacts; ++i)
+      manifold.contacts[i] = contacts[i];
+
+    manifold.numContacts  = numContacts;
+    manifold.normal       = REFERENCE_NORMAL;
+    if (flipNormal)
+      manifold.normal = -manifold.normal;
+
+    return true;
   }
 
   /*-----------------------------------------------------------------------------------*/
@@ -472,5 +542,141 @@ namespace SHADE
     return out;
   }
 
+  std::vector<int32_t> SHCollision::reduceContacts(const std::vector<SHContact>& in, const SHVec3& faceNormal) noexcept
+  {
+    std::vector<int32_t> indicesToKeep;
+
+    // Use a map to temporarily store and track the contacts we want
+    std::unordered_map<int, const SHContact*> contactMap;
+    const int32_t NUM_CONTACTS = static_cast<int32_t>(in.size());
+    for (int32_t i = 0; i < NUM_CONTACTS; ++i)
+      contactMap.emplace(i, &in[i]);
+
+    // Find the furthest point in a given direction
+    int32_t indexToKeep = -1;
+    float bestDistance  = std::numeric_limits<float>::lowest();
+
+    for (const auto& [index, contact] : contactMap)
+    {
+      const float DIST = SHVec3::Dot(contact->position, SHVec3::One);
+      if (DIST > bestDistance)
+      {
+        bestDistance  = DIST;
+        indexToKeep   = index;
+      }
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    contactMap.erase(indexToKeep);
+
+
+    indexToKeep   = -1;
+    bestDistance  = std::numeric_limits<float>::lowest();
+
+    // Find point furthest away from the first index
+    const SHVec3& FIRST_POS = in[indicesToKeep.back()].position;
+    for (const auto& [index, contact] : contactMap)
+    {
+      const float DIST_SQUARED = SHVec3::DistanceSquared(FIRST_POS, contact->position);
+      if (DIST_SQUARED > bestDistance)
+      {
+        bestDistance  = DIST_SQUARED;
+        indexToKeep   = index;
+      }
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    contactMap.erase(indexToKeep);
+
+    indexToKeep = -1;
+
+    // We compute the triangle with the largest area.
+    // The area can be positive or negative depending on the winding order.
+
+    float maxArea = std::numeric_limits<float>::lowest();
+    float minArea = std::numeric_limits<float>::max();
+
+    int32_t maxAreaIndex = -1;
+    int32_t minAreaIndex = -1;
+
+    const SHVec3& SECOND_POS = in[indicesToKeep.back()].position;
+    for (const auto& [index, contact] : contactMap)
+    {
+      const SHVec3& POS   = contact->position;
+      const SHVec3 TO_P1  = FIRST_POS - POS;
+      const SHVec3 TO_P2  = SECOND_POS - POS;
+
+      const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
+
+      if (AREA > maxArea)
+      {
+        maxArea = AREA;
+        maxAreaIndex = index;
+      }
+
+      if (AREA < minArea)
+      {
+        minArea = AREA;
+        minAreaIndex = index;
+      }
+    }
+
+    // Compare which triangle creates the largest area
+    bool isAreaPositive = false;
+    if (maxArea > (-minArea))
+    {
+      isAreaPositive  = true;
+      indexToKeep     = maxAreaIndex;
+    }
+    else
+    {
+      isAreaPositive  = false;
+      indexToKeep     = minAreaIndex;
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    contactMap.erase(indexToKeep);
+
+    indexToKeep = -1;
+
+    // For the last point, we want the point which forms the largest area that is winded opposite to the first triangle
+    // The areas should be inverted: If area was -ve, we want a +ve. Otherwise, vice versa.
+    float bestArea = 0.0f;
+
+    const SHVec3& THIRD_POS = in[indicesToKeep.back()].position;
+    const SHVec3 ABC[3] = { FIRST_POS, SECOND_POS, THIRD_POS };
+
+    for (const auto& [index, contact] : contactMap)
+    {
+      const SHVec3& Q = contact->position;
+
+      for (int i = 0; i < 3; ++i)
+      {
+        const int P1 = i;
+        const int P2 = (i + 1) % 3;
+
+        const SHVec3 TO_P1 = ABC[P1] - Q;
+        const SHVec3 TO_P2 = ABC[P2] - Q;
+
+        const float AREA = SHVec3::Dot(SHVec3::Cross(TO_P1, TO_P2), faceNormal) * 0.5f;
+
+        if (isAreaPositive && AREA < bestArea)
+        {
+          bestArea    = AREA;
+          indexToKeep = index;
+        }
+
+        if (!isAreaPositive && AREA > bestArea)
+        {
+          bestArea    = AREA;
+          indexToKeep = index;
+        }
+      }
+    }
+
+    indicesToKeep.emplace_back(indexToKeep);
+    return indicesToKeep;
+  }
+
 
 } // namespace SHADE

SHADE_Engine/src/Physics/SHPhysicsConstants.h

@@ -34,11 +34,17 @@ namespace SHADE
   */
   static constexpr float SHPHYSICS_RESTITUTION_THRESHOLD    = 1.0f;
 
- /**
+  /**
   * @brief
   * Scaling factor to control how fast overlaps are resolved.<br/>
   * 1 is ideal for instant correction, but values close to 1 can lead to overshoot.
- */
+  */
- static constexpr float SHPHYSICS_BAUMGARTE                 = 0.2f;
+  static constexpr float SHPHYSICS_BAUMGARTE                = 0.2f;
+
+  /**
+   * @brief
+   * Distance threshold to consider two contacts as the same.
+  */
+  static constexpr float SHPHYSICS_SAME_CONTACT_DISTANCE    = 0.01;
 
 }