diff options
Diffstat (limited to 'logic')
| -rw-r--r-- | logic/tracker.cpp | 45 | ||||
| -rw-r--r-- | logic/tracker.h | 8 |
2 files changed, 32 insertions, 21 deletions
diff --git a/logic/tracker.cpp b/logic/tracker.cpp index 8757e98e..a5892dd0 100644 --- a/logic/tracker.cpp +++ b/logic/tracker.cpp @@ -26,6 +26,7 @@ #endif using namespace euler; +using namespace gui_tracker_impl; constexpr double Tracker::r2d; constexpr double Tracker::d2r; @@ -34,7 +35,8 @@ Tracker::Tracker(Mappings& m, SelectedLibraries& libs, TrackLogger& logger) : m(m), libs(libs), logger(logger), - backlog_time(0) + backlog_time(0), + tracking_started(false) { set(f_center, s.center_at_startup); } @@ -169,28 +171,23 @@ void Tracker::logic() scaled_rotation.camera = get_camera_offset_matrix(c_div); real_rotation.camera = get_camera_offset_matrix(1); - scaled_rotation.rotation = scaled_rotation.camera * scaled_rotation.rotation; - real_rotation.rotation = real_rotation.camera * real_rotation.rotation; - bool nanp = is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation); if (!nanp) { - bool can_center = false; - - if (get(f_center)) + if (!tracking_started) { using std::fabs; for (int i = 0; i < 6; i++) if (fabs(newpose(i)) != 0) { - can_center = true; + tracking_started = true; break; } } - if (can_center) + if (get(f_center) && tracking_started) { set(f_center, false); @@ -260,10 +257,7 @@ void Tracker::logic() const euler_t rot = r2d * c_mult * rmat_to_euler(rotation); euler_t pos = euler_t(&value[TX]) - t_center; - if (s.use_camera_offset_from_centering) - t_compensate((real_rotation.camera * real_rotation.rot_center).t(), pos, pos, false, false, false); - else - t_compensate(real_rotation.camera.t(), pos, pos, false, false, false); + t_compensate(real_rotation.camera.t(), pos, pos, false, false, false); for (int i = 0; i < 3; i++) { @@ -452,25 +446,34 @@ void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const void bits::set(bits::flags flag_, bool val_) { - unsigned b_(b); const unsigned flag = unsigned(flag_); - const unsigned val = unsigned(!!val_); - while (!b.compare_exchange_weak(b_, + const unsigned val = unsigned(val_); + + for (;;) + { + unsigned b_(b); + if (b.compare_exchange_weak(b_, unsigned((b_ & ~flag) | (flag * val)), std::memory_order_seq_cst, std::memory_order_seq_cst)) - { /* empty */ } + break; + } } void bits::negate(bits::flags flag_) { - unsigned b_(b); const unsigned flag = unsigned(flag_); - while (!b.compare_exchange_weak(b_, - (b_ & ~flag) | (flag & ~b_), + + for (;;) + { + unsigned b_(b); + + if (b.compare_exchange_weak(b_, + b_ ^ flag, std::memory_order_seq_cst, std::memory_order_seq_cst)) - { /* empty */ } + break; + } } bool bits::get(bits::flags flag) diff --git a/logic/tracker.h b/logic/tracker.h index b9012910..4a5bdf50 100644 --- a/logic/tracker.h +++ b/logic/tracker.h @@ -29,6 +29,8 @@ #include "export.hpp" +namespace gui_tracker_impl { + using Pose = Mat<double, 6, 1>; struct bits @@ -86,6 +88,8 @@ private: long backlog_time; + bool tracking_started; + double map(double pos, Map& axis); void logic(); void t_compensate(const rmat& rmat, const euler_t& ypr, euler_t& output, @@ -115,3 +119,7 @@ public: void zero() { negate(f_zero); } void toggle_enabled() { negate(f_enabled); } }; + +} // ns impl + +using gui_tracker_impl::Tracker; |