logic/pipeline: conditionalize

Will activate only when looking down or backward.

When activating or deactivating, will slowly slide into the new
position, no instant movement involved.

Issue: #712

2 files changed, 174 insertions, 88 deletions

diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 7d073b28..43cc74c7 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -27,37 +27,16 @@
 #endif
 
 using namespace euler;
-using namespace gui_tracker_impl;
 using namespace time_units;
+using namespace gui_tracker_impl;
 
-constexpr double pipeline::r2d;
-constexpr double pipeline::d2r;
+static constexpr inline double r2d = 180. / M_PI;
+static constexpr inline double d2r = M_PI / 180.;
 
-pipeline::pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger) :
-    m(m),
-    ev(ev),
-    libs(libs),
-    logger(logger)
-{
-}
+reltrans::reltrans() {}
 
-pipeline::~pipeline()
-{
-    requestInterruption();
-    wait();
-}
-
-double pipeline::map(double pos, Map& axis)
-{
-    bool altp = (pos < 0) && axis.opts.altp;
-    axis.spline_main.set_tracking_active( !altp );
-    axis.spline_alt.set_tracking_active( altp );
-    auto& fc = altp ? axis.spline_alt : axis.spline_main;
-    return double(fc.get_value(pos));
-}
-
-void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& output,
-                            bool disable_tx, bool disable_ty, bool disable_tz)
+euler_t reltrans::rotate(const rmat& rmat, const euler_t& xyz,
+                         bool disable_tx, bool disable_ty, bool disable_tz) const
 {
     enum { tb_Z, tb_X, tb_Y };
 
@@ -65,6 +44,8 @@ void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& outpu
     // sign changes are due to right-vs-left handedness of coordinate system used
     const euler_t ret = rmat * euler_t(xyz(TZ), -xyz(TX), -xyz(TY));
 
+    euler_t output;
+
     if (disable_tz)
         output(TZ) = xyz(TZ);
     else
@@ -79,6 +60,112 @@ void pipeline::t_compensate(const rmat& rmat, const euler_t& xyz, euler_t& outpu
         output(TX) = xyz(TX);
     else
         output(TX) = -ret(tb_X);
+
+    return output;
+}
+
+Pose reltrans::apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6, 1>& disable)
+{
+    if (enable)
+    {
+        euler_t rel { value(TX), value(TY), value(TZ) };
+
+        {
+            const bool yaw_in_zone = std::fabs(value(Yaw)) > 135;
+            const bool pitch_in_zone = value(Pitch) < -30;
+            const bool tcomp_in_zone_ = yaw_in_zone || pitch_in_zone;
+
+            if (!tcomp_state && tcomp_in_zone != tcomp_in_zone_)
+            {
+                //qDebug() << "tcomp-interp: START";
+                tcomp_state = true;
+                tcomp_interp_timer.start();
+            }
+
+            tcomp_in_zone = tcomp_in_zone_;
+        }
+
+        // only when looking behind or downward
+        if (tcomp_in_zone)
+        {
+            const double tcomp_c[] = {
+                double(!disable(Yaw)),
+                double(!disable(Pitch)),
+                double(!disable(Roll)),
+            };
+
+            const rmat R = euler_to_rmat(euler_t(value(Yaw)   * d2r * tcomp_c[0],
+                                                 value(Pitch) * d2r * tcomp_c[1],
+                                                 value(Roll)  * d2r * tcomp_c[2]));
+
+            rel = rotate(R,
+                         rel,
+                         disable(TX),
+                         disable(TY),
+                         disable(TZ));
+        }
+
+        if (tcomp_state)
+        {
+            const double dt = tcomp_interp_timer.elapsed_seconds();
+            tcomp_interp_timer.start();
+
+            constexpr double RC = .1;
+            const double alpha = dt/(dt+RC);
+
+            constexpr double eps = .05;
+
+            tcomp_interp_pos = tcomp_interp_pos * (1-alpha) + rel * alpha;
+
+            const euler_t tmp = rel - tcomp_interp_pos;
+            rel = tcomp_interp_pos;
+            const double delta = std::fabs(tmp(0)) + std::fabs(tmp(0)) + std::fabs(tmp(0));
+
+            //qDebug() << "tcomp-interp: delta" << delta;
+
+            if (delta < eps)
+            {
+                //qDebug() << "tcomp-interp: STOP";
+                tcomp_state = false;
+            }
+        }
+        else
+        {
+            tcomp_interp_pos = rel;
+        }
+
+        return { rel(0), rel(1), rel(2), value(Yaw), value(Pitch), value(Roll) };
+    }
+    else
+    {
+        tcomp_state = false;
+        tcomp_in_zone = false;
+
+        return value;
+    }
+}
+
+pipeline::pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger) :
+    m(m),
+    ev(ev),
+    libs(libs),
+    logger(logger)
+{
+}
+
+pipeline::~pipeline()
+{
+    requestInterruption();
+    wait();
+}
+
+double pipeline::map(double pos, Map& axis)
+{
+    bool altp = (pos < 0) && axis.opts.altp;
+    axis.spline_main.set_tracking_active( !altp );
+    axis.spline_alt.set_tracking_active( altp );
+    auto& fc = altp ? axis.spline_alt : axis.spline_main;
+    return double(fc.get_value(pos));
 }
 
 template<int u, int w>
