summaryrefslogtreecommitdiffhomepage
path: root/opentrack/tracker.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'opentrack/tracker.cpp')
-rw-r--r--opentrack/tracker.cpp126
1 files changed, 95 insertions, 31 deletions
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index a9e60555..ad10e396 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -62,6 +62,44 @@ void Tracker::t_compensate(const rmat& rmat, const double* xyz, double* output,
output[0] = -ret(1);
}
+static inline bool nanp(double value)
+{
+ return std::isnan(value) || std::isinf(value);
+}
+
+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] = {
@@ -72,12 +110,12 @@ void Tracker::logic()
m(4).opts.invert,
m(5).opts.invert,
};
-
+
static constexpr double pi = 3.141592653;
static constexpr double r2d = 180. / pi;
-
+
Pose value, raw;
-
+
for (int i = 0; i < 6; i++)
{
auto& axis = m(i);
@@ -89,6 +127,9 @@ void Tracker::logic()
raw(i) = newpose[i];
}
+ if (is_nan(raw))
+ raw = last_raw;
+
const double off[] = {
(double)-s.camera_yaw,
(double)-s.camera_pitch,
@@ -97,12 +138,13 @@ void Tracker::logic()
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;
-
- if (centerp)
+ const bool nan = is_nan(r, t);
+
+ if (centerp && !nan)
{
for (int i = 0; i < 6; i++)
if (fabs(newpose[i]) != 0)
@@ -111,7 +153,7 @@ void Tracker::logic()
break;
}
}
-
+
if (can_center)
{
if (libs.pFilter)
@@ -121,7 +163,7 @@ void Tracker::logic()
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);
@@ -143,35 +185,57 @@ void Tracker::logic()
value(i+3) = euler(i) * r2d;
}
}
-
+
+ bool nan_ = false;
+ // we're checking NaNs after every block of numeric ops
+ 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(rmat::euler_to_rmat(&value[Yaw]),
- value,
- value,
- s.tcomp_tz);
+ for (int i = 0; i < 6; i++)
+ value(i) = map(value(i), m(i));
- for (int i = 0; i < 6; i++)
- value(i) += m(i).opts.zero;
+ 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.;
+ for (int i = 0; i < 6; i++)
+ value(i) += m(i).opts.zero;
- if (zero_)
for (int i = 0; i < 6; i++)
- value(i) = 0;
+ 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;
@@ -179,7 +243,7 @@ void Tracker::logic()
void Tracker::run() {
const int sleep_ms = 3;
-
+
#if defined(_WIN32)
(void) timeBeginPeriod(1);
#endif
@@ -187,14 +251,14 @@ void Tracker::run() {
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] = tmp[i];
-
+ newpose[i] = elide_nan(tmp[i], newpose[i]);
+
logic();
long q = sleep_ms * 1000L - t.elapsed()/1000L;