First half of re-implementing face-face contact derivation

Assets/Scenes/PhysicsSandbox.shade

@@ -45,7 +45,7 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 0, y: 0, z: 7}
+      Position: {x: 0, y: 2, z: 7}
       Pitch: 0
       Yaw: 0
       Roll: 0
@@ -62,14 +62,14 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -1.45715916, y: 7.37748241, z: 0.227711335}
+      Translate: {x: -1.5, y: 7.5, z: 0}
-      Rotate: {x: -0, y: 0, 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: 10
+      Mass: 0.52359879
       Drag: 0.00999999978
       Angular Drag: 0
       Use Gravity: true
@@ -84,7 +84,7 @@
       Freeze Rotation Z: false
       IsActive: true
     Collider Component:
-      DrawColliders: true
+      DrawColliders: false
       Colliders:
         - Is Trigger: false
           Collision Tag: 1
@@ -96,18 +96,14 @@
           Position Offset: {x: 0, y: 0, z: 0}
           Rotation Offset: {x: 0, y: 0, z: 0}
       IsActive: true
-  Scripts:
+  Scripts: ~
-    - Type: PhysicsTestObj
-      Enabled: true
-      forceAmount: 50
-      torqueAmount: 500
 - EID: 2
   Name: Default
   IsActive: true
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0, y: 4.09544182, z: 0}
+      Translate: {x: 0, y: 4, z: 0}
       Rotate: {x: -0, y: 0, z: -0.436332315}
       Scale: {x: 4.61071014, y: 0.999995887, z: 1}
       IsActive: true
@@ -272,17 +268,17 @@
   Components:
     Transform Component:
       Translate: {x: 1, y: 2, z: 3}
-      Rotate: {x: -0, y: 0.785398066, z: 0.785398185}
+      Rotate: {x: 0, y: 0.785398185, z: 0.785397708}
-      Scale: {x: 0.999999821, y: 0.999999821, z: 0.999999881}
+      Scale: {x: 0.999990404, y: 0.999994516, z: 0.999985456}
       IsActive: true
     RigidBody Component:
       Type: Dynamic
       Auto Mass: false
       Mass: 1
       Drag: 0.00999999978
-      Angular Drag: 0.00999999978
+      Angular Drag: 0
       Use Gravity: true
-      Gravity Scale: 0.5
+      Gravity Scale: 1
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false
@@ -305,7 +301,11 @@
           Position Offset: {x: 0, y: 0, z: 0}
           Rotation Offset: {x: 0, y: 0, z: 0}
       IsActive: true
-  Scripts: ~
+  Scripts:
+    - Type: PhysicsTestObj
+      Enabled: true
+      forceAmount: 50
+      torqueAmount: 5
 - EID: 8
   Name: Default
   IsActive: true

SHADE_Engine/src/Math/SHMathHelpers.h

@@ -46,7 +46,7 @@ namespace SHADE
     /*---------------------------------------------------------------------------------*/
 
     /** Standard Epsilon value for comparing Single-Precision Floating-Point values. */
-    static constexpr float EPSILON        = 0.001f;
+    static constexpr float EPSILON        = 0.0001f;
     
     /** Single-Precision Floating-Point value of infinity */ 
     static constexpr float INF            = std::numeric_limits<float>::infinity();

SHADE_Engine/src/Physics/Collision/Contacts/SHContact.h

@@ -27,20 +27,16 @@ namespace SHADE
   union SHContactFeatures
   {
   public:
-    /*---------------------------------------------------------------------------------*/
-    /* Type Definit                                                                    */
-    /*---------------------------------------------------------------------------------*/
-
     /*---------------------------------------------------------------------------------*/
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
     struct
     {
-      uint8_t inI;
+      uint8_t inI;  // Incoming Incident Edge
-      uint8_t outI;
+      uint8_t outI; // Outgoing Incident Edge
-      uint8_t inR;
+      uint8_t inR;  // Incoming Reference Edge
-      uint8_t outR;
+      uint8_t outR; // Outgoing Reference Edge
     };
 
     uint32_t key = std::numeric_limits<uint32_t>::max();

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

@@ -11,6 +11,7 @@
 #pragma once
 
 // Project Headers
+#include "Math/Geometry/SHPlane.h"
 #include "Physics/Collision/Contacts/SHManifold.h"
 #include "Physics/Collision/Contacts/SHCollisionKey.h"
 #include "Physics/Collision/Shapes/SHHalfEdgeStructure.h"
@@ -86,15 +87,21 @@ namespace SHADE
       float   bestDistance  = std::numeric_limits<float>::lowest();
     };
 