@@ -109,14 +196,20 @@ static inline bool nan_check_(const x& datum)
 template<typename>
 static bool nan_check_() = delete;
 
+static never_inline
+void emit_nan_check_msg(const char* text, int line)
+{
+    once_only(qDebug()  << "nan check failed"
+                        << "line:" << text
+                        << "for:" << line);
+}
+
 #define nan_check(...) \
     do                                                      \
     {                                                       \
         if (!nan_check_(__VA_ARGS__))                       \
         {                                                   \
-            once_only(qDebug()  << "nan check failed"       \
-                                << "line:" << __LINE__      \
-                                << "for:" << #__VA_ARGS__); \
+            emit_nan_check_msg(#__VA_ARGS__, __LINE__);     \
             goto nan;                                       \
         }                                                   \
     } while (false)
@@ -129,6 +222,7 @@ void pipeline::logic()
     logger.write_dt();
     logger.reset_dt();
 
+    // we must center prior to getting data
     const bool center_ordered = get(f_center) && tracking_started;
     set(f_center, false);
     const bool own_center_logic = center_ordered && libs.pTracker->center();
@@ -167,7 +261,7 @@ void pipeline::logic()
         using std::copysign;
         using std::fabs;
 
-        value(i) = std::fmod(value(i), 360);
+        value(i) = fmod(value(i), 360);
 
         const double x = value(i);
         if (fabs(x) - 1e-2 > 180)
@@ -188,10 +282,8 @@ void pipeline::logic()
 
     if (!tracking_started)
     {
-        using std::fabs;
-
         for (int i = 0; i < 6; i++)
-            if (fabs(newpose(i)) != 0)
+            if (std::fabs(newpose(i)) != 0)
             {
                 tracking_started = true;
                 break;
@@ -240,7 +332,7 @@ void pipeline::logic()
         case 1:
             // camera
             rotation = rotation * scaled_rotation.rot_center;
-            t_compensate(real_rotation.rot_center, pos, pos, false, false, false);
+            pos = rel.rotate(real_rotation.rot_center, pos, false, false, false);
         break;
         }
 
@@ -285,62 +377,41 @@ void pipeline::logic()
     {
         ev.run_events(EV::ev_before_mapping, value);
 
-        euler_t neck, rel;
-
-        if (s.neck_enable)
         {
-            double nz = -s.neck_z;
+            euler_t neck;
 
-            if (nz != 0)
+            if (s.neck_enable)
             {
-                const rmat R = euler_to_rmat(
-                       euler_t(value(Yaw)   * d2r,
-                               value(Pitch) * d2r,
-                               value(Roll)  * d2r));
-                euler_t xyz(0, 0, nz);
-                t_compensate(R, xyz, xyz, false, false, false);
-                neck(TX) = xyz(TX);
-                neck(TY) = xyz(TY);
-                neck(TZ) = xyz(TZ) - nz;
-
-                nan_check(Pose(0, 0, 0, neck(TX), neck(TY), neck(TZ)));
+                double nz = -s.neck_z;
+
+                if (nz != 0)
+                {
+                    const rmat R = euler_to_rmat(
+                                       euler_t(value(Yaw)   * d2r,
+                                               value(Pitch) * d2r,
+                                               value(Roll)  * d2r));
+                    neck = rel.rotate(R, { 0, 0, nz }, false, false, false);
+                    neck(TZ) = neck(TZ) - nz;
+
+                    nan_check(neck);
+                }
             }
-        }
 
-        // CAVEAT rotation only, due to tcomp
-        for (int i = 3; i < 6; i++)
-            value(i) = map(value(i), m(i));
+            // CAVEAT rotation only, due to tcomp
+            for (int i = 3; i < 6; i++)
+                value(i) = map(value(i), m(i));
 
-        nan_check(value);
+            nan_check(value);
 
-        if (s.tcomp_p)
-        {
-            const double tcomp_c[] =
-            {
-                double(!s.tcomp_disable_src_yaw),
-                double(!s.tcomp_disable_src_pitch),
-                double(!s.tcomp_disable_src_roll),
-            };
-            const rmat R = euler_to_rmat(
-                       euler_t(value(Yaw)   * d2r * tcomp_c[0],
-                               value(Pitch) * d2r * tcomp_c[1],
-                               value(Roll)  * d2r * tcomp_c[2]));
-            euler_t ret;
-            t_compensate(R,
-                         euler_t(value(TX), value(TY), value(TZ)),
-                         ret,
-                         s.tcomp_disable_tx,
-                         s.tcomp_disable_ty,
-                         s.tcomp_disable_tz);
+            value = rel.apply_pipeline(s.tcomp_p, value, {
+                !!s.tcomp_disable_src_yaw, !!s.tcomp_disable_src_pitch, !!s.tcomp_disable_src_roll,
+                !!s.tcomp_disable_tx, !!s.tcomp_disable_ty, !!s.tcomp_disable_tz
+            });
 
             for (int i = 0; i < 3; i++)
-                rel(i) = ret(i) - value(i);
+                value(i) += neck(i);
         }
 
-        // don't t_compensate existing compensated values
-        for (int i = 0; i < 3; i++)
-            value(i) += neck(i) + rel(i);
-
         // relative translation can move it
         for (unsigned k = 0; k < 6; k++)
             if (disabled[k])
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 0cb828e7..dc04d44b 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -32,6 +32,27 @@
 
 namespace gui_tracker_impl {
 
+using rmat = euler::rmat;
+using euler_t = euler::euler_t;
+
+class reltrans
+{
+    euler_t tcomp_interp_pos, tcomp_last_value;
+    Timer tcomp_interp_timer;
+    bool tcomp_state = false;
+    bool tcomp_in_zone = false;
+
+public:
+    reltrans();
+
+    warn_result_unused
+    euler_t rotate(const rmat& rmat, const euler_t& xyz,
+                   bool disable_tx, bool disable_ty, bool disable_tz) const;
+
+    warn_result_unused
+    Pose apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6, 1>& disable);
+};
+
 using namespace time_units;
 
 struct OTR_LOGIC_EXPORT bits
@@ -55,9 +76,6 @@ class OTR_LOGIC_EXPORT pipeline : private QThread, private bits
 {
     Q_OBJECT
 private:
-    using rmat = euler::rmat;
-    using euler_t = euler::euler_t;
-
     QMutex mtx;
     main_settings s;
     Mappings& m;
@@ -82,6 +100,8 @@ private:
         {}
     };
 
+    reltrans rel;
+
     state real_rotation, scaled_rotation;
     euler_t t_center;
 
@@ -91,13 +111,8 @@ private:
 
     double map(double pos, Map& axis);
     void logic();
-    void t_compensate(const rmat& rmat, const euler_t& ypr, euler_t& output,
-                      bool disable_tx, bool disable_ty, bool disable_tz);
     void run() override;
 
-    static constexpr double r2d = 180. / M_PI;
-    static constexpr double d2r = M_PI / 180.;
-
     // note: float exponent base is 2
     static constexpr double c_mult = 16;
     static constexpr double c_div = 1./c_mult;