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; }