logic/tracker: fix translation compensation

2 files changed, 33 insertions, 29 deletions

diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index f67c26b0..a1f206a1 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -61,14 +61,14 @@ double Tracker::map(double pos, Mapping& axis)
     return fc.getValue(pos);
 }
 
-void Tracker::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& output, bool rz)
+void Tracker::t_compensate(const rmat& rmat, const euler_t& xyz_, euler_t& output, bool rz)
 {
     // TY is really yaw axis. need swapping accordingly.
-    const dmat<3, 1> ret = rmat * xyz;
+    const euler_t ret = rmat * euler_t(xyz_(TZ), -xyz_(TX), -xyz_(TY));
     if (!rz)
         output(2) = ret(0);
     else
-        output(2) = xyz(2);
+        output(2) = xyz_(2);
     output(1) = -ret(2);
     output(0) = -ret(1);
 }
@@ -148,7 +148,7 @@ void Tracker::logic()
     euler_t t(value(0), value(1), value(2));
 
     bool do_center_now = false;
-    const bool nan = is_nan(r, t);
+    bool nan = is_nan(r, t);
 
     if (centerp && !nan)
     {
@@ -176,23 +176,22 @@ void Tracker::logic()
     }
 
     {
-        rmat m_;
         switch (s.center_method)
         {
         // inertial
         case 0:
         default:
-            m_ = r * r_b;
+            r = r * r_b;
             break;
         // camera
         case 1:
             euler_t degs = rmat_to_euler(r_b * r);
             degs(2) = rmat_to_euler(r * r_b)(2);
-            m_ = euler_to_rmat(degs);
+            r = euler_to_rmat(degs);
             break;
         }
 
-        const euler_t euler = r2d * rmat_to_euler(m_);
+        const euler_t euler = r2d * rmat_to_euler(r);
 
         euler_t tmp(t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2]);
 
@@ -208,11 +207,10 @@ void Tracker::logic()
         }
     }
 
-    bool nan_ = false;
     // whenever something can corrupt its internal state due to nan/inf, elide the call
     if (is_nan(value))
     {
-        nan_ = true;
+        nan = true;
     }
     else
     {
@@ -223,37 +221,42 @@ void Tracker::logic()
                 libs.pFilter->filter(tmp, value);
         }
 
-        for (int i = 0; i < 6; i++)
+        // CAVEAT rotation only, due to tcomp
+        for (int i = 3; i < 6; i++)
             value(i) = map(value(i), m(i));
 
-        if (s.tcomp_p)
-        {
-            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 < 3; i++)
-                value(i) = value_(i);
-        }
-
         for (int i = 0; i < 6; i++)
             value(i) += m(i).opts.zero;
 
         for (int i = 0; i < 6; i++)
-            value(i) *= inverts[i] ? -1. : 1.;
+            value(i) *= int(inverts[i]) * -2 + 1;
 
         if (zero_)
             for (int i = 0; i < 6; i++)
                 value(i) = 0;
 
         if (is_nan(value))
-            nan_ = true;
+            nan = true;
+    }
+
+    if (s.tcomp_p)
+    {
+        euler_t value_(value(TX), value(TY), value(TZ));
+        t_compensate(euler_to_rmat(euler_t(value(Yaw) * d2r, value(Pitch) * d2r, value(Roll) * d2r)),
+                     value_,
+                     value_,
+                     s.tcomp_tz);
+        if (is_nan(r, value_))
+            nan = true;
+        for (int i = 0; i < 3; i++)
+            value(i) = value_(i);
     }
 
-    if (nan_)
+    // CAVEAT translation only, due to tcomp
+    for (int i = 0; i < 3; i++)
+        value(i) = map(value(i), m(i));
+
+    if (nan)
     {
         value = last_mapped;
 
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index 96fac439..a6752d14 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -36,6 +36,7 @@ private:
     double axes[6];
 public:
     Pose() : axes {0,0,0, 0,0,0} {}
+    Pose(double x, double y, double z, double yaw, double pitch, double roll) : axes { x, y, z, yaw, pitch, roll } {}
 
     inline operator double*() { return axes; }
     inline operator const double*() const { return axes; }
@@ -74,8 +75,8 @@ private:
     void run() override;
 
     static constexpr double pi = OPENTRACK_PI;
-    static constexpr double r2d = 180. / pi;
-    static constexpr double d2r = pi / 180.;
+    static constexpr double r2d = 180. / OPENTRACK_PI;
+    static constexpr double d2r = OPENTRACK_PI / 180.;
 public:
     Tracker(Mappings& m, SelectedLibraries& libs);
     ~Tracker();