Cached SAT for improved stability

The effects of baumgarte stabilisation can be rather obvious especially when polyhedrons are thrown around at angles. Regardless, the system is relatively stable bar the added energy from the solving method, which may make for a more "bombastic" physics playground

Assets/Scenes/PhysicsSandbox.shade

@@ -45,7 +45,7 @@
   NumberOfChildren: 0
   Components:
     Camera Component:
-      Position: {x: 0, y: 2, z: 7}
+      Position: {x: 0, y: 2, z: 10}
       Pitch: 0
       Yaw: 0
       Roll: 0
@@ -62,7 +62,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: -1, y: 7.5, z: 0}
+      Translate: {x: -2, y: 7.5, z: 0}
       Rotate: {x: -0, y: 0, z: 0.785398185}
       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
@@ -267,8 +267,8 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 2, y: 2, z: 3}
+      Translate: {x: 0, y: 7, z: 0}
-      Rotate: {x: 0.785398185, y: 0.785398185, z: 0.785398185}
+      Rotate: {x: 0, y: 0.785398185, z: 0}
       Scale: {x: 0.999990404, y: 0.999994457, z: 0.999985337}
       IsActive: true
     RigidBody Component:

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

@@ -38,7 +38,7 @@ namespace SHADE
   {
   public:
     SHCollisionKey     info;
-    SHCollisionState  state;
+    SHCollisionState  state = SHCollisionState::INVALID;
   };
 
   /**
@@ -52,7 +52,7 @@ namespace SHADE
     static constexpr int MAX_NUM_CONTACTS = 4;
 
     SHCollisionKey    info;
-    SHCollisionState  state;
+    SHCollisionState  state = SHCollisionState::INVALID;
     SHVec3            normal;
     SHVec3            contactPoints[MAX_NUM_CONTACTS];
   };

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

@@ -12,6 +12,7 @@
 
 // Primary Header
 #include "Physics/Dynamics/SHRigidBody.h"
+#include "Physics/Collision/Narrowphase/SHSATInfo.h"
 #include "Physics/Collision/Shapes/SHCollisionShape.h"
 #include "SHContact.h"
 #include "SHCollisionEvents.h"
@@ -58,6 +59,8 @@ namespace SHADE
     uint32_t          numContacts = 0;
     SHCollisionState  state       = SHCollisionState::INVALID;
 
+    SHSATInfo         cachedSATInfo;
+
     SHVec3            normal;
     SHVec3            tangents[SHContact::NUM_TANGENTS];
     SHContact         contacts[MAX_NUM_CONTACTS];

SHADE_Engine/src/Physics/Collision/Contacts/SHManifold.hpp

@@ -34,6 +34,7 @@ namespace SHADE
   , bodyB         { rhs.bodyB }
   , numContacts   { rhs.numContacts }
   , state         { rhs.state }
+  , cachedSATInfo { rhs.cachedSATInfo }
   , normal        { rhs.normal }
   {
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
@@ -50,6 +51,7 @@ namespace SHADE
   , bodyB         { rhs.bodyB }
   , numContacts   { rhs.numContacts }
   , state         { rhs.state }
+  , cachedSATInfo { rhs.cachedSATInfo }
   , normal        { rhs.normal }
   {
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
@@ -74,6 +76,7 @@ namespace SHADE
     bodyB         = rhs.bodyB;
     numContacts   = rhs.numContacts;
     state         = rhs.state;
+    cachedSATInfo = rhs.cachedSATInfo;
     normal        = rhs.normal;
 
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
@@ -93,6 +96,7 @@ namespace SHADE
     bodyB         = rhs.bodyB;
     numContacts   = rhs.numContacts;
     state         = rhs.state;
+    cachedSATInfo = rhs.cachedSATInfo;
     normal        = rhs.normal;
 
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);

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

@@ -139,6 +139,8 @@ namespace SHADE
     , const SHConvexPolyhedron& B
     ) noexcept;
 
+    
+
     static EdgeQuery queryEdgeDirections
     (
       const SHConvexPolyhedron& A
@@ -207,5 +209,14 @@ namespace SHADE
     , const SHVec3&                 faceNormal
     ) noexcept;
 
+    // Cached Convex VS Convex
+
+    static bool cachedConvexVSConvex
+    (
+      SHManifold&               manifold
+    , const SHSATInfo&          cachedInfo
+    , const SHConvexPolyhedron& A
+    , const SHConvexPolyhedron& B
+    ) noexcept;
   };
 } // namespace SHADE

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

@@ -48,25 +48,56 @@ namespace SHADE
 
   bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
-    static constexpr float ABSOLUTE_TOLERANCE = 0.01f;
+    static constexpr float ABSOLUTE_TOLERANCE = 0.01f;  // 0.0005
-    static constexpr float RELATIVE_TOLERANCE = 0.95f;
+    static constexpr float RELATIVE_TOLERANCE = 0.95f;  // 1.0002
 
     const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
     const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
 
-    // TODO: Check against cached separating axis.
+    if (manifold.cachedSATInfo.type != SHSATInfo::Type::INVALID)
+      return cachedConvexVSConvex(manifold, manifold.cachedSATInfo, POLY_A, POLY_B);
+
+    SHSATInfo cachedInfo;
 
     const FaceQuery FACE_QUERY_A = queryFaceDirections(POLY_A, POLY_B);
     if (FACE_QUERY_A.bestDistance > 0.0f)
+    {
+      // cache the info
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
+      cachedInfo.info.refFace = FACE_QUERY_A.closestFace;
+
+      manifold.cachedSATInfo  = cachedInfo;
+
       return false;
+    }
+      
 
     const FaceQuery FACE_QUERY_B = queryFaceDirections(POLY_B, POLY_A);
     if (FACE_QUERY_B.bestDistance > 0.0f)
+    {
+      // cache the info
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
+      cachedInfo.info.refFace = FACE_QUERY_B.closestFace;
+
+      manifold.cachedSATInfo  = cachedInfo;
+
       return false;
+    }
 
     const EdgeQuery EDGE_QUERY = queryEdgeDirections(POLY_A, POLY_B);
     if (EDGE_QUERY.bestDistance > 0.0f)
+    {
+      // cache the info
+      cachedInfo.type       = SHSATInfo::Type::EDGE;
+      cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
+      cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
+
+      manifold.cachedSATInfo  = cachedInfo;
+
       return false;
+    }
 
     // Apply weight to improve frame coherence of normal directions.
     // We want a normal in the direction from A -> B, so we flip the normal if needed.
@@ -81,6 +112,10 @@ namespace SHADE
       referencePoly = &POLY_A;
       incidentPoly  = &POLY_B;
       flipNormal    = false;
+
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::A;
+      cachedInfo.info.refFace = minFaceQuery.closestFace;
     }
     else
     {
@@ -88,9 +123,13 @@ namespace SHADE
       referencePoly = &POLY_B;
       incidentPoly  = &POLY_A;
       flipNormal    = true;
+
+      cachedInfo.type         = SHSATInfo::Type::FACE;
+      cachedInfo.info.refPoly = SHSATInfo::ShapeID::B;
+      cachedInfo.info.refFace = minFaceQuery.closestFace;
     }
 
-    uint32_t numContacts = 0;
+    
 
 
     // If an edge pair contains the closest distance,vwe ignore any face queries and find the closest points on
@@ -114,6 +153,8 @@ namespace SHADE
         manifold.normal = -manifold.normal;
 
       // In this scenario, we only have one contact
+      uint32_t numContacts = 0;
+
       SHContact contact;
       contact.featurePair.key = EDGE_QUERY.axis;
       contact.position        = findClosestPointBetweenEdges(POLY_A, POLY_B, EDGE_QUERY.halfEdgeA, EDGE_QUERY.halfEdgeB);
@@ -122,12 +163,20 @@ namespace SHADE
       manifold.contacts[numContacts++] = contact;
       manifold.numContacts = numContacts;
 
+      // Cache the info
+      cachedInfo.type       = SHSATInfo::Type::EDGE;
+      cachedInfo.info.edgeA = EDGE_QUERY.halfEdgeA;
+      cachedInfo.info.edgeB = EDGE_QUERY.halfEdgeB;
+
+      manifold.cachedSATInfo = cachedInfo;
+
       return true;
     }
 
     const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
     const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
 
+    manifold.cachedSATInfo = cachedInfo;
     return findFaceContacts(manifold, *incidentPoly, INCIDENT_FACE_IDX, *referencePoly, minFaceQuery.closestFace, flipNormal);
   }
 
@@ -268,6 +317,7 @@ namespace SHADE
      * (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);
@@ -291,6 +341,8 @@ namespace SHADE
      * c =  C' /dot U
      *
      * U is either VA or VB
+     *
+     * TODO: Check if segments degenerate into a point
      */
 
     const SHVec3 C = TAIL_B - TAIL_A;
