diff options
Diffstat (limited to 'opentrack/tracker.cpp')
| -rwxr-xr-x | opentrack/tracker.cpp | 264 |
1 files changed, 151 insertions, 113 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp index 8313c8fc..d37aea93 100755 --- a/opentrack/tracker.cpp +++ b/opentrack/tracker.cpp @@ -12,35 +12,44 @@ * originally written by Wim Vriend. */ - +#include "opentrack-compat/sleep.hpp" +#include "opentrack-compat/nan.hpp" #include "tracker.h" + #include <cmath> #include <algorithm> +#include <cstdio> #if defined(_WIN32) # include <windows.h> #endif -Tracker::Tracker(main_settings& s, Mappings &m, SelectedLibraries &libs) : +Tracker::Tracker(main_settings& s, Mappings& m, SelectedLibraries& libs) : s(s), m(m), - newpose {0,0,0, 0,0,0}, - centerp(s.center_at_startup), - enabledp(true), - zero_(false), - should_quit(false), - libs(libs), - r_b(dmat<3,3>::eye()), - t_b {0,0,0} + libs(libs) { + set(f_center, s.center_at_startup); } Tracker::~Tracker() { - should_quit = true; + set(f_should_quit, true); wait(); } +Tracker::rmat Tracker::get_camera_offset_matrix(double c) +{ + const double off[] = + { + d2r * c * (double)-s.camera_yaw, + d2r * c * (double)-s.camera_pitch, + d2r * c * (double)-s.camera_roll + }; + + return euler::euler_to_rmat(off); +} + double Tracker::map(double pos, Mapping& axis) { bool altp = (pos < 0) && axis.opts.altp; @@ -50,22 +59,16 @@ double Tracker::map(double pos, Mapping& axis) return fc.getValue(pos); } -void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output, bool rz) +void Tracker::t_compensate(const rmat& rmat, const euler_t& xyz_, euler_t& output, bool rz) { // TY is really yaw axis. need swapping accordingly. - dmat<3, 1> tvec( xyz[2], -xyz[0], -xyz[1] ); - const dmat<3, 1> ret = rmat * tvec; + const euler_t ret = rmat * euler_t(xyz_(TZ), -xyz_(TX), -xyz_(TY)); if (!rz) - output[2] = ret(0); + output(2) = ret(0); else - output[2] = xyz[2]; - output[1] = -ret(2); - output[0] = -ret(1); -} - -static inline bool nanp(double value) -{ - return std::isnan(value) || std::isinf(value); + output(2) = xyz_(2); + output(1) = -ret(2); + output(0) = -ret(1); } static inline double elide_nan(double value, double def) @@ -79,41 +82,23 @@ static inline double elide_nan(double value, double def) return value; } -static bool is_nan(const dmat<3,3>& r, const dmat<3, 1>& t) +template<int u, int w> +static bool is_nan(const dmat<u,w>& r) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (nanp(r(i, j))) + for (int i = 0; i < u; i++) + for (int j = 0; j < w; j++) + if (nanp(r(u, w))) return true; - for (int i = 0; i < 3; i++) - if (nanp(t(i))) - return true; - return false; } -static bool is_nan(const Pose& value) -{ - for (int i = 0; i < 6; i++) - if (nanp(value(i))) - return true; - return false; -} +constexpr double Tracker::c_mult; +constexpr double Tracker::c_div; void Tracker::logic() { - bool inverts[6] = { - m(0).opts.invert, - m(1).opts.invert, - m(2).opts.invert, - m(3).opts.invert, - m(4).opts.invert, - m(5).opts.invert, - }; - - static constexpr double pi = 3.141592653; - static constexpr double r2d = 180. / pi; + using namespace euler; Pose value, raw; @@ -124,93 +109,132 @@ void Tracker::logic() if (k < 0 || k >= 6) value(i) = 0; else - value(i) = newpose[k]; - raw(i) = newpose[i]; + value(i) = newpose(k); + raw(i) = newpose(i); } if (is_nan(raw)) raw = last_raw; - const double off[] = { - (double)-s.camera_yaw, - (double)-s.camera_pitch, - (double)s.camera_roll - }; - const rmat cam = rmat::euler_to_rmat(off); - rmat r = rmat::euler_to_rmat(&value[Yaw]); - dmat<3, 1> t(value(0), value(1), value(2)); - - r = cam * r; - - bool can_center = false; - const bool nan = is_nan(r, t); + // TODO split this function, it's too big - if (centerp && !nan) { - for (int i = 0; i < 6; i++) - if (fabs(newpose[i]) != 0) - { - can_center = true; - break; - } + euler_t tmp = d2r * euler_t(&value[Yaw]); + scaled_rotation.rotation = euler_to_rmat(c_div * tmp); + real_rotation.rotation = euler_to_rmat(tmp); + tait_bryan_to_matrices(c_div * tmp, scaled_rotation.rr, scaled_rotation.ry, scaled_rotation.rp); } - if (can_center) + 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) { - if (libs.pFilter) - libs.pFilter->center(); - centerp = false; - for (int i = 0; i < 3; i++) - t_b[i] = t(i); - r_b = r; + bool can_center = false; + + if (get(f_center) && !nanp) + { + using std::fabs; + + for (int i = 0; i < 6; i++) + if (fabs(newpose(i)) != 0) + { + can_center = true; + break; + } + } + + if (can_center) + { + set(f_center, false); + + if (libs.pFilter) + libs.pFilter->center(); + + euler::tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation), + scaled_rotation.center_roll, + scaled_rotation.center_pitch, + scaled_rotation.center_yaw); +#if 0 + euler::tait_bryan_to_matrices(rmat_to_euler(real_rotation.rotation), + real_rotation.center_roll, + real_rotation.center_pitch, + real_rotation.center_yaw); +#endif + real_rotation.rot_center = real_rotation.rotation.t(); + scaled_rotation.rot_center = scaled_rotation.rotation.t(); + + t_center = euler_t(&value(TX)); + } } { - double tmp[3] = { t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2] }; - t_compensate(cam, tmp, tmp, false); + rmat rotation; + + rotation = scaled_rotation.rotation * scaled_rotation.rot_center; + + const euler_t rot = r2d * c_mult * rmat_to_euler(rotation); + euler_t pos = euler_t(&value[TX]) - t_center; + + t_compensate(real_rotation.camera.t(), pos, pos, false); - const dmat<3, 1> euler = rmat::rmat_to_euler(r * r_b.t()); for (int i = 0; i < 3; i++) { - value(i) = tmp[i]; - value(i+3) = euler(i) * r2d; + value(i) = pos(i); + value(i+3) = rot(i); } } - bool nan_ = false; - // we're checking NaNs after every block of numeric ops + // whenever something can corrupt its internal state due to nan/inf, elide the call if (is_nan(value)) { - nan_ = true; + nanp = true; } else { - Pose tmp = value; + Pose tmp(value); if (libs.pFilter) libs.pFilter->filter(tmp, value); - for (int i = 0; i < 6; i++) + // CAVEAT rotation only, due to tcomp + for (int i = 3; i < 6; i++) value(i) = map(value(i), m(i)); - if (s.tcomp_p) - t_compensate(rmat::euler_to_rmat(&value[Yaw]), - value, - value, - s.tcomp_tz); - - for (int i = 0; i < 6; i++) - value[i] *= inverts[i] ? -1. : 1.; - - if (zero_) + if (get(f_zero)) for (int i = 0; i < 6; i++) value(i) = 0; if (is_nan(value)) - nan_ = true; + nanp = true; + } + + if (s.tcomp_p && !get(f_tcomp_disabled)) + { + euler_t value_(value(TX), value(TY), value(TZ)); + t_compensate(euler_to_rmat(euler_t(value(Yaw) * d2r, value(Pitch) * d2r, value(Roll) * d2r)), + value_, + value_, + s.tcomp_tz); + if (is_nan(value_)) + nanp = true; + for (int i = 0; i < 3; i++) + value(i) = value_(i); } - if (nan_) + // CAVEAT translation only, due to tcomp + for (int i = 0; i < 3; i++) + value(i) = map(value(i), m(i)); + + for (int i = 0; i < 6; i++) + value(i) *= int(m(i).opts.invert) * -2 + 1; + + if (nanp) { value = last_mapped; @@ -229,28 +253,40 @@ void Tracker::logic() raw_6dof = raw; } -void Tracker::run() { - const int sleep_ms = 3; - +void Tracker::run() +{ #if defined(_WIN32) (void) timeBeginPeriod(1); #endif - while (!should_quit) - { - t.start(); + setPriority(QThread::HighPriority); + + t.start(); - double tmp[6] {0,0,0, 0,0,0}; + while (!get(f_should_quit)) + { + Pose tmp; libs.pTracker->data(tmp); - - if (enabledp) + + if (get(f_enabled)) for (int i = 0; i < 6; i++) - newpose[i] = elide_nan(tmp[i], newpose[i]); + newpose[i] = elide_nan(tmp(i), newpose(i)); logic(); - long q = sleep_ms * 1000L - t.elapsed()/1000L; - usleep(std::max(1L, q)); + static constexpr long const_sleep_us = 4000; + + using std::max; + using std::min; + + const long elapsed_usecs = t.elapsed_usecs(); + const long sleep_us = const_sleep_us * 2 - elapsed_usecs; + + const unsigned sleep_time = unsigned(max(1l, min(const_sleep_us * 4, max(1l, (sleep_us + 200l)/1000l)))); + + t.start(); + + portable::sleep(unsigned(max(1u, sleep_time))); } { @@ -270,8 +306,10 @@ void Tracker::run() { } } -void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const { +void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const +{ QMutexLocker foo(&const_cast<Tracker&>(*this).mtx); + for (int i = 0; i < 6; i++) { raw[i] = raw_6dof(i); |