diff options
| -rw-r--r-- | logic/pipeline.cpp | 231 | ||||
| -rw-r--r-- | logic/pipeline.hpp | 31 |
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; |