@@ -310,6 +362,8 @@ namespace SHADE
     const float A2_OVER_A1  = VB_DOT_VA == 0.0f ? 0.0f : VB_DOT_VB / VB_DOT_VA;
     const float NUMERATOR   = C_DOT_VA * A2_OVER_A1 - C_DOT_VB;
     const float DENOMINATOR = AV_DOT_VB - AV_DOT_VA * A2_OVER_A1;
+
+    // We clamp it because the factor cannot be beyond [0,1] as it would be beyond the segment if it was so.
     const float R           = std::clamp(NUMERATOR / DENOMINATOR, 0.0f, 1.0f);
 
     // Just take a point from A since it's A -> B
@@ -687,5 +741,82 @@ namespace SHADE
     return indicesToKeep;
   }
 
+  bool SHCollision::cachedConvexVSConvex(SHManifold& manifold, const SHSATInfo& cachedInfo, const SHConvexPolyhedron& A, const SHConvexPolyhedron& B) noexcept
+  {
+    if (cachedInfo.type == SHSATInfo::Type::FACE)
+    {
+      SHASSERT(cachedInfo.info.refPoly != SHSATInfo::ShapeID::INVALID, "Attempted to perform cached SAT with an invalid cached collision!")
+
+      // Assume the reference poly was A
+      const SHConvexPolyhedron* incPoly = &B;
+      const SHConvexPolyhedron* refPoly = &A;
+      bool flip = false;
+
+      // Swap if it was B
+      if (cachedInfo.info.refPoly == SHSATInfo::ShapeID::B)
+      {
+        refPoly = &B;
+        incPoly = &A;
+        flip    = true;
+      }
+
+      const SHHalfEdgeStructure::Face&  REF_FACE    = refPoly->GetFace(cachedInfo.info.refFace);
+      const SHVec3                      REF_NORMAL  = refPoly->GetNormal(cachedInfo.info.refFace);
+
+      const SHVec3  SUPPORT_POINT = incPoly->FindSupportPoint(-REF_NORMAL);
+
+      const SHPlane REF_FACE_PLANE { refPoly->GetVertex(REF_FACE.vertexIndices[0].index), REF_NORMAL };
+      const float   DISTANCE = REF_FACE_PLANE.SignedDistance(SUPPORT_POINT);
+
+      if (DISTANCE > 0.0f)
+        return false;
+
+      // Find the incident face
+      const int32_t INCIDENT_FACE_INDEX = findIncidentFace(*incPoly, REF_NORMAL);
+
+      return findFaceContacts(manifold, *incPoly, INCIDENT_FACE_INDEX, *refPoly, cachedInfo.info.refFace, flip);
+    }
+
+    if (cachedInfo.type == SHSATInfo::Type::EDGE)
+    {
+      const float DISTANCE = distanceBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
+      if (DISTANCE > 0.0f)
+        return false;
+
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = A.GetHalfEdge(cachedInfo.info.edgeA);
+      const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = B.GetHalfEdge(cachedInfo.info.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);
+
+      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
+      uint32_t numContacts = 0;
+
+      SHContact contact;
+      // Take feature pair key from previous manifold
+      contact.featurePair.key = manifold.contacts[0].featurePair.key;
+      contact.position        = findClosestPointBetweenEdges(A, B, cachedInfo.info.edgeA, cachedInfo.info.edgeB);
+      contact.penetration     = -DISTANCE;
+
+      manifold.contacts[numContacts++] = contact;
+      manifold.numContacts = numContacts;
+
+      return true;
+    }
+
+    // Should never reach this.
+    return false;
+  }
+
 
 } // namespace SHADE

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

