diff options
| -rw-r--r-- | opentrack/simple-mat.cpp | 41 | ||||
| -rw-r--r-- | opentrack/simple-mat.hpp | 3 |
2 files changed, 19 insertions, 25 deletions
diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp index ed372299..ad83a508 100644 --- a/opentrack/simple-mat.cpp +++ b/opentrack/simple-mat.cpp @@ -1,40 +1,35 @@ #include "simple-mat.hpp" #include "opentrack-compat/pi-constant.hpp" +#include <cmath> namespace euler { -static constexpr double pi = OPENTRACK_PI; - euler_t rmat_to_euler(const dmat<3, 3>& R) { - const double pitch_1 = asin(-R(0, 2)); - const double pitch_2 = pi - pitch_1; - const double cos_p1 = cos(pitch_1), cos_p2 = cos(pitch_2); - const double roll_1 = atan2(R(1, 2) / cos_p1, R(2, 2) / cos_p1); - const double roll_2 = atan2(R(1, 2) / cos_p2, R(2, 2) / cos_p2); - const double yaw_1 = atan2(R(0, 1) / cos_p1, R(0, 0) / cos_p1); - const double yaw_2 = atan2(R(0, 1) / cos_p2, R(0, 0) / cos_p2); - - using std::fabs; - - const double err1 = fabs(pitch_1) + fabs(roll_1) + fabs(yaw_1); - const double err2 = fabs(pitch_2) + fabs(roll_2) + fabs(yaw_2); - - if (err1 > err2) - { - bool fix_neg_pitch = pitch_1 < 0; - return euler_t(yaw_2, fix_neg_pitch ? -pi - pitch_1 : pitch_2, roll_2); - } + 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(2, 1), R(2, 2)), + atan2(R(2, 0), cy), + atan2(-R(1, 0), R(0, 0))); else - return euler_t(yaw_1, pitch_1, roll_1); + return euler_t(0., + atan2(R(2, 0), cy), + atan2(R(0, 1), R(1, 1))); } // tait-bryan angles, not euler -rmat euler_to_rmat(const euler_t input) +rmat euler_to_rmat(const euler_t& input) { const double H = input(0); const double P = input(1); - const double B = input(2); + const double B = -input(2); + + using std::cos; + using std::sin; const auto c1 = cos(H); const auto s1 = sin(H); diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp index 93d0d0c9..f1ad0717 100644 --- a/opentrack/simple-mat.hpp +++ b/opentrack/simple-mat.hpp @@ -12,7 +12,6 @@ #include <initializer_list> #include <type_traits> -#include <cmath> #include <utility> namespace { @@ -268,7 +267,7 @@ template<int y, int x> using dmat = Mat<double, y, x>; using rmat = dmat<3, 3>; using euler_t = dmat<3, 1>; -rmat OPENTRACK_API_EXPORT euler_to_rmat(const euler_t input); +OPENTRACK_API_EXPORT rmat euler_to_rmat(const euler_t& input); // http://stackoverflow.com/a/18436193 euler_t OPENTRACK_API_EXPORT rmat_to_euler(const dmat<3, 3>& R); |