diff options
Diffstat (limited to 'logic/pipeline.cpp')
| -rw-r--r-- | logic/pipeline.cpp | 433 |
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); } |