@@ -0,0 +1,87 @@
+/****************************************************************************************
+ * \file      SHSATInfo.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Interface for storing information of collision detection between two
+ *            convex shapes using SAT.
+ *
+ * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
+ *            disclosure of this file or its contents without the prior written consent
+ *            of DigiPen Institute of Technology is prohibited.
+****************************************************************************************/
+
+#pragma once
+
+#include <numeric>    // int32_t
+
+namespace SHADE
+{
+  /**
+   * @brief
+   * Primarily used to cached collision information between two convex polyhedrons for
+   * temporal coherence.
+  */
+  struct SHSATInfo
+  {
+  public:
+    /*---------------------------------------------------------------------------------*/
+    /* Type Definitions                                                                */
+    /*---------------------------------------------------------------------------------*/
+
+    enum class ShapeID : int32_t
+    {
+      A       = 0
+    , B       = 1
+
+    , INVALID = -1
+    };
+
+    enum class Type
+    {
+      EDGE
+    , FACE
+
+    , INVALID = -1
+    };
+
+    union ContactInfo
+    {
+      struct // FaceContact
+      {
+        ShapeID refPoly;
+        int32_t refFace;
+      };
+
+      struct // Edge Contact
+      {
+        int32_t edgeA;
+        int32_t edgeB;
+      };
+    };
+
+    /*---------------------------------------------------------------------------------*/
+    /* Data Members                                                                    */
+    /*---------------------------------------------------------------------------------*/
+
+    Type        type;
+    ContactInfo info;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Constructors & Destructor                                                       */
+    /*---------------------------------------------------------------------------------*/
+
+    SHSATInfo   ()                      noexcept;
+    SHSATInfo   (const SHSATInfo& rhs)  noexcept;
+    SHSATInfo   (SHSATInfo&& rhs)       noexcept;
+    ~SHSATInfo  () noexcept = default;
+
+    /*---------------------------------------------------------------------------------*/
+    /* Operator Overloads                                                              */
+    /*---------------------------------------------------------------------------------*/
+
+    SHSATInfo& operator=  (const SHSATInfo& rhs)  noexcept;
+    SHSATInfo& operator=  (SHSATInfo&& rhs)       noexcept;
+  };
+  
+} // namespace SHADE
+
+#include "SHSATInfo.hpp"

