From e0866f30d25abd62fadb4b29fff90143c288e9ff Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Thu, 18 Jan 2018 23:08:30 +0100
Subject: logic: change relative translation triggering

Relative translation mode now has three states:

- disabled
- always enabled
- enabled when not aiming

In the third mode, looking very close to center disables reltrans. The
exact values aren't final.
---
 logic/main-settings.cpp | 13 +++++------
 logic/main-settings.hpp | 12 ++++++++--
 logic/pipeline.cpp      | 62 ++++++++++++++++++++++++++++---------------------
 logic/pipeline.hpp      | 10 ++++----
 4 files changed, 57 insertions(+), 40 deletions(-)

(limited to 'logic')

diff --git a/logic/main-settings.cpp b/logic/main-settings.cpp
index 2c5d45db..c735ac6e 100644
--- a/logic/main-settings.cpp
+++ b/logic/main-settings.cpp
@@ -12,13 +12,12 @@ main_settings::main_settings() :
     a_pitch("pitch", Pitch),
     a_roll("roll", Roll),
     all_axis_opts { &a_x, &a_y, &a_z, &a_yaw, &a_pitch, &a_roll },
-    tcomp_p(b, "compensate-translation", false),
-    tcomp_disable_tx(b, "compensate-translation-disable-x-axis", false),
-    tcomp_disable_ty(b, "compensate-translation-disable-y-axis", false),
-    tcomp_disable_tz(b, "compensate-translation-disable-z-axis", false),
-    tcomp_disable_src_yaw(b, "compensate-translation-disable-source-yaw", false),
-    tcomp_disable_src_pitch(b, "compensate-translation-disable-source-pitch", false),
-    tcomp_disable_src_roll(b, "compensate-translation-disable-source-roll", false),
+    reltrans_disable_tx(b, "compensate-translation-disable-x-axis", false),
+    reltrans_disable_ty(b, "compensate-translation-disable-y-axis", false),
+    reltrans_disable_tz(b, "compensate-translation-disable-z-axis", false),
+    reltrans_disable_src_yaw(b, "compensate-translation-disable-source-yaw", false),
+    reltrans_disable_src_pitch(b, "compensate-translation-disable-source-pitch", false),
+    reltrans_disable_src_roll(b, "compensate-translation-disable-source-roll", false),
     tray_enabled(b, "use-system-tray", false),
     tray_start(b, "start-in-tray", false),
     center_at_startup(b, "center-at-startup", true),
diff --git a/logic/main-settings.hpp b/logic/main-settings.hpp
index 00fbe9cc..df86f856 100644
--- a/logic/main-settings.hpp
+++ b/logic/main-settings.hpp
@@ -15,6 +15,13 @@
 
 #include "export.hpp"
 
