REVIEWED: MatrixRotateZYX() #1642

1 files changed, 28 insertions, 5 deletions

diff --git a/src/raymath.h b/src/raymath.h
index 0e3b6042..e4aedb51 100644
--- a/src/raymath.h
+++ b/src/raymath.h
@@ -970,13 +970,36 @@ RMDEF Matrix MatrixRotateXYZ(Vector3 ang)
 }
 
 // Returns zyx-rotation matrix (angles in radians)
-// TODO: This solution is suboptimal, it should be possible to create this matrix in one go
-// instead of using a 3 matrix multiplication
 RMDEF Matrix MatrixRotateZYX(Vector3 ang)
 {
-    Matrix result = MatrixRotateZ(ang.z);
-    result = MatrixMultiply(result, MatrixRotateY(ang.y));
-    result = MatrixMultiply(result, MatrixRotateX(ang.x));
+    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);
+
+    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.m12 = 0;
+    result.m13 = 0;
+    result.m14 = 0;
+    result.m15 = 1;
 
     return result;
 }