From 68b8813965cdead743a70e8ee47458d8f1e21dd4 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Tue, 28 Mar 2017 23:51:27 +0200
Subject: tracker/steamvr: fix vive rotation order

Submitted-by: @subnet-
Issue: #353
cf.
https://github.com/opentrack/opentrack/issues/352#issuecomment-289306146

The submitter's working on the roll code. sin^1 codomain is +-90
deg. so we better use the equivalent atan2(3) formula.
---
 tracker-steamvr/CMakeLists.txt |  4 ++--
 tracker-steamvr/dialog.cpp     |  5 -----
 tracker-steamvr/dialog.ui      | 14 +++++++-----
 tracker-steamvr/steamvr.cpp    | 49 +++++++++++++++++++++++-------------------
 tracker-steamvr/steamvr.hpp    |  7 +++---
 5 files changed, 41 insertions(+), 38 deletions(-)

(limited to 'tracker-steamvr')

diff --git a/tracker-steamvr/CMakeLists.txt b/tracker-steamvr/CMakeLists.txt
index 36be2f13..512a49ff 100644
--- a/tracker-steamvr/CMakeLists.txt
+++ b/tracker-steamvr/CMakeLists.txt
@@ -27,8 +27,8 @@ endif()
 if(steamvr-dll)
     SET(SDK_VALVE_STEAMVR "" CACHE PATH "Valve's SteamVR")
     if(SDK_VALVE_STEAMVR)
-        otr_module(tracker-steamvr NO-INSTALL)
-        #install(FILES "${SDK_VALVE_STEAMVR}/bin/${steamvr-dir}/${steamvr-dll}" DESTINATION "${opentrack-hier-pfx}")
+        otr_module(tracker-steamvr)
+        install(FILES "${SDK_VALVE_STEAMVR}/bin/${steamvr-dir}/${steamvr-dll}" DESTINATION "${opentrack-hier-pfx}")
 
         target_include_directories(opentrack-tracker-steamvr SYSTEM PUBLIC "${SDK_VALVE_STEAMVR}/headers")
         target_link_libraries(opentrack-tracker-steamvr "${SDK_VALVE_STEAMVR}/lib/${steamvr-dir}/${steamvr-lib}")
diff --git a/tracker-steamvr/dialog.cpp b/tracker-steamvr/dialog.cpp
index 1bfa5bcb..bbb8866d 100644
--- a/tracker-steamvr/dialog.cpp
+++ b/tracker-steamvr/dialog.cpp
@@ -7,11 +7,6 @@ steamvr_dialog::steamvr_dialog()
 
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
-
-    for (unsigned i = 0; i < 6; i++)
-        ui.rotation_order->addItem(QStringLiteral("order #%1").arg(i));
-
-    tie_setting(s.order, ui.rotation_order);
 }
 
 void steamvr_dialog::doOK()
diff --git a/tracker-steamvr/dialog.ui b/tracker-steamvr/dialog.ui
index 7a5b2f48..9d3b1e85 100644
--- a/tracker-steamvr/dialog.ui
+++ b/tracker-steamvr/dialog.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>243</width>
-    <height>67</height>
+    <width>281</width>
+    <height>66</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -33,9 +33,6 @@
    <bool>false</bool>
   </property>
   <layout class="QGridLayout" name="gridLayout_2">
-   <item row="0" column="0">
-    <widget class="QComboBox" name="rotation_order"/>
-   </item>
    <item row="1" column="0">
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="sizePolicy">
@@ -49,6 +46,13 @@
      </property>
     </widget>
    </item>
+   <item row="0" column="0">
+    <widget class="QLabel" name="label">
+     <property name="text">
+      <string>No settings necessary</string>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>
diff --git a/tracker-steamvr/steamvr.cpp b/tracker-steamvr/steamvr.cpp
index e6e9952b..93f81f00 100644
--- a/tracker-steamvr/steamvr.cpp
+++ b/tracker-steamvr/steamvr.cpp
@@ -4,7 +4,6 @@
 
 #include <cstdlib>
 #include <cmath>