+enum reltrans_state
+{
+    reltrans_disabled   = 0,
+    reltrans_enabled    = 1,
+    reltrans_non_center = 2,
+};
+
 namespace main_settings_impl {
 
 using namespace options;
@@ -40,8 +47,9 @@ struct OTR_LOGIC_EXPORT main_settings final
     axis_opts a_x, a_y, a_z;
     axis_opts a_yaw, a_pitch, a_roll;
     axis_opts* all_axis_opts[6];
-    value<bool> tcomp_p, tcomp_disable_tx, tcomp_disable_ty, tcomp_disable_tz;
-    value<bool> tcomp_disable_src_yaw, tcomp_disable_src_pitch, tcomp_disable_src_roll;
+    value<reltrans_state> reltrans_mode { b, "relative-translation-mode", reltrans_disabled };
+    value<bool> reltrans_disable_tx, reltrans_disable_ty, reltrans_disable_tz;
+    value<bool> reltrans_disable_src_yaw, reltrans_disable_src_pitch, reltrans_disable_src_roll;
     value<bool> tray_enabled, tray_start;
     value<bool> center_at_startup;
     //value<int> center_method;
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index de7d94e6..e6ca3b70 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -15,6 +15,7 @@
 #include "compat/nan.hpp"
 #include "compat/sleep.hpp"
 #include "compat/math.hpp"
+#include "compat/meta.hpp"
 
 #include "pipeline.hpp"
 
@@ -64,29 +65,38 @@ euler_t reltrans::rotate(const rmat& rmat, const euler_t& xyz,
     return output;
 }
 
-Pose reltrans::apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6, 1>& disable)
+Pose reltrans::apply_pipeline(reltrans_state state, const Pose& value, const Mat<bool, 6, 1>& disable)
 {
-    if (enable)
+    if (state != reltrans_disabled)
     {
         euler_t rel { value(TX), value(TY), value(TZ) };
 
         {
-            const bool yaw_in_zone = std::fabs(value(Yaw)) > 135;
-            const bool pitch_in_zone = value(Pitch) < -30;
-            const bool tcomp_in_zone_ = yaw_in_zone || pitch_in_zone;
+            bool tcomp_in_zone_ = progn(
+                if (state == reltrans_non_center)
+                {
+                    const bool yaw_in_zone = std::fabs(value(Yaw)) < 12;
+                    const bool pitch_in_zone = std::fabs(value(Pitch)) < 15;
+                    const bool roll_in_zone = std::fabs(value(Roll)) < 7;
+
+                    return !(yaw_in_zone && pitch_in_zone && roll_in_zone);
+                }
+                else
+                    return true;
+            );
 
-            if (!tcomp_state && tcomp_in_zone != tcomp_in_zone_)
+            if (!cur && in_zone != tcomp_in_zone_)
             {
-                //qDebug() << "tcomp-interp: START";
-                tcomp_state = true;
-                tcomp_interp_timer.start();
+                //qDebug() << "reltrans-interp: START" << tcomp_in_zone_;
+                cur = true;
+                interp_timer.start();
             }
 
-            tcomp_in_zone = tcomp_in_zone_;
+            in_zone = tcomp_in_zone_;
         }
 
         // only when looking behind or downward
-        if (tcomp_in_zone)
+        if (in_zone)
         {
             const double tcomp_c[] = {
                 double(!disable(Yaw)),
@@ -105,41 +115,41 @@ Pose reltrans::apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6,
                          disable(TZ));
         }
 
-        if (tcomp_state)
+        if (cur)
         {
-            const double dt = tcomp_interp_timer.elapsed_seconds();
-            tcomp_interp_timer.start();
+            const double dt = interp_timer.elapsed_seconds();
+            interp_timer.start();
 
             constexpr double RC = .1;
             const double alpha = dt/(dt+RC);
 
             constexpr double eps = .05;
 
-            tcomp_interp_pos = tcomp_interp_pos * (1-alpha) + rel * alpha;
+            interp_pos = interp_pos * (1-alpha) + rel * alpha;
 
-            const euler_t tmp = rel - tcomp_interp_pos;
-            rel = tcomp_interp_pos;
+            const euler_t tmp = rel - interp_pos;
+            rel = interp_pos;
             const double delta = std::fabs(tmp(0)) + std::fabs(tmp(0)) + std::fabs(tmp(0));
 
-            //qDebug() << "tcomp-interp: delta" << delta;
+            //qDebug() << "reltrans-interp: delta" << delta;
 
             if (delta < eps)
             {
-                //qDebug() << "tcomp-interp: STOP";
-                tcomp_state = false;
+                //qDebug() << "reltrans-interp: STOP";
+                cur = false;
             }
         }
         else
         {
-            tcomp_interp_pos = rel;
+            interp_pos = rel;
         }
 
         return { rel(0), rel(1), rel(2), value(Yaw), value(Pitch), value(Roll) };
     }
     else
     {
-        tcomp_state = false;
-        tcomp_in_zone = false;
+        cur = false;
+        in_zone = false;
 
         return value;
     }
@@ -403,9 +413,9 @@ void pipeline::logic()
 
             nan_check(value);
 
-            value = rel.apply_pipeline(s.tcomp_p, value, {
-                !!s.tcomp_disable_src_yaw, !!s.tcomp_disable_src_pitch, !!s.tcomp_disable_src_roll,
-                !!s.tcomp_disable_tx, !!s.tcomp_disable_ty, !!s.tcomp_disable_tz
+            value = rel.apply_pipeline(s.reltrans_mode, value, {
+                !!s.reltrans_disable_src_yaw, !!s.reltrans_disable_src_pitch, !!s.reltrans_disable_src_roll,
+                !!s.reltrans_disable_tx, !!s.reltrans_disable_ty, !!s.reltrans_disable_tz
             });
 
             for (int i = 0; i < 3; i++)
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index dc04d44b..0061e439 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -37,10 +37,10 @@ using euler_t = euler::euler_t;
 
 class reltrans
 {
-    euler_t tcomp_interp_pos, tcomp_last_value;
-    Timer tcomp_interp_timer;
-    bool tcomp_state = false;
-    bool tcomp_in_zone = false;
+    euler_t interp_pos, last_value;
+    Timer interp_timer;
+    bool cur = false;
+    bool in_zone = false;
 
 public:
     reltrans();
@@ -50,7 +50,7 @@ public:
                    bool disable_tx, bool disable_ty, bool disable_tz) const;
 
     warn_result_unused
-    Pose apply_pipeline(bool enable, const Pose& value, const Mat<bool, 6, 1>& disable);
+    Pose apply_pipeline(reltrans_state cur, const Pose& value, const Mat<bool, 6, 1>& disable);
 };
 
 using namespace time_units;
-- 
cgit v1.2.3