Update C sources

raylib/raymath.h

@@ -18,14 +18,14 @@
 *     - Functions are always self-contained, no function use another raymath function inside,
 *       required code is directly re-implemented inside
 *     - Functions input parameters are always received by value (2 unavoidable exceptions)
-*     - Functions use always a "result" anmed variable for return
+*     - Functions use always a "result" variable for return
 *     - Functions are always defined inline
 *     - Angles are always in radians (DEG2RAD/RAD2DEG macros provided for convenience)
 *
 *
 *   LICENSE: zlib/libpng
 *
-*   Copyright (c) 2015-2021 Ramon Santamaria (@raysan5)
+*   Copyright (c) 2015-2022 Ramon Santamaria (@raysan5)
 *
 *   This software is provided "as-is", without any express or implied warranty. In no event
 *   will the authors be held liable for any damages arising from the use of this software.
@@ -77,6 +77,10 @@
     #define PI 3.14159265358979323846f
 #endif
 
+#ifndef EPSILON
+    #define EPSILON 0.000001f
+#endif
+
 #ifndef DEG2RAD
     #define DEG2RAD (PI/180.0f)
 #endif
@@ -154,7 +158,7 @@ typedef struct float16 {
     float v[16];
 } float16;
 
-#include <math.h>       // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), fminf(), fmaxf(), fabs()
+#include <math.h>       // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), floor(), fminf(), fmaxf(), fabs()
 
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Utils math
@@ -189,7 +193,23 @@ RMAPI float Normalize(float value, float start, float end)
 // Remap input value within input range to output range
 RMAPI float Remap(float value, float inputStart, float inputEnd, float outputStart, float outputEnd)
 {
-    float result =(value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart;
+    float result = (value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart;
+
+    return result;
+}
+
+// Wrap input value from min to max
+RMAPI float Wrap(float value, float min, float max)
+{
+    float result = value - (max - min)*floorf((value - min)/(max - min));
+
+    return result;
+}
+
+// Check whether two given floats are almost equal
+RMAPI int FloatEquals(float x, float y)
+{
+    int result = (fabsf(x - y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y))));
 
     return result;
 }
@@ -278,12 +298,18 @@ RMAPI float Vector2Distance(Vector2 v1, Vector2 v2)
     return result;
 }
 
-// Calculate angle from two vectors in X-axis
+// Calculate square distance between two vectors
+RMAPI float Vector2DistanceSqr(Vector2 v1, Vector2 v2)
+{
+    float result = ((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y));
+
+    return result;
+}
+
+// Calculate angle from two vectors
 RMAPI float Vector2Angle(Vector2 v1, Vector2 v2)
 {
-    float result = atan2f(v2.y - v1.y, v2.x - v1.x)*(180.0f/PI);
-
-    if (result < 0) result += 360.0f;
+    float result = atan2f(v2.y, v2.x) - atan2f(v1.y, v1.x);
 
     return result;
 }
@@ -328,13 +354,29 @@ RMAPI Vector2 Vector2Normalize(Vector2 v)
 
     if (length > 0)
     {
-        result.x = v.x*1.0f/length;
-        result.y = v.y*1.0f/length;
+        float ilength = 1.0f/length;
+        result.x = v.x*ilength;
+        result.y = v.y*ilength;
     }
 
     return result;
 }
 
