From a497af1ad4298de4ae9fab4453a0164950c4faf4 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 6 Aug 2016 09:39:39 +0200
Subject: logic/tracker: fix gimbal lock in a crude way

Divide euler angle representation by four. Now 180 maps to 45. Our
conversion back from matrix to euler won't cause gimbal lock anymore. Of
course multiply back when it's time to map.

Keep the real representation for translation compensation.

The value of four got chosen since it's a multiply of two (IEEE float
exponent is base 2).
---
 opentrack-logic/tracker.cpp | 71 +++++++++++++++++++++++++++++----------------
 opentrack-logic/tracker.h   |  8 +++--
 2 files changed, 52 insertions(+), 27 deletions(-)

diff --git a/opentrack-logic/tracker.cpp b/opentrack-logic/tracker.cpp
index f29e3100..f36591e4 100644
--- a/opentrack-logic/tracker.cpp
+++ b/opentrack-logic/tracker.cpp
@@ -30,7 +30,8 @@ Tracker::Tracker(Mappings &m, SelectedLibraries &libs, TrackLogger &logger) :
     should_quit(false),
     libs(libs),
     logger(logger),
-    r_b(get_camera_offset_matrix().t()),
+    r_b(get_camera_offset_matrix(c_div).t()),
+    r_b_real(get_camera_offset_matrix(1).t()),
     t_b {0,0,0}
 {
 }
@@ -41,13 +42,13 @@ Tracker::~Tracker()
     wait();
 }
 
-Tracker::rmat Tracker::get_camera_offset_matrix()
+Tracker::rmat Tracker::get_camera_offset_matrix(double c)
 {
     const double off[] =
     {
-        d2r * (double)-s.camera_yaw,
-        d2r * (double)-s.camera_pitch,
-        d2r * (double)-s.camera_roll
+        d2r * c * (double)-s.camera_yaw,
+        d2r * c * (double)-s.camera_pitch,
+        d2r * c * (double)-s.camera_roll
     };
 
     return euler::euler_to_rmat(off);
@@ -109,6 +110,9 @@ static bool is_nan(const Pose& value)
     return false;
 }
 
+constexpr double Tracker::c_mult;
+constexpr double Tracker::c_div;
+
 void Tracker::logic()
 {
     bool inverts[6] = {
@@ -140,12 +144,15 @@ void Tracker::logic()
     if (is_nan(raw))
         raw = last_raw;
 
-    rmat r;
+    // 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(&value[Yaw]);
-        tmp = d2r * tmp;
-        r = euler_to_rmat(&tmp[0]);
+        euler_t tmp = d2r * euler_t(&value[Yaw]);
+        r = euler_to_rmat(c_div * tmp);
+        r_real = euler_to_rmat(tmp);
     }
 
     euler_t t(value(0), value(1), value(2));
@@ -163,19 +170,34 @@ void Tracker::logic()
             }
     }
 
-    rmat cam = get_camera_offset_matrix();
+    const rmat cam = get_camera_offset_matrix(c_div);
+    const rmat cam_real = get_camera_offset_matrix(1);
 
     r = r * cam;
+    r_real = r_real * cam_real;
 
     if (do_center_now)
     {
+        centerp = false;
+
         if (libs.pFilter)
             libs.pFilter->center();
-        centerp = false;
+
+        if (libs.pTracker->center())
+        {
+            r_b = cam.t();
+            r_b_real = cam_real.t();
+            r = rmat::eye();
+            r_real = rmat::eye();
+        }
+        else
+        {
+            r_b = r.t();
+            r_b_real = r_real.t();
+        }
+
         for (int i = 0; i < 3; i++)
             t_b[i] = t(i);
-
-        r_b = r.t();
     }
 
     {
@@ -184,29 +206,26 @@ void Tracker::logic()
         // inertial
         case 0:
         default:
-            r = r * r_b;
+            r = r_b * r;
             break;
         // camera
         case 1:
-            euler_t degs = rmat_to_euler(r_b * r);
-            degs(2) = rmat_to_euler(r * r_b)(2);
-            r = euler_to_rmat(degs);
+            r = r * r_b;
             break;
         }
 
-        const euler_t euler = r2d * rmat_to_euler(r);
-
-        euler_t tmp(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(r);
+        euler_t pos(t(0) - t_b[0], t(1) - t_b[1], t(2) - t_b[2]);
 
         if (s.use_camera_offset_from_centering)
-            t_compensate(r_b.t() * cam.t(), tmp, tmp, false);
+            t_compensate(r_b_real.t() * cam_real.t(), pos, pos, false);
         else
-            t_compensate(cam.t(), tmp, tmp, false);
+            t_compensate(cam_real.t(), pos, pos, false);
 
         for (int i = 0; i < 3; i++)
         {
-            value(i) = tmp(i);
-            value(i+3) = euler(i);
+            value(i) = pos(i);
+            value(i+3) = rot(i);
         }
     }
 
@@ -350,8 +369,10 @@ void Tracker::run()
     }
 }
 
-void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const {
+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);
diff --git a/opentrack-logic/tracker.h b/opentrack-logic/tracker.h
index 310bac31..09aca424 100644
--- a/opentrack-logic/tracker.h
+++ b/opentrack-logic/tracker.h
@@ -71,7 +71,7 @@ private:
     using rmat = euler::rmat;
     using euler_t = euler::euler_t;
 
-    rmat r_b;
+    rmat r_b, r_b_real;
     double t_b[3];
 
     double map(double pos, Mapping& axis);
@@ -82,11 +82,15 @@ private:
     static constexpr double pi = OPENTRACK_PI;
     static constexpr double r2d = 180. / OPENTRACK_PI;
     static constexpr double d2r = OPENTRACK_PI / 180.;
+
+    // note: float exponent base is 2
+    static constexpr double c_mult = 4;
+    static constexpr double c_div = 1./c_mult;
 public:
     Tracker(Mappings& m, SelectedLibraries& libs, TrackLogger &logger);
     ~Tracker();
 
-    rmat get_camera_offset_matrix();
+    rmat get_camera_offset_matrix(double c);
     void get_raw_and_mapped_poses(double* mapped, double* raw) const;
     void start() { QThread::start(); }
     void toggle_enabled() { enabledp = !enabledp; }
-- 
cgit v1.2.3