Merge branch 'unstable' into trackhattrackhat-1.1p2

* unstable:
  cmake: update toolchain file
  shortcuts: fix osx/linux keystrokes persisting
  cmake: fix copy-paste comment
  cmake: add toolchain file for OSX
  cmake: add osx policy to make it shutup
  x-plane: ignore diagnostic
  osx: nix warning
  gitattributes: more text extensions to eol=lf
  cmake: timestamp logic simplify/fix
  cmake: no timestamp for tag builds
  all: update copyright where appropriate
  all: comments only
  cmake: regen before making tarball
  cmake: fix dropbox share invocation
  tracker: initialize newpose
  pt: use previous pose on NaN result from POSIT
  accela: also don't poison ewma state with nans
  cmake: fix tarball invocation
  accela: elide NaN output values
  qfc: elide NaN values
  pt: reformat more
  pt: reformat posit
  pt: refactor auto threshold somewhat
  pt: rename ill-chosen name
  pt: switch min/max point size to reals
  cmake: upload tarball to Dropbox but only if I'm the user
  cmake: add tarball timestamp so it gets rebuilt
  cmake: don't regen version if none changed
  cmake: fix git describe --dirty
  cmake: retab git module
  fix tarball target
  cmake: mark dirty tree
  cmake: regen tarball even if exists
  cmake: generate version.cc
  tracker: check for NaN values
  accela: don't check NaNs in filter, wrong place
  cmake: drop -ffast-math, allow for NaN check
  shortcuts: actually print screen binding works
  shortcuts: alias right modifier keys to left modifier keys
  shortcuts: allow for binding scroll lock and pause/break
  accela: also filter out NaNs on tracking start
  allow for filter immediate center

1 files changed, 95 insertions, 28 deletions

diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index 8a9a0511..1bc28b87 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -24,6 +24,7 @@
 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),
@@ -62,6 +63,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 +111,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 +128,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 +139,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,15 +154,17 @@ void Tracker::logic()
                 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);
@@ -141,32 +186,54 @@ 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] *= inverts[i] ? -1. : 1.;
+        if (s.tcomp_p)
+            t_compensate(rmat::euler_to_rmat(&value[Yaw]),
+                         value,
+                         value,
+                         s.tcomp_tz);
 
-    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;
@@ -174,7 +241,7 @@ void Tracker::logic()
 
 void Tracker::run() {
     const int sleep_ms = 3;
-    
+
 #if defined(_WIN32)
     (void) timeBeginPeriod(1);
 #endif
@@ -182,14 +249,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;