api/simple-mat: use correct rmat -> euler formula

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