Adding a choice of centering method

9 files changed, 220 insertions, 31 deletions

diff --git a/gui/lang/nl_NL.ts b/gui/lang/nl_NL.ts
index 60b84189..59d46b82 100644
--- a/gui/lang/nl_NL.ts
+++ b/gui/lang/nl_NL.ts
@@ -360,6 +360,22 @@ Press "clear calibration" to remove any calibration data pertaining to
         <source>None</source>
         <translation type="unfinished">Geen</translation>
     </message>
+    <message>
+        <source>Centering method</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Point</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Wireless VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Roll compensated VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>process_detector</name>
diff --git a/gui/lang/ru_RU.ts b/gui/lang/ru_RU.ts
index d40b4775..8f3c360a 100644
--- a/gui/lang/ru_RU.ts
+++ b/gui/lang/ru_RU.ts
@@ -363,6 +363,22 @@ Press &quot;clear calibration&quot; to remove any calibration data pertaining to
         <source>None</source>
         <translation>Не назначена</translation>
     </message>
+    <message>
+        <source>Centering method</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Point</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Wireless VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Roll compensated VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>process_detector</name>
diff --git a/gui/lang/stub.ts b/gui/lang/stub.ts
index b383a0d8..9dd07851 100644
--- a/gui/lang/stub.ts
+++ b/gui/lang/stub.ts
@@ -360,6 +360,22 @@ Press &quot;clear calibration&quot; to remove any calibration data pertaining to
         <source>None</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Centering method</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Point</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Wireless VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Roll compensated VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>process_detector</name>
diff --git a/gui/lang/zh_CN.ts b/gui/lang/zh_CN.ts
index fa2d512c..a88251ce 100644
--- a/gui/lang/zh_CN.ts
+++ b/gui/lang/zh_CN.ts
@@ -361,6 +361,22 @@ Press &quot;clear calibration&quot; to remove any calibration data pertaining to
         <source>None</source>
         <translation>空</translation>
     </message>
+    <message>
+        <source>Centering method</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Point</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Wireless VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Roll compensated VR 360</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>process_detector</name>
diff --git a/gui/settings-dialog.ui b/gui/settings-dialog.ui
index 6333692a..93374b06 100644
--- a/gui/settings-dialog.ui
+++ b/gui/settings-dialog.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>548</width>
-    <height>599</height>
+    <height>600</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -812,29 +812,39 @@
           <enum>QFrame::Sunken</enum>
          </property>
          <layout class="QGridLayout" name="gridLayout_2">
-          <item row="0" column="0">
-           <widget class="QCheckBox" name="center_at_startup">
+          <item row="1" column="0">
+           <widget class="QCheckBox" name="disable_translation">
             <property name="sizePolicy">
-             <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+             <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
               <horstretch>0</horstretch>
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
             <property name="text">
-             <string>Center at startup</string>
+             <string>Never translate the application interface</string>
             </property>
            </widget>
           </item>
-          <item row="1" column="0">
-           <widget class="QCheckBox" name="disable_translation">
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_18">
+            <property name="text">
+             <string>Centering method</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="0">
+           <widget class="QCheckBox" name="center_at_startup">
             <property name="sizePolicy">
-             <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+             <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
               <horstretch>0</horstretch>
               <verstretch>0</verstretch>
              </sizepolicy>
             </property>
             <property name="text">
-             <string>Never translate the application interface</string>
+             <string>Center at startup</string>
             </property>
            </widget>
           </item>
@@ -848,6 +858,39 @@
             </property>
            </widget>
           </item>
+          <item row="0" column="2">
+           <widget class="QComboBox" name="cbox_centering">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+              <horstretch>4</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="currentIndex">
+             <number>3</number>
+            </property>
+            <item>
+             <property name="text">
+              <string>Disabled</string>
+             </property>
+            </item>
+            <item>
+             <property name="text">
+              <string>Point</string>
+             </property>
+            </item>
+            <item>
+             <property name="text">
+              <string>Wireless VR 360</string>
+             </property>
+            </item>
+            <item>
+             <property name="text">
+              <string>Roll compensated VR 360</string>
+             </property>
+            </item>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>
@@ -1569,9 +1612,15 @@
              </item>
              <item row="0" column="2">
               <widget class="QLabel" name="label_4">
+               <property name="layoutDirection">
+                <enum>Qt::LeftToRight</enum>
+               </property>
                <property name="text">
                 <string>X</string>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
               </widget>
              </item>
              <item row="0" column="3">
@@ -1601,6 +1650,9 @@
                <property name="text">
                 <string>Pitch</string>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
               </widget>
              </item>
              <item row="1" column="2">
@@ -1608,6 +1660,9 @@
                <property name="text">
                 <string>Y</string>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
               </widget>
              </item>
              <item row="1" column="1">
@@ -1637,6 +1692,9 @@
                <property name="text">
                 <string>Z</string>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
               </widget>
              </item>
              <item row="2" column="0">
@@ -1644,6 +1702,9 @@
                <property name="text">
                 <string>Roll</string>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
               </widget>
              </item>
              <item row="1" column="3">
@@ -1673,6 +1734,9 @@
                <property name="text">
                 <string>Yaw</string>
                </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
               </widget>
              </item>
              <item row="0" column="1">
@@ -1808,6 +1872,9 @@
                  <verstretch>0</verstretch>
                 </sizepolicy>
                </property>
+               <property name="currentIndex">
+                <number>0</number>
+               </property>
                <item>
                 <property name="text">
                  <string>Disabled</string>
diff --git a/gui/settings.cpp b/gui/settings.cpp
index 3851f0c2..cef93d19 100644
--- a/gui/settings.cpp
+++ b/gui/settings.cpp
@@ -59,6 +59,16 @@ options_dialog::options_dialog(std::function<void(bool)> pause_keybindings) :
 
     tie_setting(main.center_at_startup, ui.center_at_startup);
 
+    const centering_state centering_modes[] = {
+        center_disabled,
+        center_point,
+        center_vr360,
+        center_roll_compensated,
+    };
+    for (unsigned k = 0; k < 4; k++)
+        ui.cbox_centering->setItemData(k, int(centering_modes[k]));
+    tie_setting(main.centering_mode, ui.cbox_centering);
+
     const reltrans_state reltrans_modes[] = {
         reltrans_disabled,
         reltrans_enabled,
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 658c62bb..3a67eb90 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 7775054e..7b3703ee 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);