[STABLE] logic/pipeline: revert refactor

3 files changed, 214 insertions, 261 deletions

diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 654a9f09..9059a251 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2018 Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2012-2015 Stanislaw Halik <sthalik@misaki.pl>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -35,39 +35,40 @@ static constexpr inline double d2r = M_PI / 180.;
 
 reltrans::reltrans() {}
 
-euler_t reltrans::rotate(const rmat& R, const euler_t& in, vec3_bool disable) const
+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 };
 
     // TY is really yaw axis. need swapping accordingly.
     // sign changes are due to right-vs-left handedness of coordinate system used
-    const euler_t ret = R * euler_t(in(TZ), -in(TX), -in(TY));
+    const euler_t ret = rmat * euler_t(xyz(TZ), -xyz(TX), -xyz(TY));
 
     euler_t output;
 
-    if (disable(TZ))
-        output(TZ) = in(TZ);
+    if (disable_tz)
+        output(TZ) = xyz(TZ);
     else
         output(TZ) = ret(tb_Z);
 
-    if (disable(TY))
-        output(TY) = in(TY);
+    if (disable_ty)
+        output(TY) = xyz(TY);
     else
         output(TY) = -ret(tb_Y);
 
-    if (disable(TX))
-        output(TX) = in(TX);
+    if (disable_tx)
+        output(TX) = xyz(TX);
     else
         output(TX) = -ret(tb_X);
 
     return output;
 }
 
-Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec6_bool& disable)
+Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat<bool, 6, 1>& disable)
 {
     if (state != reltrans_disabled)
     {
-        euler_t rel { &value[0] };
+        euler_t rel { value(TX), value(TY), value(TZ) };
 
         {
             bool tcomp_in_zone_ = progn(
@@ -95,11 +96,21 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec
         // only when looking behind or downward
         if (in_zone)
         {
-            const rmat R = euler_to_rmat(euler_t(value(Yaw)   * d2r * !disable(Yaw),
-                                                 value(Pitch) * d2r * !disable(Pitch),
-                                                 value(Roll)  * d2r * !disable(Roll)));
-
-            rel = rotate(R, rel, &disable[TX]);
+            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 (cur)
@@ -142,23 +153,6 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec
     }
 }
 
-euler_t reltrans::apply_neck(const Pose& value, bool enable, int nz) const
-{
-    if (!enable)
-        return {};
-
-    euler_t neck;
-
-    if (nz != 0)
-    {
-        const rmat R = euler_to_rmat(euler_t(&value[Yaw]) * d2r);
-        neck = rotate(R, { 0, 0, nz }, vec3_bool());
-        neck(TZ) = neck(TZ) - nz;
-    }
-
-    return neck;
-}
-
 pipeline::pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger) :
     m(m),
     ev(ev),
@@ -197,104 +191,78 @@ static bool is_nan(const dmat<u,w>& r)
 }
 
 template<typename x, typename y, typename... xs>
-static force_inline bool nan_check_(const x& datum, const y& next, const xs&... rest)
+static inline bool nan_check_(const x& datum, const y& next, const xs&... rest)
 {
-    return is_nan(datum) || nan_check_(next, rest...);
+    return !is_nan(datum) && nan_check_(next, rest...);
 }
 
 template<typename x>
-static force_inline bool nan_check_(const x& datum)
+static inline bool nan_check_(const x& datum)
 {
-    return is_nan(datum);
+    return !is_nan(datum);
 }
 
 template<typename>
 static bool nan_check_() = delete;
 
 static never_inline
-void emit_nan_check_msg(const char* text, const char* fun, int line)
+void emit_nan_check_msg(const char* text, int line)
 {
-    once_only(
-        qDebug()  << "nan check failed"
-                  << "for:" << text
-                  << "function:" << fun
-                  << "line:" << line;
-    );
+    once_only(qDebug()  << "nan check failed"
+                        << "line:" << text
+                        << "for:" << line);
 }
 
-template<typename... xs>
-static never_inline
-bool maybe_nan(const char* text, const char* fun, int line, const xs&... vals)
-{
-    if (nan_check_(vals...))
-    {
-        emit_nan_check_msg(text, fun, line);
-        return true;
-    }
-    return false;
-}
-
-#if defined _MSC_VER
-#   define OTR_FUNNAME2 (__FUNCSIG__)
-#else
-#   define OTR_FUNNAME2 (__PRETTY_FUNCTION__)
-#endif
-// don't expand
-#   define OTR_FUNNAME (OTR_FUNNAME2)
-
-#define nan_check(...)                                                      \
-    do                                                                      \
-    {                                                                       \
-        if (maybe_nan(#__VA_ARGS__, OTR_FUNNAME, __LINE__, __VA_ARGS__))    \
-            goto nan;                                                       \
+#define nan_check(...) \
+    do                                                      \
+    {                                                       \
+        if (!nan_check_(__VA_ARGS__))                       \
+        {                                                   \
+            emit_nan_check_msg(#__VA_ARGS__, __LINE__);     \
+            goto nan;                                       \
+        }                                                   \
     } while (false)
 
-void pipeline::maybe_enable_center_on_tracking_started()
+void pipeline::logic()
 {
-    if (!tracking_started)
-    {
-        for (int i = 0; i < 6; i++)
-            if (std::fabs(newpose(i)) != 0)
-            {
-                tracking_started = true;
-                break;
-            }
+    using namespace euler;
+    using EV = event_handler::event_ordinal;
 
-        if (tracking_started && s.center_at_startup)
-            set(f_center, true);
-    }
-}
+    logger.write_dt();
+    logger.reset_dt();
 
-void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
-{
-    euler_t tmp = d2r * euler_t(&value[Yaw]);
-    scaled_rotation.rotation = euler_to_rmat(c_div * tmp);
-    real_rotation.rotation = euler_to_rmat(tmp);
+    // 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();
+
+    Pose value, raw;
 
-    if (get(f_center))
     {
-        if (libs.pFilter)
-            libs.pFilter->center();
+        Pose tmp;
+        libs.pTracker->data(tmp);
+        nan_check(tmp);
+        ev.run_events(EV::ev_raw, tmp);
 
-        if (own_center_logic)
-        {
-            scaled_rotation.rot_center = rmat::eye();
-            real_rotation.rot_center = rmat::eye();
+        if (get(f_enabled_p) ^ !get(f_enabled_h))
+            for (int i = 0; i < 6; i++)
+                newpose(i) = tmp(i);
+    }
 
-            t_center = euler_t();
-        }
-        else
-        {
-            real_rotation.rot_center = real_rotation.rotation.t();
-            scaled_rotation.rot_center = scaled_rotation.rotation.t();
+    bool disabled[6];
 
-            t_center = euler_t(static_cast<const double*>(value));
-        }
+    for (int i = 0; i < 6; i++)
+    {
+        auto& axis = m(i);
+        int k = axis.opts.src;
+        disabled[i] = k == 6;
+        if (k < 0 || k >= 6)
+            value(i) = 0;
+        else
+            value(i) = newpose(k);
+        raw(i) = newpose(i);
     }
-}
 
-Pose pipeline::clamp_value(Pose value) const
-{
     // hatire, udp, and freepie trackers can mess up here
     for (unsigned i = 3; i < 6; i++)
     {
@@ -311,162 +279,159 @@ Pose pipeline::clamp_value(Pose value) const
             value(i) = clamp(x, -180, 180);
     }
 
-    return value;
-}
-
-Pose pipeline::apply_center(Pose value) const
-{
-    rmat rotation = scaled_rotation.rotation * scaled_rotation.rot_center;
-    euler_t pos = euler_t(value) - t_center;
-    euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
+    logger.write_pose(raw); // raw
 
-    pos = rel.rotate(real_rotation.rot_center, pos, vec3_bool());
+    // TODO split this function, it's too big
 
-    for (int i = 0; i < 3; i++)
     {
-        // don't invert after t_compensate
-        // inverting here doesn't break centering
-
-        if (m(i+3).opts.invert)
-            rot(i) = -rot(i);
-        if (m(i).opts.invert)
-            pos(i) = -pos(i);
+        euler_t tmp = d2r * euler_t(&value[Yaw]);
+        scaled_rotation.rotation = euler_to_rmat(c_div * tmp);
+        real_rotation.rotation = euler_to_rmat(tmp);
     }
 
-    for (int i = 0; i < 3; i++)
+    if (!tracking_started)
     {
-        value(i) = pos(i);
-        value(i+3) = rot(i);
-    }
-
-    return value;
-}
+        for (int i = 0; i < 6; i++)
+            if (std::fabs(newpose(i)) != 0)
+            {
+                tracking_started = true;
+                break;
+            }
 
-std::tuple<Pose, Pose, vec6_bool>
-pipeline::get_selected_axis_value(const Pose& newpose) const
-{
-    Pose value;
-    vec6_bool disabled;
+        if (tracking_started && s.center_at_startup)
+        {
+            set(f_center, true);
+        }
+    }
 
-    for (int i = 0; i < 6; i++)
+    if (center_ordered)
     {
-        const Map& axis = m(i);
-        const int k = axis.opts.src;
+        if (libs.pFilter)
+            libs.pFilter->center();
 
-        disabled(i) = k == 6;
+        if (own_center_logic)
+        {
+            scaled_rotation.rot_center = rmat::eye();
+            real_rotation.rot_center = rmat::eye();
 
-        if (k < 0 || k >= 6)
-            value(i) = 0;
+            t_center = euler_t();
+        }
         else
-            value(i) = newpose(k);
+        {
+            real_rotation.rot_center = real_rotation.rotation.t();
+            scaled_rotation.rot_center = scaled_rotation.rotation.t();
+
+            t_center = euler_t(&value(TX));
+        }
     }
 
-    return { newpose, value, disabled };
-}
+    {
+        rmat rotation = scaled_rotation.rotation;
+        euler_t pos = euler_t(&value[TX]) - t_center;
 
-Pose pipeline::maybe_apply_filter(const Pose& value) const
-{
-    Pose tmp(value);
+        //switch (s.center_method)
+        switch (1)
+        {
+        case 0:
+            // inertial
+            rotation = scaled_rotation.rot_center * rotation;
+        break;
+
+        default:
+        case 1:
+            // camera
+            rotation = rotation * scaled_rotation.rot_center;
+            pos = rel.rotate(real_rotation.rot_center, pos, false, false, false);
+        break;
+        }
 
-    // nan/inf values will corrupt filter internal state
-    if (libs.pFilter)
-        libs.pFilter->filter(value, tmp);
+        euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
 
-    return tmp;
-}
+        for (int i = 0; i < 3; i++)
+        {
+            // don't invert after t_compensate
+            // inverting here doesn't break centering
 
-Pose pipeline::apply_zero_pos(Pose value) const
-{
-    // custom zero position
-    for (int i = 0; i < 6; i++)
-        value(i) += m(i).opts.zero * (m(i).opts.invert ? -1 : 1);
+            if (m(i+3).opts.invert)
+                rot(i) = -rot(i);
+            if (m(i).opts.invert)
+                pos(i) = -pos(i);
+        }
 
-    return value;
-}
+        for (int i = 0; i < 3; i++)
+        {
+            value(i) = pos(i);
+            value(i+3) = rot(i);
+        }
+    }
 
-Pose pipeline::apply_reltrans(Pose value, vec6_bool disabled)
-{
-    const euler_t neck = rel.apply_neck(value, s.neck_enable, -s.neck_z);
+    logger.write_pose(value); // "corrected" - after various transformations to account for camera position
 
-    value = rel.apply_pipeline(s.reltrans_mode, value,
-                               { !!s.reltrans_disable_src_yaw,
-                                 !!s.reltrans_disable_src_pitch,
-                                 !!s.reltrans_disable_src_roll,
-                                 !!s.reltrans_disable_tx,
-                                 !!s.reltrans_disable_ty,
-                                 !!s.reltrans_disable_tz });
+    ev.run_events(EV::ev_before_filter, value);
 
-    for (int i = 0; i < 3; i++)
-        value(i) += neck(i);
+    {
+        {
+            Pose tmp(value);
 
-    // reltrans will move it
-    for (unsigned k = 0; k < 6; k++)
-        if (disabled(k))
-            value(k) = 0;
+            // nan/inf values will corrupt filter internal state
+            if (libs.pFilter)
+                libs.pFilter->filter(tmp, value);
 
-    return value;
-}
+            nan_check(value);
 
-void pipeline::logic()
-{
-    using namespace euler;
-    using EV = event_handler::event_ordinal;
-
-    logger.write_dt();
-    logger.reset_dt();
+            logger.write_pose(value); // "filtered"
+        }
+    }
 
-    // we must center prior to getting data from the tracker
-    const bool center_ordered = get(f_center) && tracking_started;
-    const bool own_center_logic = center_ordered && libs.pTracker->center();
+    {
+        ev.run_events(EV::ev_before_mapping, value);
 
-    Pose value, raw;
-    vec6_bool disabled;
+        {
+            euler_t neck;
 
-    {
-        Pose tmp;
-        libs.pTracker->data(tmp);
-        nan_check(tmp);
-        ev.run_events(EV::ev_raw, tmp);
+            if (s.neck_enable)
+            {
+                double nz = -s.neck_z;
 
-        if (get(f_enabled_p) ^ !get(f_enabled_h))
-            for (int i = 0; i < 6; i++)
-                newpose(i) = tmp(i);
-    }
+                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);
+                }
+            }
 
-    std::tie(raw, value, disabled) = get_selected_axis_value(newpose);
-    logger.write_pose(raw); // raw
+            // CAVEAT rotation only, due to tcomp
+            for (int i = 3; i < 6; i++)
+                value(i) = map(value(i), m(i));
 
-    value = clamp_value(value);
+            nan_check(value);
 
-    {
-        maybe_enable_center_on_tracking_started();
-        maybe_set_center_pose(value, own_center_logic);
-        value = apply_center(value);
-        logger.write_pose(value); // "corrected" - after various transformations to account for camera position
-    }
+            value = rel.apply_pipeline(s.reltrans_mode, value, {
+                !!s.reltrans_disable_src_yaw, !!s.reltrans_disable_src_pitch, !!s.reltrans_disable_src_roll,
+                !!s.reltrans_disable_tx, !!s.reltrans_disable_ty, !!s.reltrans_disable_tz
+            });
 
-    {
-        ev.run_events(EV::ev_before_filter, value);
-        value = maybe_apply_filter(value);
-        nan_check(value);
-        logger.write_pose(value); // "filtered"
-    }
+            for (int i = 0; i < 3; i++)
+                value(i) += neck(i);
+        }
 
-    {
-        ev.run_events(EV::ev_before_mapping, value);
-        // CAVEAT rotation only, due to tcomp
-        for (int i = 3; i < 6; i++)
-            value(i) = map(value(i), m(i));
+        // relative translation can move it
+        for (unsigned k = 0; k < 6; k++)
+            if (disabled[k])
+                value(k) = 0;
     }
 
-    value = apply_reltrans(value, disabled);
+    // CAVEAT translation only, due to tcomp
+    for (int i = 0; i < 3; i++)
+        value(i) = map(value(i), m(i));
 
-    {
-        // CAVEAT translation only, due to tcomp
-        for (int i = 0; i < 3; i++)
-            value(i) = map(value(i), m(i));
-        nan_check(value);
-    }
+    nan_check(value);
 
     goto ok;
 
@@ -479,20 +444,21 @@ nan:
 
         // for widget last value display
         for (int i = 0; i < 6; i++)
-            (void)map(raw_6dof(i), m(i));
+            (void) map(raw_6dof(i), m(i));
     }
 
 ok:
 
-    set(f_center, false);
-
     if (get(f_zero))
         for (int i = 0; i < 6; i++)
             value(i) = 0;
 
-    value = apply_zero_pos(value);
+    // custom zero position
+    for (int i = 0; i < 6; i++)
+        value(i) += m(i).opts.zero * (m(i).opts.invert ? -1 : 1);
 
     ev.run_events(EV::ev_finished, value);
+
     libs.pProtocol->pose(value);
 
     QMutexLocker foo(&mtx);
@@ -587,15 +553,16 @@ void pipeline::raw_and_mapped_pose(double* mapped, double* raw) const
     }
 }
 
-void pipeline::set_center() { set(f_center, true); }
+void pipeline::center() { set(f_center, true); }
 
-void pipeline::set_enabled(bool value) { set(f_enabled_h, value); }
+void pipeline::set_toggle(bool value) { set(f_enabled_h, value); }
 void pipeline::set_zero(bool value) { set(f_zero, value); }
 
-void pipeline::toggle_zero() { negate(f_zero); }
+void pipeline::zero() { negate(f_zero); }
 void pipeline::toggle_enabled() { negate(f_enabled_p); }
 
-void bits::set(flags flag_, bool val_)
+
+void bits::set(bits::flags flag_, bool val_)
 {
     const unsigned flag = unsigned(flag_);
     const unsigned val = unsigned(val_);
@@ -611,7 +578,7 @@ void bits::set(flags flag_, bool val_)
     }
 }
 
