diff options
| -rw-r--r-- | compat/euler.cpp | 96 | ||||
| -rw-r--r-- | compat/euler.hpp | 25 | ||||
| -rw-r--r-- | compat/simple-mat.cpp | 95 | ||||
| -rw-r--r-- | compat/simple-mat.hpp | 17 |
4 files changed, 121 insertions, 112 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp new file mode 100644 index 00000000..a1034bc9 --- /dev/null +++ b/compat/euler.cpp @@ -0,0 +1,96 @@ +#include "euler.hpp" +#include <cmath> + +namespace euler { + +euler_t OPENTRACK_COMPAT_EXPORT rmat_to_euler(const rmat& R) +{ + using std::atan2; + using std::sqrt; + + const double cy = sqrt(R(2,2)*R(2, 2) + R(2, 1)*R(2, 1)); + const bool large_enough = cy > 1e-10; + if (large_enough) + return euler_t(atan2(-R(1, 0), R(0, 0)), + atan2(R(2, 0), cy), + atan2(-R(2, 1), R(2, 2))); + else + return euler_t(atan2(R(0, 1), R(1, 1)), + atan2(R(2, 0), cy), + 0); +} + +// tait-bryan angles, not euler +rmat OPENTRACK_COMPAT_EXPORT euler_to_rmat(const euler_t& input) +{ + const double H = -input(0); + const double P = -input(1); + const double B = -input(2); + + using std::cos; + using std::sin; + + const auto c1 = cos(H); + const auto s1 = sin(H); + const auto c2 = cos(P); + const auto s2 = sin(P); + const auto c3 = cos(B); + const auto s3 = sin(B); + + return rmat( + // z + c1 * c2, + c1 * s2 * s3 - c3 * s1, + s1 * s3 + c1 * c3 * s2, + // y + c2 * s1, + c1 * c3 + s1 * s2 * s3, + c3 * s1 * s2 - c1 * s3, + // x + -s2, + c2 * s3, + c2 * c3 + ); +} + +// https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations +void OPENTRACK_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, + rmat& r_roll, + rmat& r_pitch, + rmat& r_yaw) +{ + using std::cos; + using std::sin; + + { + const double phi = -input(2); + const double sin_phi = sin(phi); + const double cos_phi = cos(phi); + + r_roll = rmat(1, 0, 0, + 0, cos_phi, -sin_phi, + 0, sin_phi, cos_phi); + } + + { + const double theta = input(1); + const double sin_theta = sin(theta); + const double cos_theta = cos(theta); + + r_pitch = rmat(cos_theta, 0, -sin_theta, + 0, 1, 0, + sin_theta, 0, cos_theta); + } + + { + const double psi = -input(0); + const double sin_psi = sin(psi); + const double cos_psi = cos(psi); + + r_yaw = rmat(cos_psi, -sin_psi, 0, + sin_psi, cos_psi, 0, + 0, 0, 1); + } +} + +} // end ns euler diff --git a/compat/euler.hpp b/compat/euler.hpp new file mode 100644 index 00000000..6a1ea1a4 --- /dev/null +++ b/compat/euler.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include "export.hpp" +#include "simple-mat.hpp" + +namespace euler { + +template<int h_, int w_> using dmat = Mat<double, h_, w_>; + +using dvec2 = Mat<double, 2, 1>; +using dvec3 = Mat<double, 3, 1>; + +using rmat = dmat<3, 3>; +using euler_t = dmat<3, 1>; + +rmat OPENTRACK_COMPAT_EXPORT euler_to_rmat(const euler_t& input); + +euler_t OPENTRACK_COMPAT_EXPORT rmat_to_euler(const rmat& R); + +void OPENTRACK_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, + rmat& r_roll, + rmat& r_pitch, + rmat& r_yaw); + +} // end ns euler diff --git a/compat/simple-mat.cpp b/compat/simple-mat.cpp index c9af9634..5d22fee6 100644 --- a/compat/simple-mat.cpp +++ b/compat/simple-mat.cpp @@ -1,96 +1 @@ #include "simple-mat.hpp" -#include <cmath> - -namespace euler { - -euler_t OPENTRACK_COMPAT_EXPORT rmat_to_euler(const rmat& R) -{ - using std::atan2; - using std::sqrt; - - const double cy = sqrt(R(2,2)*R(2, 2) + R(2, 1)*R(2, 1)); - const bool large_enough = cy > 1e-10; - if (large_enough) - return euler_t(atan2(-R(1, 0), R(0, 0)), - atan2(R(2, 0), cy), - atan2(-R(2, 1), R(2, 2))); - else - return euler_t(atan2(R(0, 1), R(1, 1)), - atan2(R(2, 0), cy), - 0); -} - -// tait-bryan angles, not euler -rmat OPENTRACK_COMPAT_EXPORT euler_to_rmat(const euler_t& input) -{ - const double H = -input(0); - const double P = -input(1); - const double B = -input(2); - - using std::cos; - using std::sin; - - const auto c1 = cos(H); - const auto s1 = sin(H); - const auto c2 = cos(P); - const auto s2 = sin(P); - const auto c3 = cos(B); - const auto s3 = sin(B); - - return rmat( - // z - c1 * c2, - c1 * s2 * s3 - c3 * s1, - s1 * s3 + c1 * c3 * s2, - // y - c2 * s1, - c1 * c3 + s1 * s2 * s3, - c3 * s1 * s2 - c1 * s3, - // x - -s2, - c2 * s3, - c2 * c3 - ); -} - -// https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations -void OPENTRACK_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, - rmat& r_roll, - rmat& r_pitch, - rmat& r_yaw) -{ - using std::cos; - using std::sin; - - { - const double phi = -input(2); - const double sin_phi = sin(phi); - const double cos_phi = cos(phi); - - r_roll = rmat(1, 0, 0, - 0, cos_phi, -sin_phi, - 0, sin_phi, cos_phi); - } - - { - const double theta = input(1); - const double sin_theta = sin(theta); - const double cos_theta = cos(theta); - - r_pitch = rmat(cos_theta, 0, -sin_theta, - 0, 1, 0, - sin_theta, 0, cos_theta); - } - - { - const double psi = -input(0); - const double sin_psi = sin(psi); - const double cos_psi = cos(psi); - - r_yaw = rmat(cos_psi, -sin_psi, 0, - sin_psi, cos_psi, 0, - 0, 0, 1); - } -} - -} // end ns euler diff --git a/compat/simple-mat.hpp b/compat/simple-mat.hpp index 9739be7f..a1509f4d 100644 --- a/compat/simple-mat.hpp +++ b/compat/simple-mat.hpp @@ -258,8 +258,6 @@ public: } }; -template<int h_, int w_> using dmat = Mat<double, h_, w_>; - template<typename num, int h, int w> Mat<num, h, w> operator*(num scalar, const Mat<num, h, w>& mat) { @@ -276,18 +274,3 @@ Mat<num, h_, w_> operator*(const Mat<num, h_, w_>& self, num other) return ret; } -namespace euler { - -using rmat = dmat<3, 3>; -using euler_t = dmat<3, 1>; - -rmat OPENTRACK_COMPAT_EXPORT euler_to_rmat(const euler_t& input); - -euler_t OPENTRACK_COMPAT_EXPORT rmat_to_euler(const rmat& R); - -void OPENTRACK_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, - rmat& r_roll, - rmat& r_pitch, - rmat& r_yaw); - -} // end ns euler |