-#include <algorithm>
 
 #include <QMessageBox>
 #include <QDebug>
@@ -54,7 +53,7 @@ void steamvr::start_tracker(QFrame*)
     if (!vr)
     {
         QMessageBox::warning(nullptr,
-                             QCoreApplication::translate("steamvr", "Valve SteamVR init error"), strerror(e),
+                             tr("Valve SteamVR init error"), strerror(e),
                              QMessageBox::Close, QMessageBox::NoButton);
         return;
     }
@@ -71,8 +70,8 @@ void steamvr::start_tracker(QFrame*)
     if (!ok)
     {
         QMessageBox::warning(nullptr,
-                             QCoreApplication::translate("steamvr", "Valve SteamVR init warning"),
-                             QCoreApplication::translate("steamvr", "No HMD connected"),
+                             tr("Valve SteamVR init warning"),
+                             tr("No HMD connected"),
                              QMessageBox::Close, QMessageBox::NoButton);
         return;
     }
@@ -87,41 +86,47 @@ void steamvr::data(double* data)
         vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin::TrackingUniverseSeated, 0,
                                             devices, vr::k_unMaxTrackedDeviceCount);
 
-        bool done = false;
-
         for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
         {
-            using namespace euler;
-
             if (!devices[k].bPoseIsValid)
                 continue;
 
             if (vr->GetTrackedDeviceClass(k) != vr::ETrackedDeviceClass::TrackedDeviceClass_HMD)
                 continue;
 
-            const auto& result = devices[k].mDeviceToAbsoluteTracking;
+            const vr::HmdMatrix34_t& result = devices[k].mDeviceToAbsoluteTracking;
 
-            static constexpr double c[3] { -1, 1, -1 };
+            int c = 10;
 
-            for (unsigned i = 0; i < 3; i++)
-                data[i] = double(result.m[i][3]) * c[i] * 100;
+            data[TX] = -result.m[0][3] * c;
+            data[TY] = result.m[1][3] * c;
+            data[TZ] = result.m[2][3] * c;
 
             using std::atan2;
             using std::asin;
 
-            //const euler_t rot = get_ypr(s.order);
-
-            static constexpr double r2d = 180/M_PI;
+            // transformation matrix to euler angles
+            if (result.m[0][0] == 1.0f || result.m[0][0] == -1.0f)
+            {
+                data[Yaw] = -atan2(result.m[0][2], result.m[2][3]);
+                data[Pitch] = 0;
+                data[Roll] = 0;
+            }
+            else
+            {
+                data[Yaw] = -atan2(-result.m[2][0], result.m[0][0]);
+                data[Pitch] = atan2(-result.m[1][2], result.m[1][1]);
+                data[Roll] = asin(result.m[1][0]);
+            }
+
+            static constexpr double r2d = 180 / M_PI;
+
+            data[Yaw] *= r2d;
+            data[Pitch] *= r2d;
+            data[Roll] *= r2d;
 
-            //for (unsigned i = 3; i < 6; i++)
-            //    data[i] = r2d * rot[i];
-
-            done = true;
             break;
         }
-
-        if (!done)
-            qDebug() << "steamvr: no device with pose found";
     }
 }
 
diff --git a/tracker-steamvr/steamvr.hpp b/tracker-steamvr/steamvr.hpp
index 4a2bd117..5546d619 100644
--- a/tracker-steamvr/steamvr.hpp
+++ b/tracker-steamvr/steamvr.hpp
@@ -20,15 +20,14 @@ using namespace options;
 
 struct settings : opts
 {
-    value<int> order;
     settings() :
-        opts("valve-steamvr"),
-        order(b, "rotation-order", 0)
+        opts("valve-steamvr")
     {}
 };
 
-class steamvr : public ITracker
+class steamvr : public QObject, public ITracker
 {
+    Q_OBJECT
 public:
     steamvr();
     ~steamvr() override;
-- 
cgit v1.2.3