Adding a choice of centering method

3 files changed, 70 insertions, 22 deletions

diff --git a/logic/main-settings.hpp b/logic/main-settings.hpp
index 8fef7ea7..0795bca4 100644
--- a/logic/main-settings.hpp
+++ b/logic/main-settings.hpp
@@ -22,6 +22,14 @@ enum reltrans_state
     reltrans_non_center = 2,
 };
 
+enum centering_state
+{
+    center_disabled         = 0,
+    center_point            = 1,
+    center_vr360            = 2,
+    center_roll_compensated = 3,
+};
+
 namespace main_settings_impl {
 
 using namespace options;
@@ -65,7 +73,7 @@ struct OTR_LOGIC_EXPORT main_settings final
     value<bool> tray_start { b, "start-in-tray", false };
 
     value<bool> center_at_startup { b, "center-at-startup", true };
-    //value<int> center_method;
+    value<centering_state> centering_mode { b, "centering-mode", center_roll_compensated };;
     value<int> neck_z { b, "neck-depth", 0 };
     value<bool> neck_enable { b, "neck-enable", false };
 
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 39c75e99..f8827b7e 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -272,7 +272,7 @@ bool pipeline::maybe_enable_center_on_tracking_started()
     return false;
 }
 
-void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
+void pipeline::maybe_set_center_pose(const centering_state mode, const Pose& value, bool own_center_logic)
 {
     if (b.get(f_center | f_held_center))
     {
@@ -283,28 +283,66 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
 
         if (own_center_logic)
         {
-            center.inv_R = rmat::eye();
-            center.T = {};
+            center.P  = {};
+            center.QC = QQuaternion(1,0,0,0);
+            center.QR = QQuaternion(1,0,0,0);
         }
         else
         {
-            center.inv_R = euler_to_rmat(Pose_(&value[Yaw]) * (M_PI / 180)).t();
-            center.T = Pose_(&value[TX]);
+            if (mode)
+            {
+                center.P  = value;
+                center.QC = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], -value[Roll]).conjugated();
+                center.QR = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], 0).conjugated();
+            }
+            else
+            {
+                // To reset the centering coordinates
+                // just select "Centering method [Disabled]" and then press [Center] button
+                center.P  = {};
+                center.QC = QQuaternion(1,0,0,0);
+                center.QR = QQuaternion(1,0,0,0);
+            }
         }
     }
 }
 
-Pose pipeline::apply_center(Pose value) const
+Pose pipeline::apply_center(const centering_state mode, Pose value) const
 {
-    {
-        for (unsigned k = 0; k < 3; k++)
-            value(k) -= center.T(k);
-
-        Pose_ rot = rmat_to_euler(
-            euler_to_rmat(Pose_(&value[Yaw]) * (M_PI / 180)) * center.inv_R
-        );
-        for (unsigned k = 0; k < 3; k++)
-            value(k+3) = rot(k) * 180 / M_PI;
+    if (mode != center_disabled)
+	{
+        for (unsigned k = TX; k <= TZ; k++)
+            value(k) -= center.P(k);
+
+        QQuaternion q;
+        QVector3D v;
+
+        switch (mode)
+        {
+        case center_point:
+            for (unsigned k = Yaw; k <= Roll; k++)
+            {
+                value(k) -= center.P(k);
+                if (fabs(value[k]) > 180) value[k] -= copysign(360, value[k]);
+            }
+            break;
+        case center_vr360:
+            q = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], -value[Roll]);
+            q = center.QC * q;
+            v = q.toEulerAngles();
+            value[Pitch] =  v.x();
+            value[Yaw]   =  v.y();
+            value[Roll]  = -v.z();
+            break;
+        case center_roll_compensated:
+            q = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], center.P[Roll] - value[Roll]);
+            q = center.QR * q;
+            v = q.toEulerAngles();
+            value[Pitch] =  v.x();
+            value[Yaw]   =  v.y();
+            value[Roll]  = -v.z();
+            break;
+        }
     }
 
     for (int i = 0; i < 6; i++)
@@ -407,8 +445,8 @@ void pipeline::logic()
 
     {
         maybe_enable_center_on_tracking_started();
-        maybe_set_center_pose(value, own_center_logic);
-        value = apply_center(value);
+        maybe_set_center_pose(s.centering_mode, value, own_center_logic);
+        value = apply_center(s.centering_mode, value);
 
         // "corrected" - after various transformations to account for camera position
         logger.write_pose(value);
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index fc1f2060..80d4d8a7 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -20,6 +20,7 @@
 
 #include <atomic>
 #include <cmath>
+#include <QQuaternion>
 
 #include "export.hpp"
 
@@ -99,8 +100,9 @@ class OTR_LOGIC_EXPORT pipeline : private QThread
     reltrans rel;
 
     struct {
-        rmat inv_R = rmat::eye();
-        Pose_ T;
+		Pose P;
+		QQuaternion QC = QQuaternion(1,0,0,0);
+		QQuaternion QR = QQuaternion(1,0,0,0);
     } center;
 
     time_units::ms backlog_time {};
@@ -111,8 +113,8 @@ class OTR_LOGIC_EXPORT pipeline : private QThread
     void logic();
     void run() override;
     bool maybe_enable_center_on_tracking_started();
-    void maybe_set_center_pose(const Pose& value, bool own_center_logic);
-    Pose apply_center(Pose value) const;
+    void maybe_set_center_pose(const centering_state mode, const Pose& value, bool own_center_logic);
+    Pose apply_center(const centering_state mode, Pose value) const;
     std::tuple<Pose, Pose, vec6_bool> get_selected_axis_values(const Pose& newpose) const;
     Pose maybe_apply_filter(const Pose& value) const;
     Pose apply_reltrans(Pose value, vec6_bool disabled, bool centerp);