[WIP] logic/pipeline: simplificationpipeline-work-20180216

NEEDS TESTING. DO NOT COMMIT!!!

1 files changed, 233 insertions, 200 deletions

diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 9059a251..c057b274 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -35,40 +35,39 @@ static constexpr inline double d2r = M_PI / 180.;
 
 reltrans::reltrans() {}
 
-euler_t reltrans::rotate(const rmat& rmat, const euler_t& xyz,
-                         bool disable_tx, bool disable_ty, bool disable_tz) const
+euler_t reltrans::rotate(const rmat& R, const euler_t& in, vec3_bool disable) 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 = rmat * euler_t(xyz(TZ), -xyz(TX), -xyz(TY));
+    const euler_t ret = R * euler_t(in(TZ), -in(TX), -in(TY));
 
     euler_t output;
 
-    if (disable_tz)
-        output(TZ) = xyz(TZ);
+    if (disable(TZ))
+        output(TZ) = in(TZ);
     else
         output(TZ) = ret(tb_Z);
 
-    if (disable_ty)
-        output(TY) = xyz(TY);
+    if (disable(TY))
+        output(TY) = in(TY);
     else
         output(TY) = -ret(tb_Y);
 
-    if (disable_tx)
-        output(TX) = xyz(TX);
+    if (disable(TX))
+        output(TX) = in(TX);
     else
         output(TX) = -ret(tb_X);
 
     return output;
 }
 
-Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat<bool, 6, 1>& disable)
+Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const vec6_bool& disable)
 {
     if (state != reltrans_disabled)
     {
-        euler_t rel { value(TX), value(TY), value(TZ) };
+        euler_t rel { &value[0] };
 
         {
             bool tcomp_in_zone_ = progn(
@@ -96,21 +95,11 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat
         // only when looking behind or downward
         if (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));
+            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]);
         }
 
         if (cur)
@@ -153,6 +142,23 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat
     }
 }
 
+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),
@@ -191,78 +197,104 @@ static bool is_nan(const dmat<u,w>& r)
 }
 
 template<typename x, typename y, typename... xs>
-static inline bool nan_check_(const x& datum, const y& next, const xs&... rest)
+static force_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 inline bool nan_check_(const x& datum)
+static force_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, int line)
+void emit_nan_check_msg(const char* text, const char* fun, int line)
 {
-    once_only(qDebug()  << "nan check failed"
-                        << "line:" << text
-                        << "for:" << line);
+    once_only(
+        qDebug()  << "nan check failed"
+                  << "for:" << text
+                  << "function:" << fun
+                  << "line:" << line;
+    );
 }
 
-#define nan_check(...) \
-    do                                                      \
-    {                                                       \
-        if (!nan_check_(__VA_ARGS__))                       \
-        {                                                   \
-            emit_nan_check_msg(#__VA_ARGS__, __LINE__);     \
-            goto nan;                                       \
-        }                                                   \
-    } while (false)
-
-void pipeline::logic()
+template<typename... xs>
+static never_inline
+bool maybe_nan(const char* text, const char* fun, int line, const xs&... vals)
 {
-    using namespace euler;
-    using EV = event_handler::event_ordinal;
-
-    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();
+    if (nan_check_(vals...))
+    {
+        emit_nan_check_msg(text, fun, line);
+        return true;
+    }
+    return false;
+}
 
-    Pose value, raw;
+#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;                                                       \
+    } while (false)
 
+void pipeline::maybe_enable_center_on_tracking_started()
+{
+    if (!tracking_started)
     {
-        Pose tmp;
-        libs.pTracker->data(tmp);
-        nan_check(tmp);
-        ev.run_events(EV::ev_raw, tmp);
+        for (int i = 0; i < 6; i++)
+            if (std::fabs(newpose(i)) != 0)
+            {
+                tracking_started = true;
+                break;
+            }
 
-        if (get(f_enabled_p) ^ !get(f_enabled_h))
-            for (int i = 0; i < 6; i++)
-                newpose(i) = tmp(i);
+        if (tracking_started && s.center_at_startup)
+            set(f_center, true);
     }
+}
 
-    bool disabled[6];
+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);
 