-void bits::negate(flags flag_)
+void bits::negate(bits::flags flag_)
 {
     const unsigned flag = unsigned(flag_);
 
@@ -627,7 +594,7 @@ void bits::negate(flags flag_)
     }
 }
 
-bool bits::get(flags flag)
+bool bits::get(bits::flags flag)
 {
     return !!(b & flag);
 }
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 5c3c3a26..1e1919f4 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -35,9 +35,6 @@ namespace gui_tracker_impl {
 using rmat = euler::rmat;
 using euler_t = euler::euler_t;
 
-using vec6_bool = Mat<bool, 6, 1>;
-using vec3_bool = Mat<bool, 6, 1>;
-
 class reltrans
 {
     euler_t interp_pos, last_value;
@@ -49,13 +46,11 @@ public:
     reltrans();
 
     warn_result_unused
-    euler_t rotate(const rmat& rmat, const euler_t& in, vec3_bool disable) const;
-
-    warn_result_unused
-    Pose apply_pipeline(reltrans_state cur, const Pose& value, const vec6_bool& disable);
+    euler_t rotate(const rmat& rmat, const euler_t& xyz,
+                   bool disable_tx, bool disable_ty, bool disable_tz) const;
 
     warn_result_unused
-    euler_t apply_neck(const Pose& value, bool enable, int nz) const;
+    Pose apply_pipeline(reltrans_state cur, const Pose& value, const Mat<bool, 6, 1>& disable);
 };
 
 using namespace time_units;
@@ -117,14 +112,6 @@ private:
     double map(double pos, Map& axis);
     void logic();
     void run() override;
-    void maybe_enable_center_on_tracking_started();
-    void maybe_set_center_pose(const Pose& value, bool own_center_logic);
-    Pose clamp_value(Pose value) const;
-    Pose apply_center(Pose value) const;
-    std::tuple<Pose, Pose, vec6_bool> get_selected_axis_value(const Pose& newpose) const;
-    Pose maybe_apply_filter(const Pose& value) const;
-    Pose apply_reltrans(Pose value, vec6_bool disabled);
-    Pose apply_zero_pos(Pose value) const;
 
     // note: float exponent base is 2
     static constexpr inline double c_mult = 16;
@@ -136,12 +123,11 @@ public:
     void raw_and_mapped_pose(double* mapped, double* raw) const;
     void start() { QThread::start(QThread::HighPriority); }
 
-    void toggle_zero();
-    void toggle_enabled();
-
-    void set_center();
-    void set_enabled(bool value);
+    void center();
+    void set_toggle(bool value);
     void set_zero(bool value);
+    void zero();
+    void toggle_enabled();
 };
 
 } // ns impl
