diff options
Diffstat (limited to 'logic/pipeline.cpp')
| -rw-r--r-- | logic/pipeline.cpp | 175 |
1 files changed, 57 insertions, 118 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp index 021a576b..bc8026ed 100644 --- a/logic/pipeline.cpp +++ b/logic/pipeline.cpp @@ -26,26 +26,18 @@ #ifdef _WIN32 # include <windows.h> -#endif - -#if defined __llvm__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wlanguage-extension-token" -# pragma clang diagnostic ignored "-Wunknown-pragmas" +# include <mmsystem.h> #endif namespace pipeline_impl { -static constexpr inline double r2d = 180 / M_PI; -static constexpr inline double d2r = M_PI / 180; - reltrans::reltrans() = default; void reltrans::on_center() { interp_pos = { 0, 0, 0 }; in_zone = false; - cur = false; + moving_to_reltans = false; } euler_t reltrans::rotate(const rmat& R, const euler_t& in, vec3_bool disable) const @@ -83,20 +75,17 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, if (state != reltrans_disabled) { - const bool in_zone_ = progn( - if (state == reltrans_non_center) - { - const bool looking_down = value(Pitch) < 20; - return looking_down ? std::fabs(value(Yaw)) > 35 : std::fabs(value(Yaw)) > 65; - } - else - return true; - ); + bool in_zone_ = true; + if (state == reltrans_non_center) + { + const bool looking_down = value(Pitch) < 20; + in_zone_ = looking_down ? std::fabs(value(Yaw)) > 35 : std::fabs(value(Yaw)) > 65; + } - if (!cur && in_zone != in_zone_) + if (!moving_to_reltans && in_zone != in_zone_) { //qDebug() << "reltrans-interp: START" << tcomp_in_zone_; - cur = true; + moving_to_reltans = true; interp_timer.start(); interp_phase_timer.start(); RC_stage = 0; @@ -107,6 +96,8 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, // only when looking behind or downward if (in_zone) { + constexpr double d2r = M_PI / 180; + const rmat R = euler_to_rmat( euler_t(value(Yaw) * d2r * !disable(Yaw), value(Pitch) * d2r * !disable(Pitch), @@ -124,12 +115,12 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, } } - if (cur) + if (moving_to_reltans) { const double dt = interp_timer.elapsed_seconds(); - static constexpr float RC_stages[] = { 2, 1, .5, .1, .05 }; - static constexpr float RC_time_deltas[] = { 1, .25, .25, 2 }; + static constexpr double RC_stages[] = { 2, 1, .5, .1, .05 }; + static constexpr double RC_time_deltas[] = { 1, .25, .25, 2 }; interp_timer.start(); @@ -156,7 +147,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, if (delta < eps) { //qDebug() << "reltrans-interp: STOP"; - cur = false; + moving_to_reltans = false; } } else @@ -164,7 +155,7 @@ Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, } else { - cur = false; + moving_to_reltans = false; in_zone = false; } @@ -204,7 +195,7 @@ double pipeline::map(double pos, Map& axis) 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)); + return fc.get_value(pos); } //#define NO_NAN_CHECK @@ -213,10 +204,10 @@ double pipeline::map(double pos, Map& axis) #ifndef NO_NAN_CHECK template<int u, int w> -static bool is_nan(const dmat<u,w>& r) +static inline bool is_nan(const dmat<u,w>& r) { - for (int i = 0; i < u; i++) - for (int j = 0; j < w; j++) + for (unsigned i = 0; i < u; i++) + for (unsigned j = 0; j < w; j++) { int val = std::fpclassify(r(i, j)); if (val == FP_NAN || val == FP_INFINITE) @@ -226,21 +217,8 @@ static bool is_nan(const dmat<u,w>& r) return false; } -template<typename x> -static inline -bool nan_check_(const x& datum) -{ - return is_nan(datum); -} - -template<typename x, typename y, typename... xs> -static inline -bool nan_check_(const x& datum, const y& next, const xs&... rest) -{ - return nan_check_(datum) || nan_check_(next, rest...); -} - -static void emit_nan_check_msg(const char* text, const char* fun, int line) +static never_inline +void emit_nan_check_msg(const char* text, const char* fun, int line) { eval_once( qDebug() << "nan check failed" @@ -251,33 +229,31 @@ static void emit_nan_check_msg(const char* text, const char* fun, int line) } template<typename... xs> -static cc_noinline +static never_inline bool maybe_nan(const char* text, const char* fun, int line, const xs&... vals) { - if (nan_check_(vals...)) - { + bool ret = (is_nan(vals) || ... || false); + + if (ret) emit_nan_check_msg(text, fun, line); - return true; - } - return false; -} -// for MSVC `else' is like `unlikely' for GNU + return ret; +} #define nan_check(...) \ do \ { \ - if (likely(!maybe_nan(#__VA_ARGS__, cc_function_name, __LINE__, __VA_ARGS__))) \ + if (likely(!maybe_nan(#__VA_ARGS__, function_name, __LINE__, __VA_ARGS__))) \ (void)0; \ else \ goto error; \ - } while (false) + } \ + while (false) #else # define nan_check(...) (void)(__VA_ARGS__) #endif - bool pipeline::maybe_enable_center_on_tracking_started() { if (!tracking_started) @@ -299,7 +275,7 @@ bool pipeline::maybe_enable_center_on_tracking_started() return false; } -void pipeline::set_center_pose(const Pose& value, bool own_center_logic) +void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic) { if (b.get(f_center | f_held_center)) { @@ -309,26 +285,12 @@ void pipeline::set_center_pose(const Pose& value, bool own_center_logic) libs.pFilter->center(); if (own_center_logic) - { - scaled_state.inv_rot_center = rmat::eye(); - scaled_state.t_center = {}; - } + center = {}; else - { - scaled_state.inv_rot_center = scaled_state.rotation.t(); - scaled_state.t_center = (const double*)(value); - } + center = value; } } -void pipeline::store_tracker_pose(const Pose& value) -{ - // alas, this is poor man's gimbal lock "prevention" - // this is some kind of "canonical" representation, - // if we can even talk of one - scaled_state.rotation = euler_to_rmat(scale_inv_c * d2r * euler_t(&value[Yaw])); -} - Pose pipeline::clamp_value(Pose value) const { // hatire, udp, and freepie trackers can mess up here @@ -347,28 +309,14 @@ Pose pipeline::clamp_value(Pose value) const Pose pipeline::apply_center(Pose value) const { - euler_t T = euler_t(value) - scaled_state.t_center; - euler_t R = scale_c * rmat_to_euler(scaled_state.rotation * scaled_state.inv_rot_center); - - // XXX check these lines, it's been here forever - T = rel.rotate(euler_to_rmat(R), T, {}); + // this is incorrect but people like it + value = value - center; - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 6; i++) // don't invert after reltrans // inverting here doesn't break centering - - if (m(i+3).opts.invert) - R(i) = -R(i); if (m(i).opts.invert) - T(i) = -T(i); - } - - for (int i = 0; i < 3; i++) - { - value(i) = T(i); - value(i+3) = R(i) * r2d; - } + value(i) = -value(i); return value; } @@ -408,7 +356,6 @@ Pose pipeline::maybe_apply_filter(const Pose& value) const 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); @@ -466,11 +413,9 @@ void pipeline::logic() value = clamp_value(value); { - store_tracker_pose(value); - maybe_enable_center_on_tracking_started(); - set_center_pose(value, own_center_logic); - value = apply_center(value); + maybe_set_center_pose(value, own_center_logic); + value = clamp_value(apply_center(value)); // "corrected" - after various transformations to account for camera position logger.write_pose(value); @@ -480,10 +425,11 @@ void pipeline::logic() ev.run_events(EV::ev_before_filter, value); // we must proceed with all the filtering since the filter // needs fresh values to prevent deconvergence - Pose tmp = maybe_apply_filter(value); - nan_check(tmp); - if (!center_ordered) - value = tmp; + if (center_ordered) + (void)maybe_apply_filter(value); + else + value = maybe_apply_filter(value); + nan_check(value); logger.write_pose(value); // "filtered" } @@ -631,7 +577,7 @@ void pipeline::run() void pipeline::raw_and_mapped_pose(double* mapped, double* raw) const { - QMutexLocker foo(&const_cast<pipeline&>(*this).mtx); + QMutexLocker foo(&mtx); for (int i = 0; i < 6; i++) { @@ -655,31 +601,28 @@ void pipeline::toggle_enabled() { b.negate(f_enabled_p); } void bits::set(bit_flags flag, bool val) { - const unsigned flag_ = unsigned(flag); - const unsigned val_ = unsigned(val); - unsigned b_ = 0; + QMutexLocker l(&lock); - for (;;) - if (b.compare_exchange_weak(b_, unsigned((b_ & ~flag_) | (flag_ * val_)))) - break; + flags &= ~flag; + if (val) + flags |= flag; } void bits::negate(bit_flags flag) { - const unsigned flag_= flag; - unsigned b_ = 0; + QMutexLocker l(&lock); - for (;;) - if (b.compare_exchange_weak(b_, b_ ^ flag_)) - break; + flags ^= flag; } bool bits::get(bit_flags flag) { - return !!(b & flag); + QMutexLocker l(&lock); + + return !!(flags & flag); } -bits::bits() : b(0u) +bits::bits() { set(f_center, false); set(f_held_center, false); @@ -689,7 +632,3 @@ bits::bits() : b(0u) } } // ns pipeline_impl - -#if defined __llvm__ -# pragma clang diagnostic pop -#endif |