-    for (int i = 0; i < 6; i++)
+    if (get(f_center))
     {
-        auto& axis = m(i);
-        int k = axis.opts.src;
-        disabled[i] = k == 6;
-        if (k < 0 || k >= 6)
-            value(i) = 0;
+        if (libs.pFilter)
+            libs.pFilter->center();
+
+        if (own_center_logic)
+        {
+            scaled_rotation.rot_center = rmat::eye();
+            real_rotation.rot_center = rmat::eye();
+
+            t_center = euler_t();
+        }
         else
-            value(i) = newpose(k);
-        raw(i) = newpose(i);
+        {
+            real_rotation.rot_center = real_rotation.rotation.t();
+            scaled_rotation.rot_center = scaled_rotation.rotation.t();
+
+            t_center = euler_t(static_cast<const double*>(value));
+        }
     }
+}
 
+Pose pipeline::clamp_value(Pose value) const
+{
     // hatire, udp, and freepie trackers can mess up here
     for (unsigned i = 3; i < 6; i++)
     {
@@ -279,159 +311,162 @@ void pipeline::logic()
             value(i) = clamp(x, -180, 180);
     }
 
-    logger.write_pose(raw); // raw
+    return value;
+}
 
-    // TODO split this function, it's too big
+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);
+
+    pos = rel.rotate(real_rotation.rot_center, pos, vec3_bool());
 
+    for (int i = 0; i < 3; 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);
+        // 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);
     }
 
-    if (!tracking_started)
+    for (int i = 0; i < 3; i++)
     {
-        for (int i = 0; i < 6; i++)
-            if (std::fabs(newpose(i)) != 0)
-            {
-                tracking_started = true;
-                break;
-            }
-
-        if (tracking_started && s.center_at_startup)
-        {
-            set(f_center, true);
-        }
+        value(i) = pos(i);
+        value(i+3) = rot(i);
     }
 
-    if (center_ordered)
+    return value;
+}
+
+std::tuple<Pose, Pose, vec6_bool>
+pipeline::get_selected_axis_value(const Pose& newpose) const
+{
+    Pose value;
+    vec6_bool disabled;
+
+    for (int i = 0; i < 6; i++)
     {
-        if (libs.pFilter)
-            libs.pFilter->center();
+        const Map& axis = m(i);
+        const int k = axis.opts.src;
 
-        if (own_center_logic)
-        {
-            scaled_rotation.rot_center = rmat::eye();
-            real_rotation.rot_center = rmat::eye();
+        disabled(i) = k == 6;
 
-            t_center = euler_t();
-        }
+        if (k < 0 || k >= 6)
+            value(i) = 0;
         else
-        {
-            real_rotation.rot_center = real_rotation.rotation.t();
-            scaled_rotation.rot_center = scaled_rotation.rotation.t();
-
-            t_center = euler_t(&value(TX));
-        }
+            value(i) = newpose(k);
     }
 
-    {
-        rmat rotation = scaled_rotation.rotation;
-        euler_t pos = euler_t(&value[TX]) - t_center;
+    return { newpose, value, disabled };
+}
 
-        //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;
-        }
+Pose pipeline::maybe_apply_filter(const Pose& value) const
+{
+    Pose tmp(value);
 
-        euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
+    // nan/inf values will corrupt filter internal state
+    if (libs.pFilter)
+        libs.pFilter->filter(value, tmp);
 
-        for (int i = 0; i < 3; i++)
-        {
-            // don't invert after t_compensate
-            // inverting here doesn't break centering
+    return tmp;
+}
 
-            if (m(i+3).opts.invert)
-                rot(i) = -rot(i);
-            if (m(i).opts.invert)
-                pos(i) = -pos(i);
-        }
+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);
 
-        for (int i = 0; i < 3; i++)
-        {
-            value(i) = pos(i);
-            value(i+3) = rot(i);
-        }
-    }
+    return value;
+}
 
-    logger.write_pose(value); // "corrected" - after various transformations to account for camera position
+Pose pipeline::apply_reltrans(Pose value, vec6_bool disabled)
+{
+    const euler_t neck = rel.apply_neck(value, s.neck_enable, -s.neck_z);
 
-    ev.run_events(EV::ev_before_filter, value);
+    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 });
 