+    struct ClipVertex
+    {
+      SHVec3            position;
+      SHContactFeatures featurePair;
+    };
+
     /*---------------------------------------------------------------------------------*/
     /* Member Functions                                                                */
     /*---------------------------------------------------------------------------------*/
 
     // Sphere VS Convex
 
-    static FaceQuery  findClosestFace   (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron) noexcept;
+    static FaceQuery  findClosestFace   (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron)                                                                noexcept;
-    static int32_t    findClosestPoint  (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron, int32_t faceIndex) noexcept;
+    static int32_t    findClosestPoint  (const SHSphere& sphere, const SHConvexPolyhedron& polyhedron, int32_t faceIndex)                                             noexcept;
-    static int32_t    findVoronoiRegion (const SHSphere& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2) noexcept;
+    static int32_t    findVoronoiRegion (const SHSphere& sphere, const SHVec3& faceVertex, const SHVec3& faceNormal, const SHVec3& tangent1, const SHVec3& tangent2)  noexcept;
 
     // Capsule VS Convex
 
@@ -102,29 +109,22 @@ 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;
-    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;
-
     /*
-     * TODO: 
-     * 
-     * 
-     * 
-     * 
-     * 
-     * static uint32_t  clip                [sutherland-hodgemann clipping]
-     *
      * ! References
      *  https://ia801303.us.archive.org/30/items/GDC2013Gregorius/GDC2013-Gregorius.pdf
      *  https://github.com/RandyGaul/qu3e/blob/master/src/collision/q3Collide.cpp
      */
 
+    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;
+    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;
 
   };
 } // namespace SHADE

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

@@ -54,6 +54,8 @@ namespace SHADE
     const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
     const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
 
+    // TODO: Check against cached separating axis.
+
     const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
     if (FACE_QUERY_A.bestDistance > 0.0f)
       return false;
@@ -78,6 +80,7 @@ namespace SHADE
       minFaceQuery  = FACE_QUERY_A;
       referencePoly = &POLY_A;
       incidentPoly  = &POLY_B;