+// Transforms a Vector2 by a given Matrix
+RMAPI Vector2 Vector2Transform(Vector2 v, Matrix mat)
+{
+    Vector2 result = { 0 };
+
+    float x = v.x;
+    float y = v.y;
+    float z = 0;
+
+    result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12;
+    result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13;
+
+    return result;
+}
+
 // Calculate linear interpolation between two vectors
 RMAPI Vector2 Vector2Lerp(Vector2 v1, Vector2 v2, float amount)
 {
@@ -364,8 +406,11 @@ RMAPI Vector2 Vector2Rotate(Vector2 v, float angle)
 {
     Vector2 result = { 0 };
 
-    result.x = v.x*cosf(angle) - v.y*sinf(angle);
-    result.y = v.x*sinf(angle) + v.y*cosf(angle);
+    float cosres = cosf(angle);
+    float sinres = sinf(angle);
+
+    result.x = v.x*cosres - v.y*sinres;
+    result.y = v.x*sinres + v.y*cosres;
 
     return result;
 }
@@ -389,6 +434,62 @@ 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 min and max values
+RMAPI Vector2 Vector2ClampValue(Vector2 v, float min, float max)
+{
+    Vector2 result = v;
+
+    float length = (v.x*v.x) + (v.y*v.y);
+    if (length > 0.0f)
+    {
+        length = sqrtf(length);
+
+        if (length < min)
+        {
+            float scale = min/length;
+            result.x = v.x*scale;
+            result.y = v.y*scale;
+        }
+        else if (length > max)
+        {
+            float scale = max/length;
+            result.x = v.x*scale;
+            result.y = v.y*scale;
+        }
+    }
+
+    return result;
+}
+
+// Check whether two given vectors are almost equal
+RMAPI int Vector2Equals(Vector2 p, Vector2 q)
+{
+    int 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
 //----------------------------------------------------------------------------------
@@ -473,14 +574,14 @@ RMAPI Vector3 Vector3Perpendicular(Vector3 v)
     float min = (float) fabs(v.x);
     Vector3 cardinalAxis = {1.0f, 0.0f, 0.0f};
 
-    if (fabs(v.y) < min)
+    if (fabsf(v.y) < min)
     {
         min = (float) fabs(v.y);
         Vector3 tmp = {0.0f, 1.0f, 0.0f};
         cardinalAxis = tmp;
     }
 
-    if (fabs(v.z) < min)
+    if (fabsf(v.z) < min)
     {
         Vector3 tmp = {0.0f, 0.0f, 1.0f};
         cardinalAxis = tmp;
@@ -531,17 +632,28 @@ RMAPI float Vector3Distance(Vector3 v1, Vector3 v2)
     return result;
 }
 
-// Calculate angle between two vectors in XY and XZ
-RMAPI Vector2 Vector3Angle(Vector3 v1, Vector3 v2)
+// Calculate square distance between two vectors
+RMAPI float Vector3DistanceSqr(Vector3 v1, Vector3 v2)
 {
-    Vector2 result = { 0 };
+    float result = 0.0f;
 
     float dx = v2.x - v1.x;
     float dy = v2.y - v1.y;
     float dz = v2.z - v1.z;
+    result = dx*dx + dy*dy + dz*dz;
 
-    result.x = atan2f(dx, dz);                      // Angle in XZ
-    result.y = atan2f(dy, sqrtf(dx*dx + dz*dz));    // Angle in XY
+    return result;
+}
+
+// Calculate angle between two vectors
+RMAPI float Vector3Angle(Vector3 v1, Vector3 v2)
+{
+    float result = 0.0f;
+
+    Vector3 cross = { v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x };
+    float len = sqrtf(cross.x*cross.x + cross.y*cross.y + cross.z*cross.z);
+    float dot = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
+    result = atan2f(len, dot);
 
     return result;
 }
@@ -641,6 +753,58 @@ RMAPI Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q)
     return result;
 }
 