-    {
-        {
-            Pose tmp(value);
+    for (int i = 0; i < 3; i++)
+        value(i) += neck(i);
 
-            // nan/inf values will corrupt filter internal state
-            if (libs.pFilter)
-                libs.pFilter->filter(tmp, value);
+    // reltrans will move it
+    for (unsigned k = 0; k < 6; k++)
+        if (disabled(k))
+            value(k) = 0;
 
-            nan_check(value);
+    return value;
+}
 
-            logger.write_pose(value); // "filtered"
-        }
-    }
+void pipeline::logic()
+{
+    using namespace euler;
+    using EV = event_handler::event_ordinal;
 
-    {
-        ev.run_events(EV::ev_before_mapping, value);
+    logger.write_dt();
+    logger.reset_dt();
 
-        {
-            euler_t neck;
+    // 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();
 
-            if (s.neck_enable)
-            {
-                double nz = -s.neck_z;
+    Pose value, raw;
+    vec6_bool disabled;
 
-                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);
-                }
-            }
+    {
+        Pose tmp;
+        libs.pTracker->data(tmp);
+        nan_check(tmp);
+        ev.run_events(EV::ev_raw, tmp);
+
+        if (get(f_enabled_p) ^ !get(f_enabled_h))
+            for (int i = 0; i < 6; i++)
+                newpose(i) = tmp(i);
+    }
 
-            // CAVEAT rotation only, due to tcomp
-            for (int i = 3; i < 6; i++)
-                value(i) = map(value(i), m(i));
+    std::tie(raw, value, disabled) = get_selected_axis_value(newpose);
+    logger.write_pose(raw); // raw
 
-            nan_check(value);
+    value = clamp_value(value);
 
-            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
-            });
+    {
+        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
+    }
 
-            for (int i = 0; i < 3; i++)
-                value(i) += neck(i);
-        }
+    {
+        ev.run_events(EV::ev_before_filter, value);
+        value = maybe_apply_filter(value);
+        nan_check(value);
+        logger.write_pose(value); // "filtered"
+    }
 
-        // relative translation can move it
-        for (unsigned k = 0; k < 6; k++)
-            if (disabled[k])
-                value(k) = 0;
+    {
+        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));
     }
 
-    // CAVEAT translation only, due to tcomp
-    for (int i = 0; i < 3; i++)
-        value(i) = map(value(i), m(i));
+    value = apply_reltrans(value, disabled);
 
-    nan_check(value);
+    {
+        // CAVEAT translation only, due to tcomp
+        for (int i = 0; i < 3; i++)
+            value(i) = map(value(i), m(i));
+        nan_check(value);
+    }
 
     goto ok;
 
@@ -444,21 +479,20 @@ 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;
 
-    // custom zero position
-    for (int i = 0; i < 6; i++)
-        value(i) += m(i).opts.zero * (m(i).opts.invert ? -1 : 1);
+    value = apply_zero_pos(value);
 
     ev.run_events(EV::ev_finished, value);
-
     libs.pProtocol->pose(value);
 
     QMutexLocker foo(&mtx);
@@ -553,16 +587,15 @@ void pipeline::raw_and_mapped_pose(double* mapped, double* raw) const
     }
 }
 
-void pipeline::center() { set(f_center, true); }
+void pipeline::set_center() { set(f_center, true); }
 
-void pipeline::set_toggle(bool value) { set(f_enabled_h, value); }
+void pipeline::set_enabled(bool value) { set(f_enabled_h, value); }
 void pipeline::set_zero(bool value) { set(f_zero, value); }
 
-void pipeline::zero() { negate(f_zero); }
+void pipeline::toggle_zero() { negate(f_zero); }
 void pipeline::toggle_enabled() { negate(f_enabled_p); }
 
-
-void bits::set(bits::flags flag_, bool val_)
+void bits::set(flags flag_, bool val_)
 {
     const unsigned flag = unsigned(flag_);
     const unsigned val = unsigned(val_);
@@ -578,7 +611,7 @@ void bits::set(bits::flags flag_, bool val_)
     }
 }
 
-void bits::negate(bits::flags flag_)
+void bits::negate(flags flag_)
 {
     const unsigned flag = unsigned(flag_);
 
@@ -594,7 +627,7 @@ void bits::negate(bits::flags flag_)
     }
 }
 
-bool bits::get(bits::flags flag)
+bool bits::get(flags flag)
 {
     return !!(b & flag);
 }