From 9b2c5a1804a03f79e75cececade7bbd72c0919ef Mon Sep 17 00:00:00 2001
From: Diren D Bharwani <diren.bharwani@gmail.com>
Date: Sat, 4 Mar 2023 19:50:53 +0800
Subject: [PATCH] Added math helpers for checking if a number is infinity or
 invalid

---
 SHADE_Engine/src/Math/SHMathHelpers.h           |  6 ++++++
 SHADE_Engine/src/Math/SHMathHelpers.hpp         | 17 +++++++++++++++++
 .../src/Physics/Dynamics/SHContactSolver.cpp    | 10 +---------
 3 files changed, 24 insertions(+), 9 deletions(-)

diff --git a/SHADE_Engine/src/Math/SHMathHelpers.h b/SHADE_Engine/src/Math/SHMathHelpers.h
index 1d65eb91..01aa5874 100644
--- a/SHADE_Engine/src/Math/SHMathHelpers.h
+++ b/SHADE_Engine/src/Math/SHMathHelpers.h
@@ -105,6 +105,12 @@ namespace SHADE
     template <IsFloatingPoint T = float>
     [[nodiscard]] static bool CompareFloat (T lhs, T rhs, T absTolerance = EPSILON, T relTolerance = EPSILON);
 
+    template <IsArithmetic T>
+    [[nodiscard]] static bool IsInfinity  (T value);
+
+    template <IsArithmetic T>
+    [[nodiscard]] static bool IsNaN       (T value);
+
   private:
     /*---------------------------------------------------------------------------------*/
     /* Static Data Members                                                             */
diff --git a/SHADE_Engine/src/Math/SHMathHelpers.hpp b/SHADE_Engine/src/Math/SHMathHelpers.hpp
index 554fa317..9a483566 100644
--- a/SHADE_Engine/src/Math/SHMathHelpers.hpp
+++ b/SHADE_Engine/src/Math/SHMathHelpers.hpp
@@ -126,4 +126,21 @@ namespace SHADE
     return std::fabs(lhs - rhs) <= Max(absTolerance, RTOL);
   }
 
+  template <IsArithmetic T>
+  bool SHMath::IsInfinity(T value)
+  {
+    const float MAX_VALUE = std::numeric_limits<T>::max();
+    const float MIN_VALUE = std::numeric_limits<T>::min();
+
+    return !(MIN_VALUE <= value && value <= MAX_VALUE);
+  }
+
+  template <IsArithmetic T>
+  bool SHMath::IsNaN(T value)
+  {
+    return value != value;
+  }
+
+
+
 } // namespace SHADE
\ No newline at end of file
diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
index 87276c82..b25f38b0 100644
--- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
+++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp
@@ -18,14 +18,6 @@
 #include "Physics/SHPhysicsConstants.h"
 #include "Physics/Collision/Narrowphase/SHCollisionUtils.h"
 
-bool IsInfinity(float value)
-{
-  const float MAX_VAL = std::numeric_limits<float>::max();
-  const float MIN_VAL = -MAX_VAL;
- 
-  return !(MIN_VAL <= value && value <= MAX_VAL);
-} 
-
 namespace SHADE
 {
   /*-----------------------------------------------------------------------------------*/
@@ -227,7 +219,7 @@ namespace SHADE
           errorBias = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP);
 
         // TODO: This should never occur. Move this check into the ConvexVSConvex to find the error.
-        if (IsInfinity(errorBias))
+        if (SHMath::IsInfinity(errorBias))
           errorBias = 0.0f;
 
         // Warm starting