diff --git a/matrix.cpp b/matrix.cpp index b65d305e..c0099bce 100644 --- a/matrix.cpp +++ b/matrix.cpp @@ -391,6 +391,37 @@ template struct util::matrix<4,float>; template struct util::matrix<4,double>; +/////////////////////////////////////////////////////////////////////////////// +// Uses the algorithm from: +// "Extracting Euler Angles from a Rotation Matrix" by +// Mike Day, Insomniac Games. +template +util::vector<3,T> +util::to_euler (const matrix &m) +{ + static_assert (S == 3 || S == 4, "only defined for 3d affine transforms"); + + const auto theta0 = std::atan2 (m[2][1], m[2][2]); + + const auto c1 = std::hypot (m[0][0], m[1][0]); + const auto theta1 = std::atan2 (-m[2][0], c1); + + const auto s0 = std::sin(theta0); + const auto c0 = std::cos(theta0); + const auto theta2 = std::atan2( + s0 * m[0][2] - c0 * m[0][1], + c0 * m[1][1] - s0 * m[1][2] + ); + + return { theta0, theta1, theta2 }; +} + + +//----------------------------------------------------------------------------- +template util::vector<3,float> util::to_euler (const matrix<3,float>&); +template util::vector<3,float> util::to_euler (const matrix<4,float>&); + + /////////////////////////////////////////////////////////////////////////////// template std::ostream& diff --git a/matrix.hpp b/matrix.hpp index 2f1903a6..3fc1143a 100644 --- a/matrix.hpp +++ b/matrix.hpp @@ -83,6 +83,16 @@ namespace util { }; + /////////////////////////////////////////////////////////////////////////// + // Convert an affine rotation matrix to euler angles. + // + // Results are undefined if the matrix is not purely a rotation matrix, + // or if the dimension is not 3x3 or 4x4. + template + vector<3,T> + to_euler (const matrix&); + + /////////////////////////////////////////////////////////////////////////// // logical operations template @@ -152,6 +162,8 @@ namespace util { T sum (const matrix&); + + /////////////////////////////////////////////////////////////////////////// template using matrix3 = matrix<3,T>; template using matrix4 = matrix<4,T>; @@ -167,6 +179,8 @@ namespace util { typedef matrix<4,float> matrix4f; typedef matrix<4,double> matrix4d; + + /////////////////////////////////////////////////////////////////////////// template std::ostream& operator<< (std::ostream&, const matrix&); }