+      flipNormal    = false;
     }
     else
     {
@@ -114,7 +117,7 @@ namespace SHADE
       SHContact contact;
       contact.featurePair.key = EDGE_QUERY.axis;
       contact.position        = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
-      contact.penetration     = EDGE_QUERY.bestDistance;
+      contact.penetration     = -EDGE_QUERY.bestDistance;
 
       manifold.contacts[numContacts++] = contact;
       manifold.numContacts = numContacts;
@@ -125,23 +128,67 @@ namespace SHADE
     const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
     const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
 
+    const SHHalfEdgeStructure::Face& INCIDENT_FACE  = incidentPoly->GetFace(INCIDENT_FACE_IDX);
+    const SHHalfEdgeStructure::Face& REFERENCE_FACE = referencePoly->GetFace(minFaceQuery.closestFace);
 
-    /*
+    const int32_t NUM_INCIDENT_VERTICES = static_cast<int32_t>(INCIDENT_FACE.vertexIndices.size());
-     * TODO:
+    const int32_t NUM_REFERENCE_VERTICES = static_cast<int32_t>(REFERENCE_FACE.vertexIndices.size());
-     *
-     * !! 
-     * 4. From above, save the axis of minimum penetration (reference face)
-     * 5. Find the most anti-parallel face on other shape (incident face)
-     * 6. Clip incident face against side planes of reference face. (Sutherland-Hodgeman Clipping).
-     *    Keep all vertices below reference face.
-     * 7. Reduce manifold to 4 contact points. We only need 4 contact points for a stable manifold.
-     *
-     * Remember to save IDs in queries.
-     * During generation of incident face, store IDs of face.
-     * 
-     */
 
+    // Build incoming vertices to clip
+    std::vector<ClipVertex> clipIn;
+    clipIn.resize(NUM_INCIDENT_VERTICES * 2, ClipVertex{});
 
+    int32_t numClipIn = 0;
+    for (int32_t i = 0; i < NUM_INCIDENT_VERTICES; ++i)
+    {
+      const int32_t prevI = i - 1 < 0 ? NUM_INCIDENT_VERTICES - 1 : i - 1;
+
+      const int32_t V_INDEX = INCIDENT_FACE.vertexIndices[i].index;
+
+      // The incoming id is the previous edge
+      // The outgoing id is the current edge (where this vertex is the tail of)
+
+      ClipVertex v;
+      v.position          = incidentPoly->GetVertex(V_INDEX);
+      v.featurePair.inI   = INCIDENT_FACE.vertexIndices[prevI].edgeIndex;
+      v.featurePair.outI  = INCIDENT_FACE.vertexIndices[i].edgeIndex;
+      v.featurePair.inR   = 0;
+      v.featurePair.outR  = 0;
+
+      clipIn[numClipIn++] = v;
+    }
+
+    // Clip the vertices against the reference face side planes.
+    // Number of side planes == number of edges == number of vertices
+    for (int32_t i = 0; i < NUM_REFERENCE_VERTICES; ++i)
+    {
+      // Side plane can be built with the vertex on the edge and the plane's normal
+      // Plane normal = faceNormal X tangent (v2 - v1)
+
+      const int32_t V1_INDEX = REFERENCE_FACE.vertexIndices[i].index;
+      const int32_t V2_INDEX = REFERENCE_FACE.vertexIndices[(i + 1) % NUM_REFERENCE_VERTICES].index;
+
+      const SHVec3  V1 = referencePoly->GetVertex(V1_INDEX);
+      const SHVec3  V2 = referencePoly->GetVertex(V2_INDEX);
+
+      const SHVec3  TANGENT = SHVec3::Normalise(V2 - V1);
+      const SHPlane CLIP_PLANE { V1, SHVec3::Cross(REFERENCE_NORMAL, TANGENT) };
+
+      std::vector<ClipVertex> clipOut = clipPolygonWithPlane(clipIn, numClipIn, CLIP_PLANE, REFERENCE_FACE.vertexIndices[i].edgeIndex);
+      if (clipOut.empty())
+        return false;
+
+      // Replace the clip container's contents with the clipped points for the next clipping pass
+      const int32_t NUM_CLIPPED = static_cast<int32_t>(clipOut.size());
+      for (int32_t clippedIndex = 0; clippedIndex < NUM_CLIPPED; ++clippedIndex)
+      {
+        clipIn[clippedIndex].position         = clipOut[clippedIndex].position;
+        clipIn[clippedIndex].featurePair.key  = clipOut[clippedIndex].featurePair.key;
+      }
+      numClipIn = NUM_CLIPPED;
+    }
+
+    // From the final set of clipped points, only keep the points that are below the reference plane.
 
     return false;
   }
@@ -353,5 +400,77 @@ namespace SHADE
     return bestFace;
   }
 
+  std::vector<SHCollision::ClipVertex> SHCollision::clipPolygonWithPlane(const std::vector<ClipVertex>& in, int32_t numIn, const SHPlane& plane, int32_t planeIdx) noexcept
+  {
+    std::vector<ClipVertex> out;
+
+    int32_t v1Index = numIn - 1;
+    int32_t v2Index = 0;
+
+    float v1Distance = plane.SignedDistance(in[v1Index].position);
+    float v2Distance = 0.0f;
+
+    for (int32_t i = 0; i < numIn; ++i)
+    {
+      v2Index = i;
+
+      const SHVec3 v1Pos = in[v1Index].position;
+      const SHVec3 v2Pos = in[v2Index].position;
+
+      v2Distance = plane.SignedDistance(v2Pos);
+
+      // v1 in front, v2 behind
+      // keep the intersection point
+      if (v1Distance >= 0.0f && v2Distance < 0.0f)
+      {
+        ClipVertex intersection;
+
+        // In case the edge is parallel, the intersection is just the start point
+        const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f);
+        const float ALPHA = IS_PARALLEL ? 0.0f : v1Distance / SHVec3::Distance(v1Pos, v2Pos);
+
+        intersection.position         = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
+        intersection.featurePair.inI  = in[v1Index].featurePair.outI;
+        intersection.featurePair.outI = 0;
+        intersection.featurePair.inR  = 0;
+        intersection.featurePair.outR = planeIdx;
+
+        out.emplace_back(intersection);
+      }
+
+      // v1 behind, v2 in front
+      // keep intersection point & v2
+      if(v1Distance < 0.0f && v2Distance >= 0.0f)
+      {
+        ClipVertex intersection;
+
+        // In case the edge is parallel, the intersection is just the start point
+        const bool IS_PARALLEL = SHMath::CompareFloat(SHVec3::Dot(v2Pos - v1Pos, plane.GetNormal()), 0.0f);
+        const float ALPHA = IS_PARALLEL ? 0.0f : -v1Distance / SHVec3::Distance(v1Pos, v2Pos);
+
+        intersection.position         = SHVec3::ClampedLerp(v1Pos, v2Pos, ALPHA);
+        intersection.featurePair.inI  = 0;
+        intersection.featurePair.outI = in[v2Index].featurePair.inI;
+        intersection.featurePair.inR  = planeIdx;
+        intersection.featurePair.outR = 0;
+
+        out.emplace_back(intersection);
+        out.emplace_back(in[v2Index]);
+      }
+
+      // both in front, keep v2
+      if (v1Distance >= 0.0f && v2Distance >= 0.0f)
+      {
+        out.emplace_back(in[v2Index]);
+      }
+
+      // v2 is the next v1
+      v1Index    = v2Index;
+      v1Distance = v2Distance;
+    }
+
+    return out;
+  }
+
 
 } // namespace SHADE

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -187,7 +187,7 @@ namespace SHADE
          * restituion bias  = restitution * (relative velocity /dot normal)
          */
 