+// Rotates a vector around an axis
+RMAPI Vector3 Vector3RotateByAxisAngle(Vector3 v, Vector3 axis, float angle)
+{
+    // Using Euler-Rodrigues Formula
+    // Ref.: https://en.wikipedia.org/w/index.php?title=Euler%E2%80%93Rodrigues_formula
+
+    Vector3 result = v;
+
+    // Vector3Normalize(axis);
+    float length = sqrtf(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z);
+    if (length == 0.0f) length = 1.0f;
+    float ilength = 1.0f / length;
+    axis.x *= ilength;
+    axis.y *= ilength;
+    axis.z *= ilength;
+
+    angle /= 2.0f;
+    float a = sinf(angle);
+    float b = axis.x * a;
+    float c = axis.y * a;
+    float d = axis.z * a;
+    a = cosf(angle);
+    Vector3 w = { b, c, d };
+
+    // Vector3CrossProduct(w, v)
+    Vector3 wv = { w.y * v.z - w.z * v.y, w.z * v.x - w.x * v.z, w.x * v.y - w.y * v.x };
+
+    // Vector3CrossProduct(w, wv)
+    Vector3 wwv = { w.y * wv.z - w.z * wv.y, w.z * wv.x - w.x * wv.z, w.x * wv.y - w.y * wv.x };
+
+    // Vector3Scale(wv, 2 * a)
+    a *= 2;
+    wv.x *= a;
+    wv.y *= a;
+    wv.z *= a;
+
+    // Vector3Scale(wwv, 2)
+    wwv.x *= 2;
+    wwv.y *= 2;
+    wwv.z *= 2;
+
+    result.x += wv.x;
+    result.y += wv.y;
+    result.z += wv.z;
+
+    result.x += wwv.x;
+    result.y += wwv.y;
+    result.z += wwv.z;
+
+    return result;
+}
+
 // Calculate linear interpolation between two vectors
 RMAPI Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
 {
@@ -815,6 +979,92 @@ 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 values
+RMAPI Vector3 Vector3ClampValue(Vector3 v, float min, float max)
+{
+    Vector3 result = v;
+
+    float length = (v.x*v.x) + (v.y*v.y) + (v.z*v.z);
+    if (length > 0.0f)
+    {
+        length = sqrtf(length);
+
+        if (length < min)
+        {
+            float scale = min/length;
+            result.x = v.x*scale;
+            result.y = v.y*scale;
+            result.z = v.z*scale;
+        }
+        else if (length > max)
+        {
+            float scale = max/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 int Vector3Equals(Vector3 p, Vector3 q)
+{
+    int 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)
+    {
+        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;
+
+        result = v;
+    }
+
+    return result;
+}
+
 //----------------------------------------------------------------------------------
 // Module Functions Definition - Matrix math
 //----------------------------------------------------------------------------------
@@ -920,45 +1170,6 @@ RMAPI Matrix MatrixInvert(Matrix mat)
     return result;
 }
 
-// Normalize provided matrix
-RMAPI Matrix MatrixNormalize(Matrix mat)
-{
-    Matrix result = { 0 };
-
-    // Cache the matrix values (speed optimization)
-    float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
-    float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
-    float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
-    float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
-
-    // MatrixDeterminant(mat)
-    float det = a30*a21*a12*a03 - a20*a31*a12*a03 - a30*a11*a22*a03 + a10*a31*a22*a03 +
-                a20*a11*a32*a03 - a10*a21*a32*a03 - a30*a21*a02*a13 + a20*a31*a02*a13 +
-                a30*a01*a22*a13 - a00*a31*a22*a13 - a20*a01*a32*a13 + a00*a21*a32*a13 +
-                a30*a11*a02*a23 - a10*a31*a02*a23 - a30*a01*a12*a23 + a00*a31*a12*a23 +
-                a10*a01*a32*a23 - a00*a11*a32*a23 - a20*a11*a02*a33 + a10*a21*a02*a33 +
-                a20*a01*a12*a33 - a00*a21*a12*a33 - a10*a01*a22*a33 + a00*a11*a22*a33;
-
-    result.m0 = mat.m0/det;
-    result.m1 = mat.m1/det;
-    result.m2 = mat.m2/det;
-    result.m3 = mat.m3/det;
-    result.m4 = mat.m4/det;
-    result.m5 = mat.m5/det;
-    result.m6 = mat.m6/det;
-    result.m7 = mat.m7/det;
-    result.m8 = mat.m8/det;
-    result.m9 = mat.m9/det;
-    result.m10 = mat.m10/det;
-    result.m11 = mat.m11/det;
-    result.m12 = mat.m12/det;
-    result.m13 = mat.m13/det;
-    result.m14 = mat.m14/det;
-    result.m15 = mat.m15/det;
-
-    return result;
-}
-
 // Get identity matrix
 RMAPI Matrix MatrixIdentity(void)
 {
@@ -1102,7 +1313,8 @@ RMAPI Matrix MatrixRotate(Vector3 axis, float angle)
     return result;
 }
 
-// Get x-rotation matrix (angle in radians)
+// Get x-rotation matrix
+// NOTE: Angle must be provided in radians
 RMAPI Matrix MatrixRotateX(float angle)
 {
     Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
@@ -1114,14 +1326,15 @@ RMAPI Matrix MatrixRotateX(float angle)
     float sinres = sinf(angle);
 
     result.m5 = cosres;
-    result.m6 = -sinres;
-    result.m9 = sinres;
+    result.m6 = sinres;
+    result.m9 = -sinres;
     result.m10 = cosres;
 
     return result;
 }
 
-// Get y-rotation matrix (angle in radians)
+// Get y-rotation matrix
+// NOTE: Angle must be provided in radians
 RMAPI Matrix MatrixRotateY(float angle)
 {
     Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
@@ -1133,14 +1346,15 @@ RMAPI Matrix MatrixRotateY(float angle)
     float sinres = sinf(angle);
 
     result.m0 = cosres;
-    result.m2 = sinres;
-    result.m8 = -sinres;
+    result.m2 = -sinres;
+    result.m8 = sinres;
     result.m10 = cosres;
 
     return result;
 }
 
-// Get z-rotation matrix (angle in radians)
+// Get z-rotation matrix
+// NOTE: Angle must be provided in radians
 RMAPI Matrix MatrixRotateZ(float angle)
 {
     Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
@@ -1152,74 +1366,76 @@ RMAPI Matrix MatrixRotateZ(float angle)
     float sinres = sinf(angle);
 
     result.m0 = cosres;
-    result.m1 = -sinres;
-    result.m4 = sinres;
+    result.m1 = sinres;
+    result.m4 = -sinres;
     result.m5 = cosres;
 
     return result;
 }
 
 
-// Get xyz-rotation matrix (angles in radians)
-RMAPI Matrix MatrixRotateXYZ(Vector3 ang)
+// Get xyz-rotation matrix
+// NOTE: Angle must be provided in radians
+RMAPI Matrix MatrixRotateXYZ(Vector3 angle)
 {
     Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
                       0.0f, 1.0f, 0.0f, 0.0f,
                       0.0f, 0.0f, 1.0f, 0.0f,
                       0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity()
 
-    float cosz = cosf(-ang.z);
-    float sinz = sinf(-ang.z);
-    float cosy = cosf(-ang.y);
-    float siny = sinf(-ang.y);
-    float cosx = cosf(-ang.x);
-    float sinx = sinf(-ang.x);
+    float cosz = cosf(-angle.z);
+    float sinz = sinf(-angle.z);
+    float cosy = cosf(-angle.y);
+    float siny = sinf(-angle.y);
+    float cosx = cosf(-angle.x);
+    float sinx = sinf(-angle.x);
 
     result.m0 = cosz*cosy;
-    result.m4 = (cosz*siny*sinx) - (sinz*cosx);
-    result.m8 = (cosz*siny*cosx) + (sinz*sinx);
+    result.m1 = (cosz*siny*sinx) - (sinz*cosx);
+    result.m2 = (cosz*siny*cosx) + (sinz*sinx);
 
-    result.m1 = sinz*cosy;
+    result.m4 = sinz*cosy;
     result.m5 = (sinz*siny*sinx) + (cosz*cosx);
-    result.m9 = (sinz*siny*cosx) - (cosz*sinx);
+    result.m6 = (sinz*siny*cosx) - (cosz*sinx);
 
-    result.m2 = -siny;
-    result.m6 = cosy*sinx;
+    result.m8 = -siny;
+    result.m9 = cosy*sinx;
     result.m10= cosy*cosx;
 
     return result;
 }
 
-// Get zyx-rotation matrix (angles in radians)
-RMAPI Matrix MatrixRotateZYX(Vector3 ang)
+// Get zyx-rotation matrix
+// NOTE: Angle must be provided in radians
+RMAPI Matrix MatrixRotateZYX(Vector3 angle)
 {
     Matrix result = { 0 };
 
-    float cz = cosf(ang.z);
-    float sz = sinf(ang.z);
-    float cy = cosf(ang.y);
-    float sy = sinf(ang.y);
-    float cx = cosf(ang.x);
-    float sx = sinf(ang.x);
+    float cz = cosf(angle.z);
+    float sz = sinf(angle.z);
+    float cy = cosf(angle.y);
+    float sy = sinf(angle.y);
+    float cx = cosf(angle.x);
+    float sx = sinf(angle.x);
 
     result.m0 = cz*cy;
-    result.m1 = cz*sy*sx - cx*sz;
-    result.m2 = sz*sx + cz*cx*sy;
-    result.m3 = 0;
-
-    result.m4 = cy*sz;
-    result.m5 = cz*cx + sz*sy*sx;
-    result.m6 = cx*sz*sy - cz*sx;
-    result.m7 = 0;
-
-    result.m8 = -sy;
-    result.m9 = cy*sx;
-    result.m10 = cy*cx;
-    result.m11 = 0;
-
+    result.m4 = cz*sy*sx - cx*sz;
+    result.m8 = sz*sx + cz*cx*sy;
     result.m12 = 0;
+
+    result.m1 = cy*sz;
+    result.m5 = cz*cx + sz*sy*sx;
+    result.m9 = cx*sz*sy - cz*sx;
     result.m13 = 0;
+
+    result.m2 = -sy;
+    result.m6 = cy*sx;
+    result.m10 = cy*cx;
     result.m14 = 0;
+
+    result.m3 = 0;
+    result.m7 = 0;
+    result.m11 = 0;
     result.m15 = 1;
 
     return result;
@@ -1269,7 +1485,7 @@ RMAPI Matrix MatrixFrustum(double left, double right, double bottom, double top,
 }
 
 // Get perspective projection matrix
-// NOTE: Angle should be provided in radians
+// NOTE: Fovy angle must be provided in radians
 RMAPI Matrix MatrixPerspective(double fovy, double aspect, double near, double far)
 {
     Matrix result = { 0 };
@@ -1478,10 +1694,9 @@ RMAPI Quaternion QuaternionInvert(Quaternion q)
 {
     Quaternion result = q;
 
-    float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
-    float lengthSq = length*length;
+    float lengthSq = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
 
-    if (lengthSq != 0.0)
+    if (lengthSq != 0.0f)
     {
         float invLength = 1.0f/lengthSq;
 
@@ -1515,12 +1730,10 @@ RMAPI Quaternion QuaternionScale(Quaternion q, float mul)
 {
     Quaternion result = { 0 };
 
-    float qax = q.x, qay = q.y, qaz = q.z, qaw = q.w;
-
-    result.x = qax*mul + qaw*mul + qay*mul - qaz*mul;
-    result.y = qay*mul + qaw*mul + qaz*mul - qax*mul;
-    result.z = qaz*mul + qaw*mul + qax*mul - qay*mul;
-    result.w = qaw*mul - qax*mul - qay*mul - qaz*mul;
+    result.x = q.x*mul;
+    result.y = q.y*mul;
+    result.z = q.z*mul;
+    result.w = q.w*mul;
 
     return result;
 }
@@ -1584,14 +1797,14 @@ RMAPI Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount)
         cosHalfTheta = -cosHalfTheta;
     }
 
-    if (fabs(cosHalfTheta) >= 1.0f) result = q1;
+    if (fabsf(cosHalfTheta) >= 1.0f) result = q1;
     else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount);
     else
     {
         float halfTheta = acosf(cosHalfTheta);
         float sinHalfTheta = sqrtf(1.0f - cosHalfTheta*cosHalfTheta);
 
-        if (fabs(sinHalfTheta) < 0.001f)
+        if (fabsf(sinHalfTheta) < 0.001f)
         {
             result.x = (q1.x*0.5f + q2.x*0.5f);
             result.y = (q1.y*0.5f + q2.y*0.5f);
@@ -1646,30 +1859,60 @@ RMAPI Quaternion QuaternionFromMatrix(Matrix mat)
 {
     Quaternion result = { 0 };
 
-    if ((mat.m0 > mat.m5) && (mat.m0 > mat.m10))
-    {
-        float s = sqrtf(1.0f + mat.m0 - mat.m5 - mat.m10)*2;
+    float fourWSquaredMinus1 = mat.m0 + mat.m5 + mat.m10;
+    float fourXSquaredMinus1 = mat.m0 - mat.m5 - mat.m10;
+    float fourYSquaredMinus1 = mat.m5 - mat.m0 - mat.m10;
+    float fourZSquaredMinus1 = mat.m10 - mat.m0 - mat.m5;
 
-        result.x = 0.25f*s;
-        result.y = (mat.m4 + mat.m1)/s;
-        result.z = (mat.m2 + mat.m8)/s;
-        result.w = (mat.m9 - mat.m6)/s;
-    }
-    else if (mat.m5 > mat.m10)
+    int biggestIndex = 0;
+    float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
+    if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
     {
-        float s = sqrtf(1.0f + mat.m5 - mat.m0 - mat.m10)*2;
-        result.x = (mat.m4 + mat.m1)/s;
-        result.y = 0.25f*s;
-        result.z = (mat.m9 + mat.m6)/s;
-        result.w = (mat.m2 - mat.m8)/s;
+        fourBiggestSquaredMinus1 = fourXSquaredMinus1;
+        biggestIndex = 1;
     }
-    else
+
+    if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
     {
-        float s = sqrtf(1.0f + mat.m10 - mat.m0 - mat.m5)*2;
-        result.x = (mat.m2 + mat.m8)/s;
-        result.y = (mat.m9 + mat.m6)/s;
-        result.z = 0.25f*s;
-        result.w = (mat.m4 - mat.m1)/s;
+        fourBiggestSquaredMinus1 = fourYSquaredMinus1;
+        biggestIndex = 2;
+    }
+
+    if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
+    {
+        fourBiggestSquaredMinus1 = fourZSquaredMinus1;
+        biggestIndex = 3;
+    }
+
+    float biggestVal = sqrtf(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;
+    float mult = 0.25f / biggestVal;
+
+    switch (biggestIndex)
+    {
+        case 0:
+            result.w = biggestVal;
+            result.x = (mat.m6 - mat.m9) * mult;
+            result.y = (mat.m8 - mat.m2) * mult;
+            result.z = (mat.m1 - mat.m4) * mult;
+            break;
+        case 1:
+            result.x = biggestVal;
+            result.w = (mat.m6 - mat.m9) * mult;
+            result.y = (mat.m1 + mat.m4) * mult;
+            result.z = (mat.m8 + mat.m2) * mult;
+            break;
+        case 2:
+            result.y = biggestVal;
+            result.w = (mat.m8 - mat.m2) * mult;
+            result.x = (mat.m1 + mat.m4) * mult;
+            result.z = (mat.m6 + mat.m9) * mult;
+            break;
+        case 3:
+            result.z = biggestVal;
+            result.w = (mat.m1 - mat.m4) * mult;
+            result.x = (mat.m8 + mat.m2) * mult;
+            result.y = (mat.m6 + mat.m9) * mult;
+            break;
     }
 
     return result;
@@ -1709,7 +1952,7 @@ RMAPI Matrix QuaternionToMatrix(Quaternion q)
 }
 
 // Get rotation quaternion for an angle and axis
-// NOTE: angle must be provided in radians
+// NOTE: Angle must be provided in radians
 RMAPI Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
 {
     Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
@@ -1757,7 +2000,7 @@ RMAPI Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
 // Get the rotation angle and axis for a given quaternion
 RMAPI void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle)
 {
-    if (fabs(q.w) > 1.0f)
+    if (fabsf(q.w) > 1.0f)
     {
         // QuaternionNormalize(q);
         float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
@@ -1850,4 +2093,19 @@ RMAPI Quaternion QuaternionTransform(Quaternion q, Matrix mat)
     return result;
 }
 
+// Check whether two given quaternions are almost equal
+RMAPI int QuaternionEquals(Quaternion p, Quaternion q)
+{
+    int 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)))))) ||
+                  (((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