R for retard

Assets/Scenes/PhysicsSandbox.shade

@@ -280,7 +280,7 @@
   NumberOfChildren: 0
   Components:
     Transform Component:
-      Translate: {x: 0.524352431, y: 11.5, z: 0.0463808179}
+      Translate: {x: 0.524352431, y: 13.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
@@ -291,7 +291,7 @@
       Drag: 0.00999999978
       Angular Drag: 0.00999999978
       Use Gravity: true
-      Gravity Scale: 1
+      Gravity Scale: 0.5
       Interpolate: true
       Sleeping Enabled: true
       Freeze Position X: false

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

@@ -108,6 +108,7 @@ namespace SHADE
     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: 
@@ -115,7 +116,7 @@ namespace SHADE
      * 
      * 
      * 
-     * static int32_t   findIncidentFace    (SHConvexPolyhedron& poly, const SHVec3& normal) noexcept;
+     * 
      * static uint32_t  clip                [sutherland-hodgemann clipping]
      *
      * ! References

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

@@ -17,6 +17,7 @@
 #include "Math/SHMathHelpers.h"
 #include "Math/Geometry/SHPlane.h"
 #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
+#include "Physics/SHPhysicsConstants.h"
 #include "Tools/Utilities/SHUtilities.h"
 
 namespace SHADE
@@ -47,24 +48,7 @@ namespace SHADE
 
   bool SHCollision::ConvexVsConvex(SHManifold& manifold, const SHCollisionShape& A, const SHCollisionShape& B) noexcept
   {
-    /*
+    static constexpr float TOLERANCE  = 0.1f + SHPHYSICS_LINEAR_SLOP;
-     * TODO:
-     *
-     * 1. Query face directions of a to b. Exit early if separation found.
-     * 2. Query face directions of b to a. Exit early if separation found.
-     * 3. Query edge directions of a & b. Exit early if separation found.
-     *
-     * (*)!! Apply weight to improve frame coherence of normal directions. DONT FORGET FLIP FLOP!
-     * 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.
-     * 
-     */
 
     const SHConvexPolyhedron& POLY_A = dynamic_cast<const SHConvexPolyhedron&>(A);
     const SHConvexPolyhedron& POLY_B = dynamic_cast<const SHConvexPolyhedron&>(B);
@@ -81,14 +65,33 @@ namespace SHADE
     if (EDGE_QUERY.bestDistance > 0.0f)
       return false;
 
-    const FaceQuery& BEST_FACE_QUERY = FACE_QUERY_A.bestDistance > FACE_QUERY_B.bestDistance ? FACE_QUERY_A : FACE_QUERY_B;
+    // 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.
+    bool                      flipNormal    = false;
+    const SHConvexPolyhedron* referencePoly = nullptr;
+    const SHConvexPolyhedron* incidentPoly  = nullptr;
+
+    FaceQuery minFaceQuery;
+    if (FACE_QUERY_A.bestDistance + TOLERANCE > FACE_QUERY_B.bestDistance)
+    {
+      minFaceQuery  = FACE_QUERY_A;
+      referencePoly = &POLY_A;
+      incidentPoly  = &POLY_B;
+    }
+    else
+    {
+      minFaceQuery  = FACE_QUERY_B;
+      referencePoly = &POLY_B;
+      incidentPoly  = &POLY_A;
+      flipNormal    = true;
+    }
 
     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.
+    if (EDGE_QUERY.bestDistance > minFaceQuery.bestDistance + TOLERANCE)
-    if (EDGE_QUERY.bestDistance > BEST_FACE_QUERY.bestDistance)
     {
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_A = POLY_A.GetHalfEdge(EDGE_QUERY.halfEdgeA);
       const SHHalfEdgeStructure::HalfEdge& HALF_EDGE_B = POLY_B.GetHalfEdge(EDGE_QUERY.halfEdgeB);
@@ -122,7 +125,24 @@ namespace SHADE
       return true;
     }
 
-    // Use a bias to favour a normal in the direction of A -> B
+    const SHVec3 REFERENCE_NORMAL = referencePoly->GetNormal(minFaceQuery.closestFace);
+    const int32_t INCIDENT_FACE_IDX = findIncidentFace(*incidentPoly, REFERENCE_NORMAL);
+
+
+    /*
+     * TODO:
+     *
+     * !! 
+     * 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.
+     * 
+     */
 
 
 
@@ -289,25 +309,48 @@ namespace SHADE
 
     const SHVec3 C = TAIL_B - TAIL_A;
 
-    const float VB_DOT_VA = SHVec3::Dot(VB, VA);
+    const float VB_DOT_VA = SHVec3::Dot(VB, VA);  // a1
-    const float VB_DOT_VB = SHVec3::Dot(VB, VB);
+    const float VB_DOT_VB = SHVec3::Dot(VB, VB);  // a2
-    const float AV_DOT_VA = SHVec3::Dot(-VA, VA);
+    const float AV_DOT_VA = SHVec3::Dot(-VA, VA); // b1
-    const float AV_DOT_VB = SHVec3::Dot(-VA, VB);
+    const float AV_DOT_VB = SHVec3::Dot(-VA, VB); // b2
-    const float C_DOT_VA  = SHVec3::Dot(C, VA);
+    const float C_DOT_VA  = SHVec3::Dot(C, VA);   // c1
-    const float C_DOT_VB  = SHVec3::Dot(C, VB);
+    const float C_DOT_VB  = SHVec3::Dot(C, VB);   // c2
 
     /*
-     * R = -c2 / ( b2 - (a2 * (c1 + b1)) / a1 )
-     * S = (b1 / a1) * (c2 / ( b2 - (a2 * b1) / a1 )) - (c1 / a1)
-     *
      * We only need to solve for R
+     * R = (c1a2 / a1 - c2) / ( b2 - a2b1/a1 )
      */
 
-    const float R = -C_DOT_VB / (AV_DOT_VB - (VB_DOT_VB * (C_DOT_VA + AV_DOT_VA)) / VB_DOT_VA);
+    const float A2_OVER_A1  = 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;
+    const float R = NUMERATOR / DENOMINATOR;
 
     // Just take a point from A since it's A -> B
     return TAIL_A + R * VA;
   }
 
+  int32_t SHCollision::findIncidentFace(const SHConvexPolyhedron& poly, const SHVec3& normal) noexcept
+  {
+    // Get the most anti-parallel face to the normal
+    int32_t bestFace        = 0;
+    float   bestProjection  = std::numeric_limits<float>::max();
+
+    const int32_t NUM_FACES = poly.GetFaceCount();
+    for (const int32_t i : std::views::iota(0, NUM_FACES))
+    {
+      const SHVec3  INC_NORMAL = poly.GetNormal(i);
+      const float   PROJECTION = SHVec3::Dot(INC_NORMAL, normal);
+
+      if (PROJECTION < bestProjection)
+      {
+        bestProjection  = PROJECTION;
+        bestFace        = i;
+      }
+    }
+
+    return bestFace;
+  }
+
 
 } // namespace SHADE

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