-        const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP);
+        const float ERROR_BIAS = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP);
 
         // Warm starting
         // Compute impulses
@@ -244,6 +244,31 @@ namespace SHADE
         SHVec3 velocityB = vB + SHVec3::Cross(wB, contact.rB);
         SHVec3 relativeVelocity = velocityB - velocityA;
 
+        // Get scalar of relative velocity along the normal
+        const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
+
+        // Compute true normal impulse
+        const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
+
+        float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
+        contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
+        newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
+
+        const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
+
+        // Apply impulses
+        vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
+        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
+
+        vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
+        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
+
+        // Solve normal impulse
+        // Re-compute relative velocity
+        velocityA = vA + SHVec3::Cross(wA, contact.rA);
+        velocityB = vB + SHVec3::Cross(wB, contact.rB);
+        relativeVelocity = velocityB - velocityA;
+
         // Solve tangent impulse
         for (int j = 0; j < SHContact::NUM_TANGENTS; ++j)
         {
@@ -269,31 +294,6 @@ namespace SHADE
           vB += TANGENT_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
           wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, TANGENT_IMPULSE) * ANGULAR_LOCK_B;
         }
-
-        // Solve normal impulse
-        // Re-compute relative velocity
-        velocityA = vA + SHVec3::Cross(wA, contact.rA);
-        velocityB = vB + SHVec3::Cross(wB, contact.rB);
-        relativeVelocity = velocityB - velocityA;
-
-        // Get scalar of relative velocity along the normal
-        const float VN = SHVec3::Dot(relativeVelocity, constraint.normal);
-
-        // Compute true normal impulse
-        const float OLD_NORMAL_IMPULSE = contact.normalImpulse;
-
-        float newNormalImpulse = -(VN + contact.bias) * contact.normalMass;
-        contact.normalImpulse = std::max(OLD_NORMAL_IMPULSE + newNormalImpulse, 0.0f);
-        newNormalImpulse = contact.normalImpulse - OLD_NORMAL_IMPULSE;
-
-        const SHVec3 NORMAL_IMPULSE = newNormalImpulse * constraint.normal;
-
-        // Apply impulses
-        vA -= NORMAL_IMPULSE * constraint.invMassA * LINEAR_LOCK_A;
-        wA -= constraint.invInertiaA * SHVec3::Cross(contact.rA, NORMAL_IMPULSE) * ANGULAR_LOCK_A;
-
-        vB += NORMAL_IMPULSE * constraint.invMassB * LINEAR_LOCK_B;
-        wB += constraint.invInertiaB * SHVec3::Cross(contact.rB, NORMAL_IMPULSE) * ANGULAR_LOCK_B;
       }
 
       velocityStateA.LinearVelocity   = vA;

SHADE_Engine/src/Physics/SHPhysicsConstants.h

@@ -26,7 +26,7 @@ namespace SHADE
    * @brief
    * Linear Collision & Constraint tolerance.
   */
-  static constexpr float SHPHYSICS_LINEAR_SLOP              = 0.05f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
+  static constexpr float SHPHYSICS_LINEAR_SLOP              = 0.01f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
 
   /**
    * @brief