From 239c37246a533681953554a303e68f25b9ceba17 Mon Sep 17 00:00:00 2001
From: Ray <raysan5@gmail.com>
Date: Sun, 3 Oct 2021 11:44:59 +0200
Subject: [PATCH] WARNING: REVIEWED: Follow a set of conventions

CONVENTIONS:
  - 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
  - Functions use always a "result" variable for return
  - Functions are always defined inline
  - Angles are always in radians (DEG2RAD/RAD2DEG macros provided for convenience)
---
 examples/core/core_quat_conversion.c |   3 -
 examples/textures/textures_poly.c    |   6 +-
 src/raymath.h                        | 586 ++++++++++++++++++++-------
 3 files changed, 433 insertions(+), 162 deletions(-)

diff --git a/examples/core/core_quat_conversion.c b/examples/core/core_quat_conversion.c
index cca6f01ef..f3c9b757e 100644
--- a/examples/core/core_quat_conversion.c
+++ b/examples/core/core_quat_conversion.c
@@ -69,9 +69,6 @@ int main(void)
         m3 = QuaternionToMatrix(q1);
 
         v2 = QuaternionToEuler(q1);
-        v2.x *= DEG2RAD;
-        v2.y *= DEG2RAD;
-        v2.z *= DEG2RAD;
 
         m4 = MatrixRotateZYX(v2);
         //--------------------------------------------------------------------------------------
diff --git a/examples/textures/textures_poly.c b/examples/textures/textures_poly.c
index ff7283430..a2423a4bb 100644
--- a/examples/textures/textures_poly.c
+++ b/examples/textures/textures_poly.c
@@ -53,7 +53,7 @@ int main(void)
 
     Texture texture = LoadTexture("resources/cat.png");
 
-    float ang = 0;
+    float angle = 0.0f;
 
     SetTargetFPS(60);               // Set our game to run at 60 frames-per-second
     //--------------------------------------------------------------------------------------
