Augment raymath.h with useful functions (#2428)

* Augment raymath.h with useful functions

* Rename Vector2ClampMagnitude and Vector3ClampMagnitude to Vector2ClampValue and Vector3ClampValue

* Remove Vector3{Up,Down,Left,Right,Forward,Backward}

1 files changed, 171 insertions, 0 deletions

diff --git a/src/raymath.h b/src/raymath.h
index 156e6638..4aa56b65 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -77,6 +77,10 @@
     #define PI 3.14159265358979323846f
 #endif
 
+#ifndef EPSILON
+    #define EPSILON 0.000001f
+#endif
+
 #ifndef DEG2RAD
     #define DEG2RAD (PI/180.0f)
 #endif
@@ -155,6 +159,7 @@ typedef struct float16 {
 } float16;
 
 #include <math.h>       // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), fminf(), fmaxf(), fabs()
+#include <stdbool.h>    // Required for: bool, false, true
 
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Utils math
@@ -194,6 +199,14 @@ RMAPI float Remap(float value, float inputStart, float inputEnd, float outputSta
     return result;
 }
 
+// Check whether two given floats are almost equal
+RMAPI bool FloatEquals(float x, float y)
+{
+    bool result = (fabsf(x - y)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))));
+
+    return result;
+}
+
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Vector2 math
 //----------------------------------------------------------------------------------
@@ -414,6 +427,63 @@ RMAPI Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance)
     return result;
 }
 
+// Invert the given vector
+RMAPI Vector2 Vector2Invert(Vector2 v)
+{
+    Vector2 result = { 1.0f / v.x, 1.0f / v.y };
+
+    return result;
+}
+
+// Clamp the components of the vector between
+// min and max values specified by the given vectors
+RMAPI Vector2 Vector2Clamp(Vector2 v, Vector2 min, Vector2 max)
+{
+    Vector2 result = { 0 };
+
+    result.x = fminf(max.x, fmaxf(min.x, v.x));
+    result.y = fminf(max.y, fmaxf(min.y, v.y));
+
+    return result;
+}
+
+// Clamp the magnitude of the vector between two
+// given min and max values
+RMAPI Vector2 Vector2ClampValue(Vector2 v, float minMag, float maxMag)
+{
+    Vector2 result = { 0 };
+
+    float length = (v.x * v.x) + (v.y * v.y);
+    if (length > 0.0f)
+    {
+        length = sqrtf(length);
+
+        if (length < minMag)
+        {
+            float scale = minMag / length;
+            result.x = v.x * scale;
+            result.y = v.y * scale;
+        }
+        else if (length > maxMag)
+        {
+            float scale = maxMag / length;
+            result.x = v.x * scale;
+            result.y = v.y * scale;
+        }
+    }
+
+    return result;
+}
+
+// Check whether two given vectors are almost equal
+RMAPI bool Vector2Equals(Vector2 p, Vector2 q)
+{
+    bool result = ((fabsf(p.x - q.x)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) &&
+                  ((fabsf(p.y - q.y)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y)))));
+
+    return result;
+}
+
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Vector3 math
 //----------------------------------------------------------------------------------
@@ -851,6 +921,96 @@ RMAPI float3 Vector3ToFloatV(Vector3 v)
     return buffer;
 }
 
+// Invert the given vector
+RMAPI Vector3 Vector3Invert(Vector3 v)
+{
+    Vector3 result = { 1.0f / v.x, 1.0f / v.y, 1.0f / v.z };
+
+    return result;
+}
+
+// Clamp the components of the vector between
+// min and max values specified by the given vectors
+RMAPI Vector3 Vector3Clamp(Vector3 v, Vector3 min, Vector3 max)
+{
+    Vector3 result = { 0 };
+
+    result.x = fminf(max.x, fmaxf(min.x, v.x));
+    result.y = fminf(max.y, fmaxf(min.y, v.y));
+    result.z = fminf(max.z, fmaxf(min.z, v.z));
+
+    return result;
+}
+
+// Clamp the magnitude of the vector between two
+// given min and max values
+RMAPI Vector3 Vector3ClampValue(Vector3 v, float minMag, float maxMag)
+{
+    Vector3 result = { 0 };
+
+    float length = (v.x * v.x) + (v.y * v.y) + (v.z * v.z);
+    if (length > 0.0f)
+    {
+        length = sqrtf(length);
+
+        if (length < minMag)
+        {
+            float scale = minMag / length;
+            result.x = v.x * scale;
+            result.y = v.y * scale;
+            result.z = v.z * scale;
+        }
+        else if (length > maxMag)
+        {
+            float scale = maxMag / length;
+            result.x = v.x * scale;
+            result.y = v.y * scale;
+            result.z = v.z * scale;
+        }
+    }
+
+    return result;
+}
+
+// Check whether two given vectors are almost equal
+RMAPI bool Vector3Equals(Vector3 p, Vector3 q)
+{
+    bool result = ((fabsf(p.x - q.x)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) &&
+                  ((fabsf(p.y - q.y)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) &&
+                  ((fabsf(p.z - q.z)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z)))));
+
+    return result;
+}
+
+// Compute the direction of a refracted ray where v specifies the
+// normalized direction of the incoming ray, n specifies the
+// normalized normal vector of the interface of two optical media,
+// and r specifies the ratio of the refractive index of the medium
+// from where the ray comes to the refractive index of the medium
+// on the other side of the surface
+RMAPI Vector3 Vector3Refract(Vector3 v, Vector3 n, float r)
+{
+    Vector3 result = { 0 };
+
+    float dot = v.x * n.x + v.y * n.y + v.z * n.z;
+
+    float d = 1.0f - r * r * (1.0f - dot * dot);
+    if (d < 0.0f)
+    {
+        // Total internal reflection
+        return result;
+    }
+    else
+    {
+        d = sqrtf(d);
+        v.x = r * v.x - (r * dot + d) * n.x;
+        v.y = r * v.y - (r * dot + d) * n.y;
+        v.z = r * v.z - (r * dot + d) * n.z;
+
+        return result;
+    }
+}
+
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Matrix math
 //----------------------------------------------------------------------------------
@@ -1844,4 +2004,15 @@ RMAPI Quaternion QuaternionTransform(Quaternion q, Matrix mat)
     return result;
 }
 
+// Check whether two given quaternions are almost equal
+RMAPI bool QuaternionEquals(Quaternion p, Quaternion q)
+{
+    bool result = ((fabsf(p.x - q.x)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) &&
+                  ((fabsf(p.y - q.y)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) &&
+                  ((fabsf(p.z - q.z)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z))))) &&
+                  ((fabsf(p.w - q.w)) <= (EPSILON * fmaxf(1.0f, fmaxf(fabsf(p.w), fabsf(q.w)))));
+
+    return result;
+}
+
 #endif  // RAYMATH_H