@@ -19,6 +19,7 @@
 #include "Math/SHMathHelpers.h"
 #include "Physics/Collision/Shapes/SHSphere.h"
 #include "Physics/Collision/Shapes/SHConvexPolyhedron.h"
+#include "Physics/SHPhysicsConstants.h"
 
 namespace SHADE
 {

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp

@@ -15,6 +15,7 @@
 
 // Project Headers
 #include "Math/SHMathHelpers.h"
+#include "Physics/SHPhysicsConstants.h"
 
 namespace SHADE
 {
@@ -190,8 +191,8 @@ namespace SHADE
         const SHVec3  RV_B = vB + SHVec3::Cross(wB, contact.rB);
         const float   RV_N = SHVec3::Dot(RV_B - RV_A, constraint.normal);
 
-        const float ERROR_BIAS = BAUMGARTE_FACTOR * INV_DT * std::min(0.0f, -contact.penetration + PENETRATION_SLOP);
+        const float ERROR_BIAS = SHPHYSICS_BAUMGARTE * INV_DT * std::min(0.0f, -contact.penetration + SHPHYSICS_LINEAR_SLOP);
-        const float RESTITUTION_BIAS = -constraint.restitution * RV_N;
+        const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f;
 
         contact.bias = ERROR_BIAS + RESTITUTION_BIAS;
 

SHADE_Engine/src/Physics/Dynamics/SHContactSolver.h

@@ -78,9 +78,6 @@ namespace SHADE
     /* Data Members                                                                    */
     /*---------------------------------------------------------------------------------*/
 
-    static constexpr float BAUMGARTE_FACTOR = 0.2f;
-    static constexpr float PENETRATION_SLOP = 0.05f;
-
     VelocityStates      velocityStates;
     ContactConstraints  contactConstraints;
 

SHADE_Engine/src/Physics/SHPhysicsConstants.h

@@ -0,0 +1,44 @@
+/****************************************************************************************
+ * \file      SHPhysicsConstants.h
+ * \author    Diren D Bharwani, diren.dbharwani, 390002520
+ * \brief     Definitions for constants used in physics simulations
+ *
+ * \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
+
+// Project Includes
+#include "Math/SHMathHelpers.h"
+
+namespace SHADE
+{
+  /**
+   * @brief
+   * The number of simulations length for every real world unit meter. <br/>
+   * Modify this to change the global scale of the simulation.
+  */
+  static constexpr float SHPHYSICS_LENGTHS_PER_UNIT_METER   = 1.0f;
+
+  /**
+   * @brief
+   * Linear Collision & Constraint tolerance.
+  */
+  static constexpr float SHPHYSICS_LINEAR_SLOP              = 0.005f * SHPHYSICS_LENGTHS_PER_UNIT_METER;
+
+  /**
+   * @brief
+   * Velocity threshold for restitution to be applied.
+  */
+  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;
+
+}