diff --git a/Source/Core/Common/Matrix.cpp b/Source/Core/Common/Matrix.cpp index 9581a5a4ce..6ccfbe459c 100644 --- a/Source/Core/Common/Matrix.cpp +++ b/Source/Core/Common/Matrix.cpp @@ -45,6 +45,22 @@ Matrix33 Matrix33::Identity() return mtx; } +Matrix33 Matrix33::FromQuaternion(float qx, float qy, float qz, float qw) +{ + // Normalize. + const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); + qx *= n; + qy *= n; + qz *= n; + qw *= n; + + return { + 1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw, + 2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz, 2 * qy * qz - 2 * qx * qw, + 2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx * qx - 2 * qy * qy, + }; +} + Matrix33 Matrix33::RotateX(float rad) { const float s = std::sin(rad); diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h index ed58abdccb..b0ec811545 100644 --- a/Source/Core/Common/Matrix.h +++ b/Source/Core/Common/Matrix.h @@ -273,6 +273,7 @@ class Matrix33 { public: static Matrix33 Identity(); + static Matrix33 FromQuaternion(float x, float y, float z, float w); // Return a rotation matrix around the x,y,z axis static Matrix33 RotateX(float rad);