diff options
| -rw-r--r-- | opentrack-logic/tracker.cpp | 71 | ||||
| -rw-r--r-- | opentrack-logic/tracker.h | 8 |
2 files changed, 52 insertions, 27 deletions
diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp index f29e3100..f36591e4 100644 --- a/opentrack-logic/tracker.cpp +++ b/opentrack-logic/tracker.cpp @@ -30,7 +30,8 @@ Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) : should_quit(false), libs(libs), logger(logger), - r_b(get_camera_offset_matrix().t()), + r_b(get_camera_offset_matrix(c_div).t()), + r_b_real(get_camera_offset_matrix(1).t()), t_b {0,0,0} { } @@ -41,13 +42,13 @@ Tracker::~Tracker() wait(); } -Tracker::rmat Tracker::get_camera_offset_matrix() +Tracker::rmat Tracker::get_camera_offset_matrix(double c) { const double off[] = { - d2r * (double)-s.camera_yaw, - d2r * (double)-s.camera_pitch, - d2r * (double)-s.camera_roll + d2r * c * (double)-s.camera_yaw, + d2r * c * (double)-s.camera_pitch, + d2r * c * (double)-s.camera_roll }; return euler::euler_to_rmat(off); @@ -109,6 +110,9 @@ static bool is_nan(const Pose& value) return false; } +constexpr double Tracker::c_mult; +constexpr double Tracker::c_div; + void Tracker::logic() { bool inverts[6] = { @@ -140,12 +144,15 @@ void Tracker::logic() if (is_nan(raw)) raw = last_raw; - rmat r; + // TODO fix gimbal lock by dividing euler angle input by >=3. + // maintain the real rmat separately for translation compensation + // TODO split this function, it's too big + rmat r, r_real; { - euler_t tmp(&value[Yaw]); - tmp = d2r * tmp; - r = euler_to_rmat(&tmp[0]); + euler_t tmp = d2r * euler_t(&value[Yaw]); + r = euler_to_rmat(c_div * tmp); + r_real = euler_to_rmat(tmp); } euler_t t(value(0), value(1), value(2)); @@ -163,19 +170,34 @@ void Tracker::logic() } } - rmat cam = get_camera_offset_matrix(); + const rmat cam = get_camera_offset_matrix(c_div); + const rmat cam_real = get_camera_offset_matrix(1); r = r * cam; + r_real = r_real * cam_real; if (do_center_now) { + centerp = false; + if (libs.pFilter) libs.pFilter->center(); - centerp = false; + + if (libs.pTracker->center()) + { + r_b = cam.t(); + r_b_real = cam_real.t(); + r = rmat::eye(); + r_real = rmat::eye(); + } + else + { + r_b = r.t(); + r_b_real = r_real.t(); + } + for (int i = 0; i < 3; i++) t_b[i] = t(i); - - r_b = r.t(); } { @@ -184,29 +206,26 @@ void Tracker::logic() // inertial case 0: default: - r = r * r_b; + r = r_b * r; break; // camera case 1: - euler_t degs = rmat_to_euler(r_b * r); - degs(2) = rmat_to_euler(r * r_b)(2); - r = euler_to_rmat(degs); + r = r * r_b; break; } - const euler_t euler = r2d * rmat_to_euler(r); - - euler_t tmp(t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2]); + const euler_t rot = r2d * c_mult * rmat_to_euler(r); + euler_t pos(t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2]); if (s.use_camera_offset_from_centering) - t_compensate(r_b.t() * cam.t(), tmp, tmp, false); + t_compensate(r_b_real.t() * cam_real.t(), pos, pos, false); else - t_compensate(cam.t(), tmp, tmp, false); + t_compensate(cam_real.t(), pos, pos, false); for (int i = 0; i < 3; i++) { - value(i) = tmp(i); - value(i+3) = euler(i); + value(i) = pos(i); + value(i+3) = rot(i); } } @@ -350,8 +369,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); diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h index 310bac31..09aca424 100644 --- a/opentrack-logic/tracker.h +++ b/opentrack-logic/tracker.h @@ -71,7 +71,7 @@ private: using rmat = euler::rmat; using euler_t = euler::euler_t; - rmat r_b; + rmat r_b, r_b_real; double t_b[3]; double map(double pos, Mapping& axis); @@ -82,11 +82,15 @@ private: static constexpr double pi = OPENTRACK_PI; static constexpr double r2d = 180. / OPENTRACK_PI; static constexpr double d2r = OPENTRACK_PI / 180.; + + // note: float exponent base is 2 + static constexpr double c_mult = 4; + static constexpr double c_div = 1./c_mult; public: Tracker(Mappings& m, SelectedLibraries& libs, TrackLogger &logger); ~Tracker(); - rmat get_camera_offset_matrix(); + rmat get_camera_offset_matrix(double c); void get_raw_and_mapped_poses(double* mapped, double* raw) const; void start() { QThread::start(); } void toggle_enabled() { enabledp = !enabledp; } |