SHADE_Engine/src/Physics/Collision/Narrowphase/SHSATInfo.hpp

@@ -0,0 +1,71 @@
+/****************************************************************************************
+ * \file      SHSATInfo.hpp
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Implementation of inlined methods for cached SAT Info.
+ *
+ * \copyright Copyright (C) 2022 DigiPen Institute of Technology. Reproduction or
+ *            disclosure of this file or its contents without the prior written consent
+ *            of DigiPen Institute of Technology is prohibited.
+****************************************************************************************/
+
+#pragma once
+
+// Primary Header
+#include "SHSATInfo.h"
+
+namespace SHADE
+{
+  /*-----------------------------------------------------------------------------------*/
+  /* Constructors & Destructor                                                         */
+  /*-----------------------------------------------------------------------------------*/
+
+  inline SHSATInfo::SHSATInfo() noexcept
+  : type  { Type::INVALID }
+  , info  {}
+  {
+    info.refPoly = ShapeID::INVALID;
+    info.refFace = -1;
+  }
+
+  inline SHSATInfo::SHSATInfo(const SHSATInfo& rhs) noexcept
+  : type  { rhs.type }
+  , info  {}
+  {
+    info.refPoly = rhs.info.refPoly;
+    info.refFace = rhs.info.refFace;
+    
+  }
+
+  inline SHSATInfo::SHSATInfo(SHSATInfo&& rhs) noexcept
+  : type  { rhs.type }
+  , info  {}
+  {
+    info.refPoly = rhs.info.refPoly;
+    info.refFace = rhs.info.refFace;
+  }
+
+  /*---------------------------------------------------------------------------------- */
+  /* Operator Overloads                                                                */
+  /*---------------------------------------------------------------------------------- */
+
+  inline SHSATInfo& SHSATInfo::operator=(const SHSATInfo& rhs) noexcept
+  {
+    if (this == &rhs)
+      return *this;
+
+    type          = rhs.type;
+    info.refPoly  = rhs.info.refPoly;
+    info.refFace  = rhs.info.refFace;
+
+    return *this;
+  }
+
+  inline SHSATInfo& SHSATInfo::operator=(SHSATInfo&& rhs) noexcept
+  {
+    type          = rhs.type;
+    info.refPoly  = rhs.info.refPoly;
+    info.refFace  = rhs.info.refFace;
+
+    return *this;
+  }
+}