logic/tracker: cleanup gimbal lock fix

There were too many cases of variables with a "_real" suffix and without. Move
both cases into struct data members.

Also add experimental camera centering method.

Cleanup some other stuff like "is_nan" should take an arbitrary Mat.

2 files changed, 92 insertions, 71 deletions

diff --git a/logic/tracker.cpp b/logic/tracker.cpp
index c7580500..92c9bcbf 100644
--- a/logic/tracker.cpp
+++ b/logic/tracker.cpp
@@ -28,9 +28,6 @@ Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) :
     newpose {0,0,0, 0,0,0},
     libs(libs),
     logger(logger),
-    r_b(get_camera_offset_matrix(c_div).t()),
-    r_b_real(get_camera_offset_matrix(1).t()),
-    t_b {0,0,0},
     centerp(s.center_at_startup),
     enabledp(true),
     zero_(false),
@@ -90,25 +87,14 @@ static inline double elide_nan(double value, double def)
     return value;
 }
 
-static bool is_nan(const dmat<3,3>& r, const dmat<3, 1>& t)
+template<int u, int w>
+static bool is_nan(const dmat<u,w>& r)
 {
-    for (int i = 0; i < 3; i++)
-        for (int j = 0; j < 3; j++)
-            if (nanp(r(i, j)))
+    for (int i = 0; i < u; i++)
+        for (int j = 0; j < w; j++)
+            if (nanp(r(u, w)))
                 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;
 }
 
@@ -140,83 +126,107 @@ void Tracker::logic()
     if (is_nan(raw))
         raw = last_raw;
 
-    // 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 = d2r * euler_t(&value[Yaw]);
-        r = euler_to_rmat(c_div * tmp);
-        r_real = euler_to_rmat(tmp);
+        scaled_rotation.rotation = euler_to_rmat(c_div * tmp);
+        real_rotation.rotation = euler_to_rmat(tmp);
+        tait_bryan_to_matrices(c_div * tmp, scaled_rotation.rr, scaled_rotation.ry, scaled_rotation.rp);
     }
 
-    euler_t t(value(0), value(1), value(2));
-
-    bool do_center_now = false;
-    bool nan = is_nan(r, t);
+    scaled_rotation.camera = get_camera_offset_matrix(c_div);
+    real_rotation.camera = get_camera_offset_matrix(1);
 
-    if (centerp && !nan)
-    {
-        for (int i = 0; i < 6; i++)
-            if (fabs(newpose[i]) != 0)
-            {
-                do_center_now = true;
-                break;
-            }
-    }
+    scaled_rotation.rotation = scaled_rotation.camera * scaled_rotation.rotation;
+    real_rotation.rotation = real_rotation.camera * real_rotation.rotation;
 
-    const rmat cam = get_camera_offset_matrix(c_div);
-    const rmat cam_real = get_camera_offset_matrix(1);
+    bool nanp = is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation);
 
