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

@@ -28,13 +28,14 @@ namespace SHADE
   }
 
   inline SHManifold::SHManifold(const SHManifold& rhs) noexcept
-  : shapeA      { rhs.shapeA }
+  : shapeA        { rhs.shapeA }
-  , shapeB      { rhs.shapeB }
+  , shapeB        { rhs.shapeB }
-  , bodyA       { rhs.bodyA }
+  , bodyA         { rhs.bodyA }
-  , bodyB       { rhs.bodyB }
+  , bodyB         { rhs.bodyB }
-  , numContacts { rhs.numContacts }
+  , numContacts   { rhs.numContacts }
-  , state       { rhs.state }
+  , state         { rhs.state }
-  , normal      { rhs.normal }
+  , cachedSATInfo { rhs.cachedSATInfo }
+  , normal        { rhs.normal }
   {
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
     memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
@@ -44,13 +45,14 @@ namespace SHADE
   }
 
   inline SHManifold::SHManifold(SHManifold&& rhs) noexcept
-  : shapeA      { rhs.shapeA }
+  : shapeA        { rhs.shapeA }
-  , shapeB      { rhs.shapeB }
+  , shapeB        { rhs.shapeB }
-  , bodyA       { rhs.bodyA }
+  , bodyA         { rhs.bodyA }
-  , bodyB       { rhs.bodyB }
+  , bodyB         { rhs.bodyB }
-  , numContacts { rhs.numContacts }
+  , numContacts   { rhs.numContacts }
-  , state       { rhs.state }
+  , state         { rhs.state }
-  , normal      { rhs.normal }
+  , cachedSATInfo { rhs.cachedSATInfo }
+  , normal        { rhs.normal }
   {
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
     memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
@@ -68,13 +70,14 @@ namespace SHADE
     if (this == &rhs)
       return *this;
 
-    shapeA      = rhs.shapeA;
+    shapeA        = rhs.shapeA;
-    shapeB      = rhs.shapeB;
+    shapeB        = rhs.shapeB;
-    bodyA       = rhs.bodyA;
+    bodyA         = rhs.bodyA;
-    bodyB       = rhs.bodyB;
+    bodyB         = rhs.bodyB;
-    numContacts = rhs.numContacts;
+    numContacts   = rhs.numContacts;
-    state       = rhs.state;
+    state         = rhs.state;
-    normal      = rhs.normal;
+    cachedSATInfo = rhs.cachedSATInfo;
+    normal        = rhs.normal;
 
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
     memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_CONTACTS);
@@ -87,13 +90,14 @@ namespace SHADE
 
   inline SHManifold& SHManifold::operator=(SHManifold&& rhs) noexcept
   {
-    shapeA      = rhs.shapeA;
+    shapeA        = rhs.shapeA;
-    shapeB      = rhs.shapeB;
+    shapeB        = rhs.shapeB;
-    bodyA       = rhs.bodyA;
+    bodyA         = rhs.bodyA;
-    bodyB       = rhs.bodyB;
+    bodyB         = rhs.bodyB;
-    numContacts = rhs.numContacts;
+    numContacts   = rhs.numContacts;
-    state       = rhs.state;
+    state         = rhs.state;
-    normal      = rhs.normal;
+    cachedSATInfo = rhs.cachedSATInfo;
+    normal        = rhs.normal;
 
     static constexpr size_t SIZE_OF_CONTACTS = sizeof(SHContact) * static_cast<size_t>(MAX_NUM_CONTACTS);
     memcpy_s(contacts, SIZE_OF_CONTACTS, rhs.contacts, SIZE_OF_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,15 +341,17 @@ 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;
 
-    const float VB_DOT_VA = SHVec3::Dot(VB, VA);  // a1
+    const float VB_DOT_VA = SHVec3::Dot(VB, VA);  // a1   
-    const float VB_DOT_VB = SHVec3::Dot(VB, VB);  // a2
+    const float VB_DOT_VB = SHVec3::Dot(VB, VB);  // a2   
-    const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1
+    const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1   
-    const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2
+    const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2   
-    const float C_DOT_VA  = SHVec3::Dot(C, VA);   // c1
+    const float C_DOT_VA  = SHVec3::Dot(C, VA);   // c1   
     const float C_DOT_VB  = SHVec3::Dot(C, VB);   // c2
 
     /*
@@ -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;
+  }
+}