move rmat->euler and euler->rmat conversions to header

3 files changed, 67 insertions, 63 deletions

diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index b36b21ba..447327a8 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -75,7 +75,63 @@ struct Mat
 
         return ret;
     }
+    
+    template<int h_, int w_> using dmat = Mat<double, h_, w_>;
+    
+    // http://stackoverflow.com/a/18436193
+    static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R)
+    {
+        static constexpr double pi = 3.141592653;
+        const double up = 90 * pi / 180.;
+        static constexpr double bound = 1. - 2e-4;
+        if (R(0, 2) > bound)
+        {
+            double roll = atan(R(1, 0) / R(2, 0));
+            return dmat<3, 1>({0., up, roll});
+        }
+        if (R(0, 2) < -bound)
+        {
+            double roll = atan(R(1, 0) / R(2, 0));
+            return dmat<3, 1>({0., -up, roll});
+        }
+        double pitch = asin(-R(0, 2));
+        double roll = atan2(R(1, 2), R(2, 2));
+        double yaw = atan2(R(0, 1), R(0, 0));
+        return dmat<3, 1>({yaw, pitch, roll});
+    }
+    
+    // tait-bryan angles, not euler
+    static dmat<3, 3> euler_to_rmat(const double* input)
+    {
+        static constexpr double pi = 3.141592653;
+        auto H = input[0] * pi / 180;
+        auto P = input[1] * pi / 180;
+        auto B = input[2] * pi / 180;
+    
+        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);
+    
+        double foo[] = {
+            // 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
+        };
+    
+        return dmat<3, 3>(foo);
+    }
 };
 
 template<int h, int w> using dmat = Mat<double, h, w>;
-
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index a6433da5..f621083b 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -49,62 +49,7 @@ double Tracker::map(double pos, bool invertp, Mapping& axis)
     return fc.getValue(pos) + axis.opts.zero;
 }
 
-// http://stackoverflow.com/a/18436193
-static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R)
-{
-    static constexpr double pi = 3.141592653;
-    const double up = 90 * pi / 180.;
-    static constexpr double bound = 1. - 2e-4;
-    if (R(0, 2) > bound)
-    {
-        double roll = atan(R(1, 0) / R(2, 0));
-        return dmat<3, 1>({0., up, roll});
-    }
-    if (R(0, 2) < -bound)
-    {
-        double roll = atan(R(1, 0) / R(2, 0));
-        return dmat<3, 1>({0., -up, roll});
-    }
-    double pitch = asin(-R(0, 2));
-    double roll = atan2(R(1, 2), R(2, 2));
-    double yaw = atan2(R(0, 1), R(0, 0));
-    return dmat<3, 1>({yaw, pitch, roll});
-}
-
-// tait-bryan angles, not euler
-static dmat<3, 3> euler_to_rmat(const double* input)
-{
-    static constexpr double pi = 3.141592653;
-    auto H = input[0] * pi / 180;
-    auto P = input[1] * pi / 180;
-    auto B = input[2] * pi / 180;
-
-    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);
-
-    double foo[] = {
-        // 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
-    };
-
-    return dmat<3, 3>(foo);
-}
-
-void Tracker::t_compensate(const dmat<3, 3>& rmat, const double* xyz, double* output, bool rz)
+void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output, bool rz)
 {
     // TY is really yaw axis. need swapping accordingly.
     dmat<3, 1> tvec({ xyz[2], -xyz[0], -xyz[1] });
@@ -141,7 +86,7 @@ void Tracker::logic()
         }
     else
     {
-        auto mat = rmat_to_euler(r_b);
+        auto mat = rmat::rmat_to_euler(r_b);
         
         for (int i = 0; i < 3; i++)
         {
@@ -155,13 +100,13 @@ void Tracker::logic()
         centerp = false;
         for (int i = 0; i < 3; i++)
             t_b[i] = value(i);
-        r_b = euler_to_rmat(&value[Yaw]);
+        r_b = rmat::euler_to_rmat(&value[Yaw]);
     }
     
     {
-        const dmat<3, 3> rmat = euler_to_rmat(&value[Yaw]);
-        const dmat<3, 3> m_ = rmat * r_b.t();
-        const dmat<3, 1> euler = rmat_to_euler(m_);
+        const rmat r = rmat::euler_to_rmat(&value[Yaw]);
+        const rmat m_ = r * r_b.t();
+        const dmat<3, 1> euler = rmat::rmat_to_euler(m_);
         for (int i = 0; i < 3; i++)
         {
             value(i) -= t_b[i];
@@ -184,7 +129,7 @@ void Tracker::logic()
         value[i] *= inverts[i] ? -1. : 1.;
 
     if (s.tcomp_p)
-        t_compensate(euler_to_rmat(&value[Yaw]),
+        t_compensate(rmat::euler_to_rmat(&value[Yaw]),
                      value,
                      value,
                      s.tcomp_tz);
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index f7f69a7b..58eb69dd 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -32,6 +32,8 @@ private:
     volatile bool should_quit;
     SelectedLibraries const& libs;
 
+    using rmat = dmat<3, 3>;
+    
     dmat<3, 3> r_b;
     double t_b[3];
 
@@ -40,6 +42,7 @@ private:
 
     void t_compensate(const dmat<3, 3>& rmat, const double* ypr, double* output, bool rz);
     void run() override;
+    
 public:
     Tracker(main_settings& s, Mappings& m, SelectedLibraries& libs);
     ~Tracker();