-    r = r * cam;
-    r_real = r_real * cam_real;
-
-    if (do_center_now)
+    if (!nanp)
     {
-        centerp = false;
-
-        if (libs.pFilter)
-            libs.pFilter->center();
+        bool can_center = false;
 
-        if (libs.pTracker->center())
+        if (centerp && !nanp)
         {
-            r_b = cam.t();
-            r_b_real = cam_real.t();
-            r = rmat::eye();
-            r_real = rmat::eye();
+            for (int i = 0; i < 6; i++)
+                if (fabs(newpose[i]) != 0)
+                {
+                    can_center = true;
+                    break;
+                }
         }
-        else
+
+        if (can_center)
         {
-            r_b = r.t();
-            r_b_real = r_real.t();
-        }
 
-        for (int i = 0; i < 3; i++)
-            t_b[i] = t(i);
+            centerp = false;
+
+            if (libs.pFilter)
+                libs.pFilter->center();
+
+            if (libs.pTracker->center())
+            {
+                scaled_rotation.rotation = scaled_rotation.camera.t();
+                real_rotation.rotation = real_rotation.camera.t();
+
+                scaled_rotation.rotation = rmat::eye();
+                real_rotation.rotation = rmat::eye();
+            }
+            else
+            {
+                euler::tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation),
+                                              scaled_rotation.center_roll,
+                                              scaled_rotation.center_pitch,
+                                              scaled_rotation.center_yaw);
+                euler::tait_bryan_to_matrices(rmat_to_euler(real_rotation.rotation),
+                                              real_rotation.center_roll,
+                                              real_rotation.center_pitch,
+                                              real_rotation.center_yaw);
+                real_rotation.rot_center = real_rotation.rotation.t();
+                scaled_rotation.rot_center = scaled_rotation.rotation.t();
+            }
+
+            t_center = euler_t(&value(TX));
+        }
     }
 
     {
+        rmat rotation;
+
         switch (s.center_method)
         {
         // inertial
         case 0:
         default:
-            r = r_b * r;
+            //scaled_rotation.rotation = scaled_rotation
+            rotation = scaled_rotation.rot_center * scaled_rotation.rotation;
             break;
         // camera
         case 1:
-            r = r * r_b;
+            rotation = scaled_rotation.rotation * scaled_rotation.rot_center;
+            break;
+        // alternative camera
+        case 2:
+            rmat cy, cp, cr;
+            tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation), cr, cp, cy);
+
+            rmat ry = cy * scaled_rotation.center_yaw.t();
+            rmat rp = cp * scaled_rotation.center_pitch.t();
+            rmat rr = cr * scaled_rotation.center_roll.t();
+
+            // roll yaw pitch
+            rotation = rr * ry * rp;
             break;
         }
 
-        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]);
+        const euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
+        euler_t pos = euler_t(&value[TX]) - t_center;
 
         if (s.use_camera_offset_from_centering)
-            t_compensate(r_b_real.t() * cam_real.t(), pos, pos, false);
+            t_compensate(real_rotation.rot_center.t() * real_rotation.camera.t(), pos, pos, false);
         else
-            t_compensate(cam_real.t(), pos, pos, false);
+            t_compensate(real_rotation.camera.t(), pos, pos, false);
 
         for (int i = 0; i < 3; i++)
         {
@@ -230,7 +240,7 @@ void Tracker::logic()
     // whenever something can corrupt its internal state due to nan/inf, elide the call
     if (is_nan(value))
     {
-        nan = true;
+        nanp = true;
         logger.write_pose(value); // "filtered"
     }
     else
@@ -258,7 +268,7 @@ void Tracker::logic()
                 value(i) = 0;
 
         if (is_nan(value))
-            nan = true;
+            nanp = true;
     }
 
     if (s.tcomp_p)
@@ -268,8 +278,8 @@ void Tracker::logic()
                      value_,
                      value_,
                      s.tcomp_tz);
-        if (is_nan(r, value_))
-            nan = true;
+        if (is_nan(value_))
+            nanp = true;
         for (int i = 0; i < 3; i++)
             value(i) = value_(i);
     }
@@ -280,7 +290,7 @@ void Tracker::logic()
 
     logger.write_pose(value); // "mapped"
 
-    if (nan)
+    if (nanp)
     {
         value = last_mapped;
 
diff --git a/logic/tracker.h b/logic/tracker.h
index d3c2660b..6e5faddf 100644
--- a/logic/tracker.h
+++ b/logic/tracker.h
@@ -50,8 +50,19 @@ private:
     // the logger while the tracker is running.
     TrackLogger &logger;
 
-    rmat r_b, r_b_real;
-    double t_b[3];
+    struct state
+    {
+        rmat center_yaw, center_pitch, center_roll;
+        rmat rot_center;
+        rmat camera;
+        rmat rotation, ry, rp, rr;
+
+        state() : center_yaw(rmat::eye()), center_pitch(rmat::eye()), center_roll(rmat::eye()), rot_center(rmat::eye())
+        {}
+    };
+
+    state real_rotation, scaled_rotation;
+    euler_t t_center;
 
     volatile bool centerp;
     volatile bool enabledp;