From 64250f9bb6f5d95f14a8d7f3329d018679710540 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 30 Dec 2016 04:40:06 +0100
Subject: tracker/steamvr: broken, but we already know the right rotation order

---
 tracker-steamvr/dialog.cpp  |  5 +++++
 tracker-steamvr/dialog.ui   | 14 +++++---------
 tracker-steamvr/steamvr.cpp | 36 +++++++-----------------------------
 tracker-steamvr/steamvr.hpp | 12 ++++++++----
 4 files changed, 25 insertions(+), 42 deletions(-)

(limited to 'tracker-steamvr')

diff --git a/tracker-steamvr/dialog.cpp b/tracker-steamvr/dialog.cpp
index bbb8866d..1bfa5bcb 100644
--- a/tracker-steamvr/dialog.cpp
+++ b/tracker-steamvr/dialog.cpp
@@ -7,6 +7,11 @@ 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 c3fa36c0..7a5b2f48 100644
--- a/tracker-steamvr/dialog.ui
+++ b/tracker-steamvr/dialog.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>184</width>
-    <height>58</height>
+    <width>243</width>
+    <height>67</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -33,6 +33,9 @@
    <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">
@@ -46,13 +49,6 @@
      </property>
     </widget>
    </item>
-   <item row="0" column="0">
-    <widget class="QLabel" name="label">
-     <property name="text">
-      <string>No options so far</string>
-     </property>
-    </widget>
-   </item>
   </layout>
  </widget>
  <resources/>
diff --git a/tracker-steamvr/steamvr.cpp b/tracker-steamvr/steamvr.cpp
index 9ceb5db2..360ba3d2 100644
--- a/tracker-steamvr/steamvr.cpp
+++ b/tracker-steamvr/steamvr.cpp
@@ -79,35 +79,13 @@ void steamvr::start_tracker(QFrame*)
     }
 }
 
-quat steamvr::get_quaternion(const vr::HmdMatrix34_t& r)
-{
-    using std::max;
-    using std::sqrt;
-    using std::acos;
-
-    const auto& m = r.m;
-
-    float qw = sqrt(max(0.f, 1 + m[0][0] + m[1][1] + m[2][2])) / 2;
-    float qx = sqrt(max(0.f, 1 + m[0][0] - m[1][1] - m[2][2])) / 2;
-    float qy = sqrt(max(0.f, 1 - m[0][0] + m[1][1] - m[2][2])) / 2;
-    float qz = sqrt(max(0.f, 1 - m[0][0] - m[1][1] + m[2][2])) / 2;
-    qx = (m[1][2] - m[2][1]) < 0 ? -qx : qx;
-    qy = (m[2][0] - m[0][2]) < 0 ? -qy : qy;
-    qz = (m[0][1] - m[1][0]) < 0 ? -qz : qz;
-
-    float s = sqrt(1 - qw * qw);
-    s = s < .001 ? 1 : s;
-
-    return quat(2 * acos(qw), qx/s, qy/s, qz/s);
-}
-
 void steamvr::data(double* data)
 {
     if (vr)
     {
         vr::TrackedDevicePose_t devices[vr::k_unMaxTrackedDeviceCount] = {};
 
-        vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin::TrackingUniverseStanding, 0,
+        vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin::TrackingUniverseSeated, 0,
                                             devices, vr::k_unMaxTrackedDeviceCount);
 
         bool done = false;
@@ -129,15 +107,15 @@ void steamvr::data(double* data)
             for (unsigned i = 0; i < 3; i++)
                 data[i] = double(result.m[i][3]) * c[i] * 100;
 
-            const quat q = get_quaternion(result);
-            static constexpr double r2d = 180/M_PI;
-
             using std::atan2;
             using std::asin;
 
-            data[Roll]  = r2d * atan2(2*(q(0)*q(1) + q(2)*q(3)), 1 - 2*(q(1)*q(1) + q(2)*q(2)));
-            data[Pitch] = r2d * asin(2*(q(0)*q(2) - q(3)*q(1)));
-            data[Yaw]   = r2d * atan2(2*(q(0)*q(3) + q(1)*q(2)), 1 - 2*(q(2)*q(2) + q(3)*q(3)));
+            //const euler_t rot = get_ypr(s.order);
+
+            static constexpr double r2d = 180/M_PI;
+
+            //for (unsigned i = 3; i < 6; i++)
+            //    data[i] = r2d * rot[i];
 
             done = true;
             break;
diff --git a/tracker-steamvr/steamvr.hpp b/tracker-steamvr/steamvr.hpp
index 1adb40b1..4a2bd117 100644
--- a/tracker-steamvr/steamvr.hpp
+++ b/tracker-steamvr/steamvr.hpp
@@ -18,11 +18,13 @@
 
 using namespace options;
 
-using quat = Mat<float, 4, 1>;
-
 struct settings : opts
 {
-    settings() : opts("valve-steamvr") {}
+    value<int> order;
+    settings() :
+        opts("valve-steamvr"),
+        order(b, "rotation-order", 0)
+    {}
 };
 
 class steamvr : public ITracker
@@ -41,7 +43,9 @@ private:
 
     settings s;
 
-    static quat get_quaternion(const vr::HmdMatrix34_t& r);
+    using rmat = euler::rmat;
+    using euler_t = euler::euler_t;
+
     static void vr_deleter();
     static vr_t vr_init(error_t& error);
     static QString strerror(error_t error);
-- 
cgit v1.2.3