diff --git a/logic/work.cpp b/logic/work.cpp
index 11ec9912..cdc27b05 100644
--- a/logic/work.cpp
+++ b/logic/work.cpp
@@ -65,17 +65,17 @@ Work::Work(Mappings& m, event_handler& ev,  QFrame* frame, std::shared_ptr<dylib
     tracker(std::make_shared<pipeline>(m, libs, ev, *logger)),
     sc(std::make_shared<Shortcuts>()),
     keys {
-        key_tuple(s.key_center1, [&](bool) { tracker->set_center(); }, true),
-        key_tuple(s.key_center2, [&](bool) { tracker->set_center(); }, true),
+        key_tuple(s.key_center1, [&](bool) { tracker->center(); }, true),
+        key_tuple(s.key_center2, [&](bool) { tracker->center(); }, true),
 
         key_tuple(s.key_toggle1, [&](bool) { tracker->toggle_enabled(); }, true),
         key_tuple(s.key_toggle2, [&](bool) { tracker->toggle_enabled(); }, true),
 
-        key_tuple(s.key_zero1, [&](bool) { tracker->toggle_zero(); }, true),
-        key_tuple(s.key_zero2, [&](bool) { tracker->toggle_zero(); }, true),
+        key_tuple(s.key_zero1, [&](bool) { tracker->zero(); }, true),
+        key_tuple(s.key_zero2, [&](bool) { tracker->zero(); }, true),
 
-        key_tuple(s.key_toggle_press1, [&](bool x) { tracker->set_enabled(!x); }, false),
-        key_tuple(s.key_toggle_press2, [&](bool x) { tracker->set_enabled(!x); }, false),
+        key_tuple(s.key_toggle_press1, [&](bool x) { tracker->set_toggle(!x); }, false),
+        key_tuple(s.key_toggle_press2, [&](bool x) { tracker->set_toggle(!x); }, false),
 
         key_tuple(s.key_zero_press1, [&](bool x) { tracker->set_zero(x); }, false),
         key_tuple(s.key_zero_press2, [&](bool x) { tracker->set_zero(x); }, false),