logic/tracker, api/simple-mat: don't convert rad <-> deg all the time

4 files changed, 32 insertions, 13 deletions

diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index 3fb5c6fc..6557b061 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -110,6 +110,7 @@ void Tracker::logic()
 
     static constexpr double pi = 3.141592653;
     static constexpr double r2d = 180. / pi;
+    static constexpr double d2r = pi / 180.;
 
     using namespace euler;
 
@@ -131,12 +132,17 @@ void Tracker::logic()
 
     const double off[] =
     {
-        (double)-s.camera_yaw,
-        (double)-s.camera_pitch,
-        (double)-s.camera_roll
+        d2r * (double)-s.camera_yaw,
+        d2r * (double)-s.camera_pitch,
+        d2r * (double)-s.camera_roll
     };
     const rmat cam = euler_to_rmat(off);
-    rmat r = euler_to_rmat(&value[Yaw]);
+    rmat r;
+    {
+        euler_t tmp(&value[Yaw]);
+        tmp = d2r * tmp;
+        r = euler_to_rmat(&tmp[0]);
+    }
     euler_t t(value(0), value(1), value(2));
 
     r = cam * r;
@@ -179,12 +185,12 @@ void Tracker::logic()
             m_ = r_b.t() * r;
         }
 
-        const euler_t euler = rmat_to_euler(m_);
+        const euler_t euler = r2d * rmat_to_euler(m_);
 
         for (int i = 0; i < 3; i++)
         {
             value(i) = tmp[i];
-            value(i+3) = euler(i) * r2d;
+            value(i+3) = euler(i);
         }
     }
 
@@ -207,10 +213,15 @@ void Tracker::logic()
             value(i) = map(value(i), m(i));
 
         if (s.tcomp_p)
-            t_compensate(euler_to_rmat(&value[Yaw]),
+        {
+            euler_t value_(value(Yaw) * d2r,
+                           value(Pitch) * d2r,
+                           value(Roll) * d2r);
+            t_compensate(euler_to_rmat(&value_[0]),
                          value,
                          value,
                          s.tcomp_tz);
+        }
 
         for (int i = 0; i < 6; i++)
             value(i) += m(i).opts.zero;
@@ -266,9 +277,10 @@ void Tracker::run()
 
         logic();
 
-        long q = sleep_ms * 1000L - t.elapsed()/1000L;
+        long q = long(sleep_ms * 1000L - t.elapsed()/1000L);
         using std::max;
-        usleep(max(1L, q));
+        using ulong = unsigned long;
+        usleep(ulong(max(1L, q)));
     }
 
     {
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index dad955c8..1c29fb8d 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -61,7 +61,8 @@ private:
     volatile bool should_quit;
     SelectedLibraries const& libs;
 
-    using rmat = dmat<3, 3>;
+    using rmat = euler::rmat;
+    using euler_t = euler::euler_t;
 
     rmat r_b;
     double t_b[3];
diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp
index 4ab68e0f..367cf365 100644
--- a/opentrack/simple-mat.cpp
+++ b/opentrack/simple-mat.cpp
@@ -36,9 +36,9 @@ euler_t rmat_to_euler(const dmat<3, 3>& R)
 // tait-bryan angles, not euler
 rmat euler_to_rmat(const double* input)
 {
-    auto H = input[0] * pi / 180;
-    auto P = input[1] * pi / 180;
-    auto B = input[2] * pi / 180;
+    const double H = input[0];
+    const double P = input[1];
+    const double B = input[2];
 
     const auto c1 = cos(H);
     const auto s1 = sin(H);
diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index f5680ec4..8dc03014 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -253,3 +253,9 @@ euler_t OPENTRACK_API_EXPORT rmat_to_euler(const dmat<3, 3>& R);
 } // end ns euler
 
 template<int h_, int w_> using dmat = Mat<double, h_, w_>;
+
+template<typename num, int h, int w>
+inline Mat<num, h, w> operator*(num scalar, const Mat<num, h, w>& mat)
+{
+    return mat * scalar;
+}