summaryrefslogtreecommitdiffhomepage
path: root/opentrack/tracker.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-10-19 16:08:46 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-10-19 18:28:08 +0200
commitdd87370ff2818ee3c3d67ba104405b85ee44daba (patch)
treed5ebdf22aaccd0e345ce7a7c2ecb7b7193ea0791 /opentrack/tracker.cpp
parent7e2e341c45b86f76e08d74e056929ad76ff1a383 (diff)
decruft more
Diffstat (limited to 'opentrack/tracker.cpp')
-rw-r--r--opentrack/tracker.cpp187
1 files changed, 187 insertions, 0 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
new file mode 100644
index 00000000..41db7c19
--- /dev/null
+++ b/opentrack/tracker.cpp
@@ -0,0 +1,187 @@
+/* Copyright (c) 2012-2013 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 <opencv2/core/core.hpp>
+
+#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),
+ centerp(false),
+ enabledp(true),
+ should_quit(false),
+ libs(libs)
+{
+}
+
+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;
+ double invert = axis.opts.invert ? -1 : 1;
+ return invert * (fc.getValue(pos) + axis.opts.zero);
+}
+
+static cv::Matx33d euler_to_rmat(const double* input)
+{
+ static constexpr double pi = 3.141592653;
+ const auto H = input[0] * pi / -180;
+ const auto P = input[1] * pi / -180;
+ const auto B = input[2] * pi / 180;
+
+ const auto cosH = cos(H);
+ const auto sinH = sin(H);
+ const auto cosP = cos(P);
+ const auto sinP = sin(P);
+ const auto cosB = cos(B);
+ const auto sinB = sin(B);
+
+ double foo[] = {
+ cosH * cosB - sinH * sinP * sinB,
+ - sinB * cosP,
+ sinH * cosB + cosH * sinP * sinB,
+ cosH * sinB + sinH * sinP * cosB,
+ cosB * cosP,
+ sinB * sinH - cosH * sinP * cosB,
+ - sinH * cosP,
+ - sinP,
+ cosH * cosP,
+ };
+
+ return cv::Matx33d(foo);
+}
+
+void Tracker::t_compensate(const double* input, double* output, bool rz)
+{
+ const cv::Matx33d rmat = euler_to_rmat(&input[Yaw]);
+ const cv::Vec3d tvec(input);
+ const cv::Vec3d ret = rmat * tvec;
+
+ const int max = !rz ? 3 : 2;
+
+ for (int i = 0; i < max; i++)
+ output[i] = ret(i);
+}
+
+void Tracker::logic()
+{
+ libs.pTracker->data(newpose);
+
+ Pose final_raw;
+
+ if (enabledp)
+ {
+ for (int i = 0; i < 6; i++)
+ {
+ auto& axis = m(i);
+ int k = axis.opts.src;
+ if (k < 0 || k >= 6)
+ {
+ final_raw(i) = 0;
+ continue;
+ }
+ // not really raw, after axis remap -sh
+ final_raw(i) = newpose[k];
+ }
+ unstopped_raw = final_raw;
+ }
+
+ Pose filtered_pose;
+
+ if (libs.pFilter)
+ libs.pFilter->filter(final_raw, filtered_pose);
+ else
+ filtered_pose = final_raw;
+
+ if (centerp)
+ {
+ centerp = false;
+ raw_center = final_raw;
+ }
+
+ Pose raw_centered = filtered_pose & raw_center;
+
+ Pose mapped_pose_precomp;
+
+ for (int i = 0; i < 6; i++)
+ mapped_pose_precomp(i) = map(raw_centered(i), m(i));
+
+ Pose mapped_pose;
+
+ mapped_pose = mapped_pose_precomp;
+ if (s.tcomp_p)
+ t_compensate(mapped_pose_precomp, mapped_pose, s.tcomp_tz);
+
+ libs.pProtocol->pose(mapped_pose);
+
+ {
+ QMutexLocker foo(&mtx);
+ output_pose = mapped_pose;
+ raw_6dof = unstopped_raw;
+ }
+}
+
+void Tracker::run() {
+ const int sleep_ms = 3;
+
+#if defined(_WIN32)
+ (void) timeBeginPeriod(1);
+#endif
+
+ while (!should_quit)
+ {
+ t.start();
+
+ logic();
+
+ double q = sleep_ms * 1000L;
+ q -= t.elapsed();
+ q = std::max(0., q);
+ usleep((long)q);
+ }
+
+#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);
+ }
+}
+