diff options
Diffstat (limited to 'logic/pipeline.cpp')
| -rw-r--r-- | logic/pipeline.cpp | 106 |
1 files changed, 72 insertions, 34 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp index 658c62bb..2e8efe55 100644 --- a/logic/pipeline.cpp +++ b/logic/pipeline.cpp @@ -15,7 +15,7 @@ #include "compat/sleep.hpp" #include "compat/math.hpp" #include "compat/meta.hpp" -#include "compat/macros.hpp" +#include "compat/macros.h" #include "compat/thread-name.hpp" #include "pipeline.hpp" @@ -184,8 +184,8 @@ Pose_ reltrans::apply_neck(const rmat& R, int nz, bool disable_tz) const return neck; } -pipeline::pipeline(const Mappings& m, const runtime_libraries& libs, event_handler& ev, TrackLogger& logger) : - m(m), ev(ev), libs(libs), logger(logger) +pipeline::pipeline(const Mappings& m, const runtime_libraries& libs, TrackLogger& logger) : + m(m), libs(libs), logger(logger) { } @@ -272,7 +272,7 @@ bool pipeline::maybe_enable_center_on_tracking_started() return false; } -void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic) +void pipeline::maybe_set_center_pose(const centering_state mode, const Pose& value, bool own_center_logic) { if (b.get(f_center | f_held_center)) { @@ -283,34 +283,73 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic) if (own_center_logic) { - center.inv_R = rmat::eye(); - center.T = {}; + center.P = {}; + center.QC = QQuaternion(1,0,0,0); + center.QR = QQuaternion(1,0,0,0); } else { - center.inv_R = euler_to_rmat(Pose_(&value[Yaw]) * (M_PI / 180)).t(); - center.T = Pose_(&value[TX]); + if (mode) + { + center.P = value; + center.QC = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], -value[Roll]).conjugated(); + center.QR = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], 0).conjugated(); + } + else + { + // To reset the centering coordinates + // just select "Centering method [Disabled]" and then press [Center] button + center.P = {}; + center.QC = QQuaternion(1,0,0,0); + center.QR = QQuaternion(1,0,0,0); + } } } } -Pose pipeline::apply_center(Pose value) const +Pose pipeline::apply_center(const centering_state mode, Pose value) const { - { - for (unsigned k = 0; k < 3; k++) - value(k) -= center.T(k); - - Pose_ rot = rmat_to_euler( - euler_to_rmat(Pose_(&value[Yaw]) * (M_PI / 180)) * center.inv_R - ); - for (unsigned k = 0; k < 3; k++) - value(k+3) = rot(k) * 180 / M_PI; + if (mode != center_disabled) + { + for (unsigned k = TX; k <= TZ; k++) + value(k) -= center.P(k); + + QQuaternion q; + QVector3D v; + + switch (mode) + { + case center_point: + for (unsigned k = Yaw; k <= Roll; k++) + { + value(k) -= center.P(k); + if (fabs(value[k]) > 180) value[k] -= copysign(360, value[k]); + } + break; + case center_vr360: + q = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], -value[Roll]); + q = center.QC * q; + v = q.toEulerAngles(); + value[Pitch] = v.x(); + value[Yaw] = v.y(); + value[Roll] = -v.z(); + break; + case center_roll_compensated: + q = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], center.P[Roll] - value[Roll]); + q = center.QR * q; + v = q.toEulerAngles(); + value[Pitch] = v.x(); + value[Yaw] = v.y(); + value[Roll] = -v.z(); + break; + case center_disabled: break; + } } for (int i = 0; i < 6; i++) // don't invert after reltrans // inverting here doesn't break centering - if (m(i).opts.invert) + if (m(i).opts.invert_pre) value(i) = -value(i); return value; @@ -352,7 +391,7 @@ Pose pipeline::maybe_apply_filter(const Pose& value) const Pose pipeline::apply_zero_pos(Pose value) const { for (int i = 0; i < 6; i++) - value(i) += m(i).opts.zero * (m(i).opts.invert ? -1 : 1); + value(i) += m(i).opts.zero * (m(i).opts.invert_pre ? -1 : 1); return value; } @@ -383,7 +422,6 @@ Pose pipeline::apply_reltrans(Pose value, vec6_bool disabled, bool centerp) void pipeline::logic() { using namespace euler; - using EV = event_handler::event_ordinal; logger.write_dt(); logger.reset_dt(); @@ -396,7 +434,6 @@ void pipeline::logic() { Pose tmp; libs.pTracker->data(tmp); - ev.run_events(EV::ev_raw, tmp); newpose = tmp; } @@ -407,15 +444,14 @@ void pipeline::logic() { maybe_enable_center_on_tracking_started(); - maybe_set_center_pose(value, own_center_logic); - value = apply_center(value); + maybe_set_center_pose(s.centering_mode, value, own_center_logic); + value = apply_center(s.centering_mode, value); // "corrected" - after various transformations to account for camera position logger.write_pose(value); } { - ev.run_events(EV::ev_before_filter, value); // we must proceed with all the filtering since the filter // needs fresh values to prevent deconvergence if (center_ordered) @@ -427,7 +463,6 @@ void pipeline::logic() } { - ev.run_events(EV::ev_before_mapping, value); // CAVEAT rotation only, due to reltrans for (int i = 3; i < 6; i++) value(i) = map(value(i), m(i)); @@ -442,14 +477,13 @@ void pipeline::logic() nan_check(value); } - if (!hold_ordered) - goto ok; + goto ok; error: { QMutexLocker foo(&mtx); - value = output_pose; + value = last_value; raw = raw_6dof; // for widget last value display @@ -465,10 +499,15 @@ ok: for (int i = 0; i < 6; i++) value(i) = 0; - if (hold_ordered) value = output_pose; - else value = apply_zero_pos(value); + if (hold_ordered) + value = last_value; + last_value = value; + value = apply_zero_pos(value); + + for (int i = 0; i < 6; i++) + if (m(i).opts.invert_post) + value(i) = -value(i); - ev.run_events(EV::ev_finished, value); libs.pProtocol->pose(value, raw); QMutexLocker foo(&mtx); @@ -565,8 +604,7 @@ void pipeline::run() backlog_time = {}; } - const int sleep_ms = (int)clamp(interval - backlog_time, - ms{0}, ms{10}).count(); + const int sleep_ms = (int)std::clamp(interval - backlog_time, ms{0}, ms{10}).count(); #ifdef DEBUG_TIMINGS debug_timings(backlog_time.count(), sleep_ms); |