@@ -63,11 +63,11 @@ int main(void)
     {
         // Update
         //----------------------------------------------------------------------------------
-        ang++;
+        angle++;
 
         Vector2 positions[MAX_POINTS] = { 0 };
 
-        for (int i = 0; i < MAX_POINTS; i++) positions[i] = Vector2Rotate(points[i], ang);
+        for (int i = 0; i < MAX_POINTS; i++) positions[i] = Vector2Rotate(points[i], angle*DEG2RAD);
         //----------------------------------------------------------------------------------
 
         // Draw
diff --git a/src/raymath.h b/src/raymath.h
index 79fd5480b..abd455194 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -1,6 +1,6 @@
 /**********************************************************************************************
 *
-*   raymath v1.3 - Math functions to work with Vector3, Matrix and Quaternions
+*   raymath v1.5 - Math functions to work with Vector2, Vector3, Matrix and Quaternions
 *
 *   CONFIGURATION:
 *
@@ -13,6 +13,15 @@
 *       Define static inline functions code, so #include header suffices for use.
 *       This may use up lots of memory.
 *
+*   CONVENTIONS:
+*
+*     - 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
+*     - 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
 *
@@ -153,26 +162,35 @@ typedef struct float16 {
 // Clamp float value
 RMDEF float Clamp(float value, float min, float max)
 {
-    const float res = value < min ? min : value;
-    return res > max ? max : res;
+    float result = (value < min)? min : value;
+
+    if (result > max) result = max;
+
+    return result;
 }
 
 // Calculate linear interpolation between two floats
 RMDEF float Lerp(float start, float end, float amount)
 {
-    return start + amount*(end - start);
+    float result = start + amount*(end - start);
+
+    return result;
 }
 
 // Normalize input value within input range
 RMDEF float Normalize(float value, float start, float end)
 {
-    return (value - start)/(end - start);
+    float result = (value - start)/(end - start);
+
+    return result;
 }
 
 // Remap input value within input range to output range
 RMDEF float Remap(float value, float inputStart, float inputEnd, float outputStart, float outputEnd)
 {
-    return (value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart;
+    float result =(value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart;
+
+    return result;
 }
 
 //----------------------------------------------------------------------------------
@@ -183,6 +201,7 @@ RMDEF float Remap(float value, float inputStart, float inputEnd, float outputSta
 RMDEF Vector2 Vector2Zero(void)
 {
     Vector2 result = { 0.0f, 0.0f };
+
     return result;
 }
 
@@ -190,6 +209,7 @@ RMDEF Vector2 Vector2Zero(void)
 RMDEF Vector2 Vector2One(void)
 {
     Vector2 result = { 1.0f, 1.0f };
+
     return result;
 }
 
@@ -197,6 +217,7 @@ RMDEF Vector2 Vector2One(void)
 RMDEF Vector2 Vector2Add(Vector2 v1, Vector2 v2)
 {
     Vector2 result = { v1.x + v2.x, v1.y + v2.y };
+
     return result;
 }
 
@@ -204,6 +225,7 @@ RMDEF Vector2 Vector2Add(Vector2 v1, Vector2 v2)
 RMDEF Vector2 Vector2AddValue(Vector2 v, float add)
 {
     Vector2 result = { v.x + add, v.y + add };
+
     return result;
 }
 
@@ -211,6 +233,7 @@ RMDEF Vector2 Vector2AddValue(Vector2 v, float add)
 RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2)
 {
     Vector2 result = { v1.x - v2.x, v1.y - v2.y };
+
     return result;
 }
 
@@ -218,6 +241,7 @@ RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2)
 RMDEF Vector2 Vector2SubtractValue(Vector2 v, float sub)
 {
     Vector2 result = { v.x - sub, v.y - sub };
+
     return result;
 }
 
@@ -225,6 +249,7 @@ RMDEF Vector2 Vector2SubtractValue(Vector2 v, float sub)
 RMDEF float Vector2Length(Vector2 v)
 {
     float result = sqrtf((v.x*v.x) + (v.y*v.y));
+
     return result;
 }
 
@@ -232,6 +257,7 @@ RMDEF float Vector2Length(Vector2 v)
 RMDEF float Vector2LengthSqr(Vector2 v)
 {
     float result = (v.x*v.x) + (v.y*v.y);
+
     return result;
 }
 
@@ -239,6 +265,7 @@ RMDEF float Vector2LengthSqr(Vector2 v)
 RMDEF float Vector2DotProduct(Vector2 v1, Vector2 v2)
 {
     float result = (v1.x*v2.x + v1.y*v2.y);
+
     return result;
 }
 
@@ -246,6 +273,7 @@ RMDEF float Vector2DotProduct(Vector2 v1, Vector2 v2)
 RMDEF float Vector2Distance(Vector2 v1, Vector2 v2)
 {
     float result = sqrtf((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y));
+
     return result;
 }
 
@@ -253,7 +281,9 @@ RMDEF float Vector2Distance(Vector2 v1, Vector2 v2)
 RMDEF 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;
+
     return result;
 }
 
@@ -261,6 +291,7 @@ RMDEF float Vector2Angle(Vector2 v1, Vector2 v2)
 RMDEF Vector2 Vector2Scale(Vector2 v, float scale)
 {
     Vector2 result = { v.x*scale, v.y*scale };
+
     return result;
 }
 
@@ -268,6 +299,7 @@ RMDEF Vector2 Vector2Scale(Vector2 v, float scale)
 RMDEF Vector2 Vector2Multiply(Vector2 v1, Vector2 v2)
 {
     Vector2 result = { v1.x*v2.x, v1.y*v2.y };
+
     return result;
 }
 
@@ -275,6 +307,7 @@ RMDEF Vector2 Vector2Multiply(Vector2 v1, Vector2 v2)
 RMDEF Vector2 Vector2Negate(Vector2 v)
 {
     Vector2 result = { -v.x, -v.y };
+
     return result;
 }
 
@@ -282,17 +315,22 @@ RMDEF Vector2 Vector2Negate(Vector2 v)
 RMDEF Vector2 Vector2Divide(Vector2 v1, Vector2 v2)
 {
     Vector2 result = { v1.x/v2.x, v1.y/v2.y };
+
     return result;
 }
 
 // Normalize provided vector
 RMDEF Vector2 Vector2Normalize(Vector2 v)
 {
-    float length = Vector2Length(v);
-    if (length <= 0)
-        return v;
+    Vector2 result = { 0 };
+    float length = sqrtf((v.x*v.x) + (v.y*v.y));
+
+    if (length > 0)
+    {
+        result.x = v.x*1.0f/length;
+        result.y = v.y*1.0f/length;
+    }
 
-    Vector2 result = Vector2Scale(v, 1/length);
     return result;
 }
 
@@ -312,7 +350,7 @@ RMDEF Vector2 Vector2Reflect(Vector2 v, Vector2 normal)
 {
     Vector2 result = { 0 };
 
-    float dotProduct = Vector2DotProduct(v, normal);
+    float dotProduct = (v.x*normal.x + v.y*normal.y); // Dot product
 
     result.x = v.x - (2.0f*normal.x)*dotProduct;
     result.y = v.y - (2.0f*normal.y)*dotProduct;
@@ -320,11 +358,14 @@ RMDEF Vector2 Vector2Reflect(Vector2 v, Vector2 normal)
     return result;
 }
 
-// Rotate Vector by float in Degrees.
-RMDEF Vector2 Vector2Rotate(Vector2 v, float degs)
+// Rotate vector by angle
+RMDEF Vector2 Vector2Rotate(Vector2 v, float angle)
 {
-    float rads = degs*DEG2RAD;
-    Vector2 result = {v.x*cosf(rads) - v.y*sinf(rads) , v.x*sinf(rads) + v.y*cosf(rads) };
+    Vector2 result = { 0 };
+
+    result.x = v.x*cosf(angle) - v.y*sinf(angle);
+    result.y = v.x*sinf(angle) + v.y*cosf(angle);
+
     return result;
 }
 
@@ -332,6 +373,7 @@ RMDEF Vector2 Vector2Rotate(Vector2 v, float degs)
 RMDEF Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance)
 {
     Vector2 result = { 0 };
+
     float dx = target.x - v.x;
     float dy = target.y - v.y;
     float value = (dx*dx) + (dy*dy);
@@ -354,6 +396,7 @@ RMDEF Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance)
 RMDEF Vector3 Vector3Zero(void)
 {
     Vector3 result = { 0.0f, 0.0f, 0.0f };
+
     return result;
 }
 
@@ -361,6 +404,7 @@ RMDEF Vector3 Vector3Zero(void)
 RMDEF Vector3 Vector3One(void)
 {
     Vector3 result = { 1.0f, 1.0f, 1.0f };
+
     return result;
 }
 
@@ -368,6 +412,7 @@ RMDEF Vector3 Vector3One(void)
 RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2)
 {
     Vector3 result = { v1.x + v2.x, v1.y + v2.y, v1.z + v2.z };
+
     return result;
 }
 
@@ -375,6 +420,7 @@ RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2)
 RMDEF Vector3 Vector3AddValue(Vector3 v, float add)
 {
     Vector3 result = { v.x + add, v.y + add, v.z + add };
+
     return result;
 }
 
@@ -382,6 +428,7 @@ RMDEF Vector3 Vector3AddValue(Vector3 v, float add)
 RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2)
 {
     Vector3 result = { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
+
     return result;
 }
 
@@ -389,6 +436,7 @@ RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2)
 RMDEF Vector3 Vector3SubtractValue(Vector3 v, float sub)
 {
     Vector3 result = { v.x - sub, v.y - sub, v.z - sub };
+
     return result;
 }
 
@@ -396,6 +444,7 @@ RMDEF Vector3 Vector3SubtractValue(Vector3 v, float sub)
 RMDEF Vector3 Vector3Scale(Vector3 v, float scalar)
 {
     Vector3 result = { v.x*scalar, v.y*scalar, v.z*scalar };
+
     return result;
 }
 
@@ -403,6 +452,7 @@ RMDEF Vector3 Vector3Scale(Vector3 v, float scalar)
 RMDEF Vector3 Vector3Multiply(Vector3 v1, Vector3 v2)
 {
     Vector3 result = { v1.x*v2.x, v1.y*v2.y, v1.z*v2.z };
+
     return result;
 }
 
@@ -410,6 +460,7 @@ RMDEF Vector3 Vector3Multiply(Vector3 v1, Vector3 v2)
 RMDEF Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2)
 {
     Vector3 result = { v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x };
+
     return result;
 }
 
@@ -434,7 +485,10 @@ RMDEF Vector3 Vector3Perpendicular(Vector3 v)
         cardinalAxis = tmp;
     }
 
-    result = Vector3CrossProduct(v, cardinalAxis);
+    // Cross product between vectors
+    result.x = v.y*cardinalAxis.z - v.z*cardinalAxis.y;
+    result.y = v.z*cardinalAxis.x - v.x*cardinalAxis.z;
+    result.z = v.x*cardinalAxis.y - v.y*cardinalAxis.x;
 
     return result;
 }
@@ -443,6 +497,7 @@ RMDEF Vector3 Vector3Perpendicular(Vector3 v)
 RMDEF float Vector3Length(const Vector3 v)
 {
     float result = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+
     return result;
 }
 
@@ -450,6 +505,7 @@ RMDEF float Vector3Length(const Vector3 v)
 RMDEF float Vector3LengthSqr(const Vector3 v)
 {
     float result = v.x*v.x + v.y*v.y + v.z*v.z;
+
     return result;
 }
 
@@ -457,16 +513,20 @@ RMDEF float Vector3LengthSqr(const Vector3 v)
 RMDEF float Vector3DotProduct(Vector3 v1, Vector3 v2)
 {
     float result = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
+
     return result;
 }
 
 // Calculate distance between two vectors
 RMDEF float Vector3Distance(Vector3 v1, Vector3 v2)
 {
+    float result = 0.0f;
+
     float dx = v2.x - v1.x;
     float dy = v2.y - v1.y;
     float dz = v2.z - v1.z;
-    float result = sqrtf(dx*dx + dy*dy + dz*dz);
+    result = sqrtf(dx*dx + dy*dy + dz*dz);
+
     return result;
 }
 
@@ -489,6 +549,7 @@ RMDEF Vector2 Vector3Angle(Vector3 v1, Vector3 v2)
 RMDEF Vector3 Vector3Negate(Vector3 v)
 {
     Vector3 result = { -v.x, -v.y, -v.z };
+
     return result;
 }
 
@@ -496,6 +557,7 @@ RMDEF Vector3 Vector3Negate(Vector3 v)
 RMDEF Vector3 Vector3Divide(Vector3 v1, Vector3 v2)
 {
     Vector3 result = { v1.x/v2.x, v1.y/v2.y, v1.z/v2.z };
+
     return result;
 }
 
@@ -504,14 +566,13 @@ RMDEF Vector3 Vector3Normalize(Vector3 v)
 {
     Vector3 result = v;
 
-    float length, inverseLength;
-    length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+    float length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
     if (length == 0.0f) length = 1.0f;
-    inverseLength = 1.0f/length;
+    float ilength = 1.0f/length;
 
-    result.x *= inverseLength;
-    result.y *= inverseLength;
-    result.z *= inverseLength;
+    result.x *= ilength;
+    result.y *= ilength;
+    result.z *= ilength;
 
     return result;
 }
@@ -521,16 +582,41 @@ RMDEF Vector3 Vector3Normalize(Vector3 v)
 // Gram-Schmidt function implementation
 RMDEF void Vector3OrthoNormalize(Vector3 *v1, Vector3 *v2)
 {
-    *v1 = Vector3Normalize(*v1);
-    Vector3 vn = Vector3CrossProduct(*v1, *v2);
-    vn = Vector3Normalize(vn);
-    *v2 = Vector3CrossProduct(vn, *v1);
+    float length = 0.0f;
+    float ilength = 0.0f;
+
+    // Vector3Normalize(*v1);
+    Vector3 v = *v1;
+    length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+    if (length == 0.0f) length = 1.0f;
+    ilength = 1.0f/length;
+    v1->x *= ilength;
+    v1->y *= ilength;
+    v1->z *= ilength;
+
+    // Vector3CrossProduct(*v1, *v2)
+    Vector3 vn1 = { v1->y*v2->z - v1->z*v2->y, v1->z*v2->x - v1->x*v2->z, v1->x*v2->y - v1->y*v2->x };
+
+    // Vector3Normalize(vn1);
+    v = vn1;
+    length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+    if (length == 0.0f) length = 1.0f;
+    ilength = 1.0f/length;
+    vn1.x *= ilength;
+    vn1.y *= ilength;
+    vn1.z *= ilength;
+
+    // Vector3CrossProduct(vn1, *v1)
+    Vector3 vn2 = { vn1.y*v1->z - vn1.z*v1->y, vn1.z*v1->x - vn1.x*v1->z, vn1.x*v1->y - vn1.y*v1->x };
+
+    *v2 = vn2;
 }
 
 // Transforms a Vector3 by a given Matrix
 RMDEF Vector3 Vector3Transform(Vector3 v, Matrix mat)
 {
     Vector3 result = { 0 };
+
     float x = v.x;
     float y = v.y;
     float z = v.z;
@@ -569,13 +655,13 @@ RMDEF Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
 // Calculate reflected vector to normal
 RMDEF Vector3 Vector3Reflect(Vector3 v, Vector3 normal)
 {
-    // I is the original vector
-    // N is the normal of the incident plane
-    // R = I - (2*N*( DotProduct[ I,N] ))
-
     Vector3 result = { 0 };
 
-    float dotProduct = Vector3DotProduct(v, normal);
+    // I is the original vector
+    // N is the normal of the incident plane
+    // R = I - (2*N*(DotProduct[I, N]))
+
+    float dotProduct = (v.x*normal.x + v.y*normal.y + v.z*normal.z);
 
     result.x = v.x - (2.0f*normal.x)*dotProduct;
     result.y = v.y - (2.0f*normal.y)*dotProduct;
@@ -612,21 +698,19 @@ RMDEF Vector3 Vector3Max(Vector3 v1, Vector3 v2)
 // NOTE: Assumes P is on the plane of the triangle
 RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
 {
-    //Vector v0 = b - a, v1 = c - a, v2 = p - a;
+    Vector3 result = { 0 };
 
-    Vector3 v0 = Vector3Subtract(b, a);
-    Vector3 v1 = Vector3Subtract(c, a);
-    Vector3 v2 = Vector3Subtract(p, a);
-    float d00 = Vector3DotProduct(v0, v0);
-    float d01 = Vector3DotProduct(v0, v1);
-    float d11 = Vector3DotProduct(v1, v1);
-    float d20 = Vector3DotProduct(v2, v0);
-    float d21 = Vector3DotProduct(v2, v1);
+    Vector3 v0 = { b.x - a.x, b.y - a.y, b.z - a.z };   // Vector3Subtract(b, a)
+    Vector3 v1 = { c.x - a.x, c.y - a.y, c.z - a.z };   // Vector3Subtract(c, a)
+    Vector3 v2 = { p.x - a.x, p.y - a.y, p.z - a.z };   // Vector3Subtract(p, a)
+    float d00 = (v0.x*v0.x + v0.y*v0.y + v0.z*v0.z);    // Vector3DotProduct(v0, v0)
+    float d01 = (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z);    // Vector3DotProduct(v0, v1)
+    float d11 = (v1.x*v1.x + v1.y*v1.y + v1.z*v1.z);    // Vector3DotProduct(v1, v1)
+    float d20 = (v2.x*v0.x + v2.y*v0.y + v2.z*v0.z);    // Vector3DotProduct(v2, v0)
+    float d21 = (v2.x*v1.x + v2.y*v1.y + v2.z*v1.z);    // Vector3DotProduct(v2, v1)
 
     float denom = d00*d11 - d01*d01;
 
-    Vector3 result = { 0 };
-
     result.y = (d11*d20 - d01*d21)/denom;
     result.z = (d00*d21 - d01*d20)/denom;
     result.x = 1.0f - (result.z + result.y);
@@ -634,6 +718,90 @@ RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
     return result;
 }
 
+// Projects a Vector3 from screen space into object space
+// NOTE: We are avoiding calling other raymath functions despite available
+RMDEF Vector3 Vector3Unproject(Vector3 source, Matrix projection, Matrix view)
+{
+    Vector3 result = { 0 };
+
+    // Calculate unproject matrix (multiply view patrix by projection matrix) and invert it
+    Matrix matViewProj = {      // MatrixMultiply(view, projection);
+        view.m0*projection.m0 + view.m1*projection.m4 + view.m2*projection.m8 + view.m3*projection.m12,
+        view.m0*projection.m1 + view.m1*projection.m5 + view.m2*projection.m9 + view.m3*projection.m13,
+        view.m0*projection.m2 + view.m1*projection.m6 + view.m2*projection.m10 + view.m3*projection.m14,
+        view.m0*projection.m3 + view.m1*projection.m7 + view.m2*projection.m11 + view.m3*projection.m15,
+        view.m4*projection.m0 + view.m5*projection.m4 + view.m6*projection.m8 + view.m7*projection.m12,
+        view.m4*projection.m1 + view.m5*projection.m5 + view.m6*projection.m9 + view.m7*projection.m13,
+        view.m4*projection.m2 + view.m5*projection.m6 + view.m6*projection.m10 + view.m7*projection.m14,
+        view.m4*projection.m3 + view.m5*projection.m7 + view.m6*projection.m11 + view.m7*projection.m15,
+        view.m8*projection.m0 + view.m9*projection.m4 + view.m10*projection.m8 + view.m11*projection.m12,
+        view.m8*projection.m1 + view.m9*projection.m5 + view.m10*projection.m9 + view.m11*projection.m13,
+        view.m8*projection.m2 + view.m9*projection.m6 + view.m10*projection.m10 + view.m11*projection.m14,
+        view.m8*projection.m3 + view.m9*projection.m7 + view.m10*projection.m11 + view.m11*projection.m15,
+        view.m12*projection.m0 + view.m13*projection.m4 + view.m14*projection.m8 + view.m15*projection.m12,
+        view.m12*projection.m1 + view.m13*projection.m5 + view.m14*projection.m9 + view.m15*projection.m13,
+        view.m12*projection.m2 + view.m13*projection.m6 + view.m14*projection.m10 + view.m15*projection.m14,
+        view.m12*projection.m3 + view.m13*projection.m7 + view.m14*projection.m11 + view.m15*projection.m15 };
+
+    // Calculate inverted matrix -> MatrixInvert(matViewProj);
+    // Cache the matrix values (speed optimization)
+    float a00 = matViewProj.m0, a01 = matViewProj.m1, a02 = matViewProj.m2, a03 = matViewProj.m3;
+    float a10 = matViewProj.m4, a11 = matViewProj.m5, a12 = matViewProj.m6, a13 = matViewProj.m7;
+    float a20 = matViewProj.m8, a21 = matViewProj.m9, a22 = matViewProj.m10, a23 = matViewProj.m11;
+    float a30 = matViewProj.m12, a31 = matViewProj.m13, a32 = matViewProj.m14, a33 = matViewProj.m15;
+
+    float b00 = a00*a11 - a01*a10;
+    float b01 = a00*a12 - a02*a10;
+    float b02 = a00*a13 - a03*a10;
+    float b03 = a01*a12 - a02*a11;
+    float b04 = a01*a13 - a03*a11;
+    float b05 = a02*a13 - a03*a12;
+    float b06 = a20*a31 - a21*a30;
+    float b07 = a20*a32 - a22*a30;
+    float b08 = a20*a33 - a23*a30;
+    float b09 = a21*a32 - a22*a31;
+    float b10 = a21*a33 - a23*a31;
+    float b11 = a22*a33 - a23*a32;
+
+    // Calculate the invert determinant (inlined to avoid double-caching)
+    float invDet = 1.0f/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06);
+
+    Matrix matViewProjInv = {
+        (a11*b11 - a12*b10 + a13*b09)*invDet,
+        (-a01*b11 + a02*b10 - a03*b09)*invDet,
+        (a31*b05 - a32*b04 + a33*b03)*invDet,
+        (-a21*b05 + a22*b04 - a23*b03)*invDet,
+        (-a10*b11 + a12*b08 - a13*b07)*invDet,
+        (a00*b11 - a02*b08 + a03*b07)*invDet,
+        (-a30*b05 + a32*b02 - a33*b01)*invDet,
+        (a20*b05 - a22*b02 + a23*b01)*invDet,
+        (a10*b10 - a11*b08 + a13*b06)*invDet,
+        (-a00*b10 + a01*b08 - a03*b06)*invDet,
+        (a30*b04 - a31*b02 + a33*b00)*invDet,
+        (-a20*b04 + a21*b02 - a23*b00)*invDet,
+        (-a10*b09 + a11*b07 - a12*b06)*invDet,
+        (a00*b09 - a01*b07 + a02*b06)*invDet,
+        (-a30*b03 + a31*b01 - a32*b00)*invDet,
+        (a20*b03 - a21*b01 + a22*b00)*invDet };
+
+    // Create quaternion from source point
+    Quaternion quat = { source.x, source.y, source.z, 1.0f };
+
+    // Multiply quat point by unproject matrix
+    Quaternion qtransformed = {     // QuaternionTransform(quat, matViewProjInv)
+        matViewProjInv.m0*quat.x + matViewProjInv.m4*quat.y + matViewProjInv.m8*quat.z + matViewProjInv.m12*quat.w,
+        matViewProjInv.m1*quat.x + matViewProjInv.m5*quat.y + matViewProjInv.m9*quat.z + matViewProjInv.m13*quat.w,
+        matViewProjInv.m2*quat.x + matViewProjInv.m6*quat.y + matViewProjInv.m10*quat.z + matViewProjInv.m14*quat.w,
+        matViewProjInv.m3*quat.x + matViewProjInv.m7*quat.y + matViewProjInv.m11*quat.z + matViewProjInv.m15*quat.w };
+
+    // Normalized world points in vectors
+    result.x = qtransformed.x/qtransformed.w;
+    result.y = qtransformed.y/qtransformed.w;
+    result.z = qtransformed.z/qtransformed.w;
+
+    return result;
+}
+
 // Get Vector3 as float array
 RMDEF float3 Vector3ToFloatV(Vector3 v)
 {
@@ -653,18 +821,20 @@ RMDEF float3 Vector3ToFloatV(Vector3 v)
 // Compute matrix determinant
 RMDEF float MatrixDeterminant(Matrix mat)
 {
+    float result = 0.0f;
+
     // 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;
 
-    float result = 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 = 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;
 
     return result;
 }
@@ -673,6 +843,7 @@ RMDEF float MatrixDeterminant(Matrix mat)
 RMDEF float MatrixTrace(Matrix mat)
 {
     float result = (mat.m0 + mat.m5 + mat.m10 + mat.m15);
+
     return result;
 }
 
@@ -753,7 +924,19 @@ RMDEF Matrix MatrixNormalize(Matrix mat)
 {
     Matrix result = { 0 };
 
-    float det = MatrixDeterminant(mat);
+    // 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;
@@ -789,7 +972,7 @@ RMDEF Matrix MatrixIdentity(void)
 // Add two matrices
 RMDEF Matrix MatrixAdd(Matrix left, Matrix right)
 {
-    Matrix result = MatrixIdentity();
+    Matrix result = { 0 };
 
     result.m0 = left.m0 + right.m0;
     result.m1 = left.m1 + right.m1;
@@ -814,7 +997,7 @@ RMDEF Matrix MatrixAdd(Matrix left, Matrix right)
 // Subtract two matrices (left - right)
 RMDEF Matrix MatrixSubtract(Matrix left, Matrix right)
 {
-    Matrix result = MatrixIdentity();
+    Matrix result = { 0 };
 
     result.m0 = left.m0 - right.m0;
     result.m1 = left.m1 - right.m1;
@@ -885,10 +1068,10 @@ RMDEF Matrix MatrixRotate(Vector3 axis, float angle)
 
     if ((lengthSquared != 1.0f) && (lengthSquared != 0.0f))
     {
-        float inverseLength = 1.0f/sqrtf(lengthSquared);
-        x *= inverseLength;
-        y *= inverseLength;
-        z *= inverseLength;
+        float ilength = 1.0f/sqrtf(lengthSquared);
+        x *= ilength;
+        y *= ilength;
+        z *= ilength;
     }
 
     float sinres = sinf(angle);
@@ -921,7 +1104,10 @@ RMDEF Matrix MatrixRotate(Vector3 axis, float angle)
 // Get x-rotation matrix (angle in radians)
 RMDEF Matrix MatrixRotateX(float angle)
 {
-    Matrix result = MatrixIdentity();
+    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 cosres = cosf(angle);
     float sinres = sinf(angle);
@@ -937,7 +1123,10 @@ RMDEF Matrix MatrixRotateX(float angle)
 // Get y-rotation matrix (angle in radians)
 RMDEF Matrix MatrixRotateY(float angle)
 {
-    Matrix result = MatrixIdentity();
+    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 cosres = cosf(angle);
     float sinres = sinf(angle);
@@ -953,7 +1142,10 @@ RMDEF Matrix MatrixRotateY(float angle)
 // Get z-rotation matrix (angle in radians)
 RMDEF Matrix MatrixRotateZ(float angle)
 {
-    Matrix result = MatrixIdentity();
+    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 cosres = cosf(angle);
     float sinres = sinf(angle);
@@ -970,7 +1162,10 @@ RMDEF Matrix MatrixRotateZ(float angle)
 // Get xyz-rotation matrix (angles in radians)
 RMDEF Matrix MatrixRotateXYZ(Vector3 ang)
 {
-    Matrix result = MatrixIdentity();
+    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);
@@ -1049,13 +1244,13 @@ RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top,
     float tb = (float)(top - bottom);
     float fn = (float)(far - near);
 
-    result.m0 = ((float) near*2.0f)/rl;
+    result.m0 = ((float)near*2.0f)/rl;
     result.m1 = 0.0f;
     result.m2 = 0.0f;
     result.m3 = 0.0f;
 
     result.m4 = 0.0f;
-    result.m5 = ((float) near*2.0f)/tb;
+    result.m5 = ((float)near*2.0f)/tb;
     result.m6 = 0.0f;
     result.m7 = 0.0f;
 
@@ -1076,9 +1271,25 @@ RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top,
 // NOTE: Angle should be provided in radians
 RMDEF Matrix MatrixPerspective(double fovy, double aspect, double near, double far)
 {
+    Matrix result = { 0 };
+
     double top = near*tan(fovy*0.5);
+    double bottom = -top;
     double right = top*aspect;
-    Matrix result = MatrixFrustum(-right, right, -top, top, near, far);
+    double left = -right;
+
+    // MatrixFrustum(-right, right, -top, top, near, far);
+    float rl = (float)(right - left);
+    float tb = (float)(top - bottom);
+    float fn = (float)(far - near);
+
+    result.m0 = ((float)near*2.0f)/rl;
+    result.m5 = ((float)near*2.0f)/tb;
+    result.m8 = ((float)right + (float)left)/rl;
+    result.m9 = ((float)top + (float)bottom)/tb;
+    result.m10 = -((float)far + (float)near)/fn;
+    result.m11 = -1.0f;
+    result.m14 = -((float)far*(float)near*2.0f)/fn;
 
     return result;
 }
@@ -1117,27 +1328,51 @@ RMDEF Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up)
 {
     Matrix result = { 0 };
 
-    Vector3 z = Vector3Subtract(eye, target);
-    z = Vector3Normalize(z);
-    Vector3 x = Vector3CrossProduct(up, z);
-    x = Vector3Normalize(x);
-    Vector3 y = Vector3CrossProduct(z, x);
+    float length = 0.0f;
+    float ilength = 0.0f;
 
-    result.m0 = x.x;
-    result.m1 = y.x;
-    result.m2 = z.x;
+    // Vector3Subtract(eye, target)
+    Vector3 vz = { eye.x - target.x, eye.y - target.y, eye.z - target.z };
+
+    // Vector3Normalize(vz)
+    Vector3 v = vz;
+    length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+    if (length == 0.0f) length = 1.0f;
+    ilength = 1.0f/length;
+    vz.x *= ilength;
+    vz.y *= ilength;
+    vz.z *= ilength;
+
+    // Vector3CrossProduct(up, vz)
+    Vector3 vx = { up.y*vz.z - up.z*vz.y, up.z*vz.x - up.x*vz.z, up.x*vz.y - up.y*vz.x };
+
+    // Vector3Normalize(x)
+    v = vx;
+    length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+    if (length == 0.0f) length = 1.0f;
+    ilength = 1.0f/length;
+    vx.x *= ilength;
+    vx.y *= ilength;
+    vx.z *= ilength;
+
+    // Vector3CrossProduct(vz, vx)
+    Vector3 vy = { vz.y*vx.z - vz.z*vx.y, vz.z*vx.x - vz.x*vx.z, vz.x*vx.y - vz.y*vx.x };
+
+    result.m0 = vx.x;
+    result.m1 = vy.x;
+    result.m2 = vz.x;
     result.m3 = 0.0f;
-    result.m4 = x.y;
-    result.m5 = y.y;
-    result.m6 = z.y;
+    result.m4 = vx.y;
+    result.m5 = vy.y;
+    result.m6 = vz.y;
     result.m7 = 0.0f;
-    result.m8 = x.z;
-    result.m9 = y.z;
-    result.m10 = z.z;
+    result.m8 = vx.z;
+    result.m9 = vy.z;
+    result.m10 = vz.z;
     result.m11 = 0.0f;
-    result.m12 = -Vector3DotProduct(x, eye);
-    result.m13 = -Vector3DotProduct(y, eye);
-    result.m14 = -Vector3DotProduct(z, eye);
+    result.m12 = -(vx.x*eye.x + vx.y*eye.y + vx.z*eye.z);   // Vector3DotProduct(vx, eye)
+    result.m13 = -(vy.x*eye.x + vy.y*eye.y + vy.z*eye.z);   // Vector3DotProduct(vy, eye)
+    result.m14 = -(vz.x*eye.x + vz.y*eye.y + vz.z*eye.z);   // Vector3DotProduct(vz, eye)
     result.m15 = 1.0f;
 
     return result;
@@ -1146,26 +1381,26 @@ RMDEF Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up)
 // Get float array of matrix data
 RMDEF float16 MatrixToFloatV(Matrix mat)
 {
-    float16 buffer = { 0 };
+    float16 result = { 0 };
 
-    buffer.v[0] = mat.m0;
-    buffer.v[1] = mat.m1;
-    buffer.v[2] = mat.m2;
-    buffer.v[3] = mat.m3;
-    buffer.v[4] = mat.m4;
-    buffer.v[5] = mat.m5;
-    buffer.v[6] = mat.m6;
-    buffer.v[7] = mat.m7;
-    buffer.v[8] = mat.m8;
-    buffer.v[9] = mat.m9;
-    buffer.v[10] = mat.m10;
-    buffer.v[11] = mat.m11;
-    buffer.v[12] = mat.m12;
-    buffer.v[13] = mat.m13;
-    buffer.v[14] = mat.m14;
-    buffer.v[15] = mat.m15;
+    result.v[0] = mat.m0;
+    result.v[1] = mat.m1;
+    result.v[2] = mat.m2;
+    result.v[3] = mat.m3;
+    result.v[4] = mat.m4;
+    result.v[5] = mat.m5;
+    result.v[6] = mat.m6;
+    result.v[7] = mat.m7;
+    result.v[8] = mat.m8;
+    result.v[9] = mat.m9;
+    result.v[10] = mat.m10;
+    result.v[11] = mat.m11;
+    result.v[12] = mat.m12;
+    result.v[13] = mat.m13;
+    result.v[14] = mat.m14;
+    result.v[15] = mat.m15;
 
-    return buffer;
+    return result;
 }
 
 //----------------------------------------------------------------------------------
@@ -1176,6 +1411,7 @@ RMDEF float16 MatrixToFloatV(Matrix mat)
 RMDEF Quaternion QuaternionAdd(Quaternion q1, Quaternion q2)
 {
     Quaternion result = {q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w};
+
     return result;
 }
 
@@ -1183,6 +1419,7 @@ RMDEF Quaternion QuaternionAdd(Quaternion q1, Quaternion q2)
 RMDEF Quaternion QuaternionAddValue(Quaternion q, float add)
 {
     Quaternion result = {q.x + add, q.y + add, q.z + add, q.w + add};
+
     return result;
 }
 
@@ -1190,6 +1427,7 @@ RMDEF Quaternion QuaternionAddValue(Quaternion q, float add)
 RMDEF Quaternion QuaternionSubtract(Quaternion q1, Quaternion q2)
 {
     Quaternion result = {q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w};
+
     return result;
 }
 
@@ -1197,6 +1435,7 @@ RMDEF Quaternion QuaternionSubtract(Quaternion q1, Quaternion q2)
 RMDEF Quaternion QuaternionSubtractValue(Quaternion q, float sub)
 {
     Quaternion result = {q.x - sub, q.y - sub, q.z - sub, q.w - sub};
+
     return result;
 }
 
@@ -1204,6 +1443,7 @@ RMDEF Quaternion QuaternionSubtractValue(Quaternion q, float sub)
 RMDEF Quaternion QuaternionIdentity(void)
 {
     Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
+
     return result;
 }
 
@@ -1211,6 +1451,7 @@ RMDEF Quaternion QuaternionIdentity(void)
 RMDEF float QuaternionLength(Quaternion q)
 {
     float result = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
+
     return result;
 }
 
@@ -1219,10 +1460,9 @@ RMDEF Quaternion QuaternionNormalize(Quaternion q)
 {
     Quaternion result = { 0 };
 
-    float length, ilength;
-    length = QuaternionLength(q);
+    float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
     if (length == 0.0f) length = 1.0f;
-    ilength = 1.0f/length;
+    float ilength = 1.0f/length;
 
     result.x = q.x*ilength;
     result.y = q.y*ilength;
@@ -1236,17 +1476,18 @@ RMDEF Quaternion QuaternionNormalize(Quaternion q)
 RMDEF Quaternion QuaternionInvert(Quaternion q)
 {
     Quaternion result = q;
-    float length = QuaternionLength(q);
+
+    float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
     float lengthSq = length*length;
 
     if (lengthSq != 0.0)
     {
-        float i = 1.0f/lengthSq;
+        float invLength = 1.0f/lengthSq;
 
-        result.x *= -i;
-        result.y *= -i;
-        result.z *= -i;
-        result.w *= i;
+        result.x *= -invLength;
+        result.y *= -invLength;
+        result.z *= -invLength;
+        result.w *= invLength;
     }
 
     return result;
@@ -1287,6 +1528,7 @@ RMDEF Quaternion QuaternionScale(Quaternion q, float mul)
 RMDEF Quaternion QuaternionDivide(Quaternion q1, Quaternion q2)
 {
     Quaternion result = { q1.x/q2.x, q1.y/q2.y, q1.z/q2.z, q1.w/q2.w };
+
     return result;
 }
 
@@ -1306,8 +1548,24 @@ RMDEF Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount)
 // Calculate slerp-optimized interpolation between two quaternions
 RMDEF Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount)
 {
-    Quaternion result = QuaternionLerp(q1, q2, amount);
-    result = QuaternionNormalize(result);
+    Quaternion result = { 0 };
+
+    // QuaternionLerp(q1, q2, amount)
+    result.x = q1.x + amount*(q2.x - q1.x);
+    result.y = q1.y + amount*(q2.y - q1.y);
+    result.z = q1.z + amount*(q2.z - q1.z);
+    result.w = q1.w + amount*(q2.w - q1.w);
+
+    // QuaternionNormalize(q);
+    Quaternion q = result;
+    float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
+    if (length == 0.0f) length = 1.0f;
+    float ilength = 1.0f/length;
+
+    result.x = q.x*ilength;
+    result.y = q.y*ilength;
+    result.z = q.z*ilength;
+    result.w = q.w*ilength;
 
     return result;
 }
@@ -1359,19 +1617,25 @@ RMDEF Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to)
 {
     Quaternion result = { 0 };
 
-    float cos2Theta = Vector3DotProduct(from, to);
-    Vector3 cross = Vector3CrossProduct(from, to);
+    float cos2Theta = (from.x*to.x + from.y*to.y + from.z*to.z);    // Vector3DotProduct(from, to)
+    Vector3 cross = { from.y*to.z - from.z*to.y, from.z*to.x - from.x*to.z, from.x*to.y - from.y*to.x }; // Vector3CrossProduct(from, to)
 
     result.x = cross.x;
     result.y = cross.y;
     result.z = cross.z;
-    result.w = 1.0f + cos2Theta;     // NOTE: Added QuaternioIdentity()
+    result.w = 1.0f + cos2Theta;
 
-    // Normalize to essentially nlerp the original and identity to 0.5
-    result = QuaternionNormalize(result);
+    // QuaternionNormalize(q);
+    // NOTE: Normalize to essentially nlerp the original and identity to 0.5
+    Quaternion q = result;
+    float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
+    if (length == 0.0f) length = 1.0f;
+    float ilength = 1.0f/length;
 
-    // Above lines are equivalent to:
-    //Quaternion result = QuaternionNlerp(q, QuaternionIdentity(), 0.5f);
+    result.x = q.x*ilength;
+    result.y = q.y*ilength;
+    result.z = q.z*ilength;
+    result.w = q.w*ilength;
 
     return result;
 }
@@ -1413,7 +1677,10 @@ RMDEF Quaternion QuaternionFromMatrix(Matrix mat)
 // Get a matrix for a given quaternion
 RMDEF Matrix QuaternionToMatrix(Quaternion q)
 {
-    Matrix result = MatrixIdentity();
+    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 a2 = q.x*q.x;
     float b2 = q.y*q.y;
@@ -1445,13 +1712,24 @@ RMDEF Matrix QuaternionToMatrix(Quaternion q)
 RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
 {
     Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
+
     float axisLength = sqrtf(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
 
     if (axisLength != 0.0f)
     {
         angle *= 0.5f;
 
-        axis = Vector3Normalize(axis);
+        float length = 0.0f;
+        float ilength = 0.0f;
+
+        // Vector3Normalize(axis)
+        Vector3 v = axis;
+        length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
+        if (length == 0.0f) length = 1.0f;
+        ilength = 1.0f/length;
+        axis.x *= ilength;
+        axis.y *= ilength;
+        axis.z *= ilength;
 
         float sinres = sinf(angle);
         float cosres = cosf(angle);
@@ -1461,7 +1739,15 @@ RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
         result.z = axis.z*sinres;
         result.w = cosres;
 
-        result = QuaternionNormalize(result);
+        // QuaternionNormalize(q);
+        Quaternion q = result;
+        length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
+        if (length == 0.0f) length = 1.0f;
+        ilength = 1.0f/length;
+        result.x = q.x*ilength;
+        result.y = q.y*ilength;
+        result.z = q.z*ilength;
+        result.w = q.w*ilength;
     }
 
     return result;
@@ -1470,7 +1756,18 @@ RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
 // Get the rotation angle and axis for a given quaternion
 RMDEF void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle)
 {
-    if (fabs(q.w) > 1.0f) q = QuaternionNormalize(q);
+    if (fabs(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);
+        if (length == 0.0f) length = 1.0f;
+        float ilength = 1.0f/length;
+
+        q.x = q.x*ilength;
+        q.y = q.y*ilength;
+        q.z = q.z*ilength;
+        q.w = q.w*ilength;
+    }
 
     Vector3 resAxis = { 0.0f, 0.0f, 0.0f };
     float resAngle = 2.0f*acosf(q.w);
@@ -1497,7 +1794,7 @@ RMDEF void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle
 // NOTE: Rotation order is ZYX
 RMDEF Quaternion QuaternionFromEuler(float pitch, float yaw, float roll)
 {
-    Quaternion q = { 0 };
+    Quaternion result = { 0 };
 
     float x0 = cosf(pitch*0.5f);
     float x1 = sinf(pitch*0.5f);
@@ -1506,35 +1803,35 @@ RMDEF Quaternion QuaternionFromEuler(float pitch, float yaw, float roll)
     float z0 = cosf(roll*0.5f);
     float z1 = sinf(roll*0.5f);
 
-    q.x = x1*y0*z0 - x0*y1*z1;
-    q.y = x0*y1*z0 + x1*y0*z1;
-    q.z = x0*y0*z1 - x1*y1*z0;
-    q.w = x0*y0*z0 + x1*y1*z1;
+    result.x = x1*y0*z0 - x0*y1*z1;
+    result.y = x0*y1*z0 + x1*y0*z1;
+    result.z = x0*y0*z1 - x1*y1*z0;
+    result.w = x0*y0*z0 + x1*y1*z1;
 
-    return q;
+    return result;
 }
 
 // Get the Euler angles equivalent to quaternion (roll, pitch, yaw)
-// NOTE: Angles are returned in a Vector3 struct in degrees
+// NOTE: Angles are returned in a Vector3 struct in radians
 RMDEF Vector3 QuaternionToEuler(Quaternion q)
 {
     Vector3 result = { 0 };
 
-    // roll (x-axis rotation)
+    // Roll (x-axis rotation)
     float x0 = 2.0f*(q.w*q.x + q.y*q.z);
     float x1 = 1.0f - 2.0f*(q.x*q.x + q.y*q.y);
-    result.x = atan2f(x0, x1)*RAD2DEG;
+    result.x = atan2f(x0, x1);
 
-    // pitch (y-axis rotation)
+    // Pitch (y-axis rotation)
     float y0 = 2.0f*(q.w*q.y - q.z*q.x);
     y0 = y0 > 1.0f ? 1.0f : y0;
     y0 = y0 < -1.0f ? -1.0f : y0;
-    result.y = asinf(y0)*RAD2DEG;
+    result.y = asinf(y0);
 
-    // yaw (z-axis rotation)
+    // Yaw (z-axis rotation)
     float z0 = 2.0f*(q.w*q.z + q.x*q.y);
     float z1 = 1.0f - 2.0f*(q.y*q.y + q.z*q.z);
-    result.z = atan2f(z0, z1)*RAD2DEG;
+    result.z = atan2f(z0, z1);
 
     return result;
 }
@@ -1552,27 +1849,4 @@ RMDEF Quaternion QuaternionTransform(Quaternion q, Matrix mat)
     return result;
 }
 
-// Projects a Vector3 from screen space into object space
-RMDEF Vector3 Vector3Unproject(Vector3 source, Matrix projection, Matrix view)
-{
-    Vector3 result = { 0.0f, 0.0f, 0.0f };
-
-    // Calculate unproject matrix (multiply view patrix by projection matrix) and invert it
-    Matrix matViewProj = MatrixMultiply(view, projection);
-    matViewProj = MatrixInvert(matViewProj);
-
-    // Create quaternion from source point
-    Quaternion quat = { source.x, source.y, source.z, 1.0f };
-
-    // Multiply quat point by unproject matrix
-    quat = QuaternionTransform(quat, matViewProj);
-
-    // Normalized world points in vectors
-    result.x = quat.x/quat.w;
-    result.y = quat.y/quat.w;
-    result.z = quat.z/quat.w;
-
-    return result;
-}
-
 #endif  // RAYMATH_H