summaryrefslogtreecommitdiffhomepage
path: root/opentrack/tracker.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2016-06-16 12:34:31 +0200
committerStanislaw Halik <sthalik@misaki.pl>2016-06-16 12:35:51 +0200
commit0760fe011114fa440275b487eaf766b015f40e5b (patch)
treea0141fb291b7dc1e38d16dd0eb2c767ec93b310a /opentrack/tracker.cpp
parent60460f56cabe0155996adf8ba5e9f6730ef0b203 (diff)
all: split "api" into "api" and "logic"
Here, the "logic" module has all the stuff for building one's own graphical user interface. The "api" module has stuff used by other trackers. While at it, each of "api", "logic", and "compat" need their own export headers. This is because of preprocessor symbol clashes. This is all because a change in the "gui"-only dependency required a relink of all the trackers, protocols, and flters. It takes too long when building in the release configuration. With the split, only the "gui" module gets rebuilt. Since it has close to no static dependencies, it's fast enough.
Diffstat (limited to 'opentrack/tracker.cpp')
-rw-r--r--opentrack/tracker.cpp305
1 files changed, 0 insertions, 305 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
deleted file mode 100644
index 9d754324..00000000
--- a/opentrack/tracker.cpp
+++ /dev/null
@@ -1,305 +0,0 @@
-/* Copyright (c) 2012-2015 Stanislaw Halik <sthalik@misaki.pl>
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- */
-
-/*
- * this file appeared originally in facetracknoir, was rewritten completely
- * following opentrack fork.
- *
- * originally written by Wim Vriend.
- */
-
-
-#include "tracker.h"
-#include <cmath>
-#include <algorithm>
-
-#if defined(_WIN32)
-# include <windows.h>
-#endif
-
-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}
-{
-}
-
-Tracker::~Tracker()
-{
- should_quit = true;
- wait();
-}
-
-double Tracker::map(double pos, Mapping& axis)
-{
- bool altp = (pos < 0) && axis.opts.altp;
- axis.curve.setTrackingActive( !altp );
- axis.curveAlt.setTrackingActive( altp );
- auto& fc = altp ? axis.curveAlt : axis.curve;
- return fc.getValue(pos);
-}
-
-void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* 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;
- if (!rz)
- output[2] = ret(0);
- else
- output[2] = xyz[2];
- output[1] = -ret(2);
- output[0] = -ret(1);
-}
-
-#ifdef _WIN32
-__declspec(noinline) bool nanp(double value);
-#elif defined(__GNUC__)
-bool __attribute__ ((noinline)) nanp(double value);
-#else
-bool nanp(double value);
-#endif
-
-static inline double elide_nan(double value, double def)
-{
- if (nanp(value))
- {
- if (nanp(def))
- return 0;
- return def;
- }
- return value;
-}
-
-static bool is_nan(const dmat<3,3>& r, const dmat<3, 1>& t)
-{
- for (int i = 0; i < 3; i++)
- for (int j = 0; j < 3; j++)
- if (nanp(r(i, j)))
- 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;
-}
-
-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;
-
- for (int i = 0; i < 6; i++)
- {
- auto& axis = m(i);
- int k = axis.opts.src;
- if (k < 0 || k >= 6)
- value(i) = 0;
- else
- 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 = euler_to_rmat(off);
- rmat r = euler_to_rmat(&value[Yaw]);
- euler_t t(value(0), value(1), value(2));
-
- r = cam * r;
-
- bool can_center = false;
- const bool nan = is_nan(r, t);
-
- if (centerp && !nan)
- {
- for (int i = 0; i < 6; i++)
- if (fabs(newpose[i]) != 0)
- {
- can_center = true;
- break;
- }
- }
-
- if (can_center)
- {
- if (libs.pFilter)
- libs.pFilter->center();
- centerp = false;
- for (int i = 0; i < 3; i++)
- t_b[i] = t(i);
- r_b = r;
- }
-
- {
- 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 m_;
- switch (s.center_method)
- {
- case 0:
- default:
- m_ = r * r_b.t();
- break;
- case 1:
- m_ = r_b.t() * r;
- }
-
- const euler_t euler = rmat_to_euler(m_);
-
- for (int i = 0; i < 3; i++)
- {
- value(i) = tmp[i];
- value(i+3) = euler(i) * r2d;
- }
- }
-
- bool nan_ = false;
- // whenever something can corrupt its internal state due to nan/inf, elide the call
- if (is_nan(value))
- {
- nan_ = true;
- }
- else
- {
- {
- Pose tmp = value;
-
- if (libs.pFilter)
- libs.pFilter->filter(tmp, value);
- }
-
- for (int i = 0; i < 6; i++)
- value(i) = map(value(i), m(i));
-
- if (s.tcomp_p)
- t_compensate(euler_to_rmat(&value[Yaw]),
- value,
- value,
- s.tcomp_tz);
-
- for (int i = 0; i < 6; i++)
- value(i) += m(i).opts.zero;
-
- for (int i = 0; i < 6; i++)
- value[i] *= inverts[i] ? -1. : 1.;
-
- if (zero_)
- for (int i = 0; i < 6; i++)
- value(i) = 0;
-
- if (is_nan(value))
- nan_ = true;
- }
-
- if (nan_)
- {
- value = last_mapped;
-
- // for widget last value display
- for (int i = 0; i < 6; i++)
- (void) map(value(i), m(i));
- }
-
- libs.pProtocol->pose(value);
-
- last_mapped = value;
- last_raw = raw;
-
- QMutexLocker foo(&mtx);
- output_pose = value;
- raw_6dof = raw;
-}
-
-void Tracker::run()
-{
- const int sleep_ms = 3;
-
-#if defined(_WIN32)
- (void) timeBeginPeriod(1);
-#endif
-
- while (!should_quit)
- {
- t.start();
-
- double tmp[6] {0,0,0, 0,0,0};
- libs.pTracker->data(tmp);
-
- if (enabledp)
- for (int i = 0; i < 6; i++)
- newpose[i] = elide_nan(tmp[i], newpose[i]);
-
- logic();
-
- long q = sleep_ms * 1000L - t.elapsed()/1000L;
- using std::max;
- usleep(max(1L, q));
- }
-
- {
- // filter may inhibit exact origin
- Pose p;
- libs.pProtocol->pose(p);
- }
-
-#if defined(_WIN32)
- (void) timeEndPeriod(1);
-#endif
-
- for (int i = 0; i < 6; i++)
- {
- m(i).curve.setTrackingActive(false);
- m(i).curveAlt.setTrackingActive(false);
- }
-}
-
-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);
- mapped[i] = output_pose(i);
- }
-}
-