tracker/rift-{025,042,080}: fix gimbal lockopentrack-2.3-rc99p13

It was tested only on 080 but the API is almost the same on 042
and 025.

Issue: #464
Reported-by: @Jerrow

3 files changed, 46 insertions, 19 deletions

diff --git a/tracker-rift-025/ftnoir_tracker_rift_025.cpp b/tracker-rift-025/ftnoir_tracker_rift_025.cpp
index dcb97fc5..e0ea2ad7 100644
--- a/tracker-rift-025/ftnoir_tracker_rift_025.cpp
+++ b/tracker-rift-025/ftnoir_tracker_rift_025.cpp
@@ -66,11 +66,19 @@ void Rift_Tracker::data(double *data)
 {
     if (pSFusion != NULL && pSensor != NULL)
     {
-        Quatf hmdOrient = pSFusion->GetOrientation();
+        Quatf rot = pSFusion->GetOrientation();
 
-        float yaw = 0, pitch = 0, roll = 0;
+        static constexpr float c_mult = 8;
+        static constexpr float c_div = 1/c_mult;
 
-        hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch , &roll);
+        Vector3f axis;
+        float angle;
+
+        rot.GetAxisAngle(&axis, &angle);
+        angle *= c_div;
+
+        float yaw, pitch, roll;
+        Quatf(axis, angle).GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
 
         double yaw_ = double(yaw);
 
diff --git a/tracker-rift-042/ftnoir_tracker_rift_042.cpp b/tracker-rift-042/ftnoir_tracker_rift_042.cpp
index c868eee8..1b819328 100644
--- a/tracker-rift-042/ftnoir_tracker_rift_042.cpp
+++ b/tracker-rift-042/ftnoir_tracker_rift_042.cpp
@@ -53,10 +53,19 @@ void Rift_Tracker::data(double *data)
         ovrTrackingState ss = ovrHmd_GetTrackingState(hmd, 0);
         if (ss.StatusFlags & ovrStatus_OrientationTracked)
         {
-            auto pose = ss.HeadPose.ThePose;
-            Quatf quat = pose.Orientation;
+            static constexpr float c_mult = 8;
+            static constexpr float c_div = 1/c_mult;
+
+            Vector3f axis;
+            float angle;
+
+            const Posef pose(ss.HeadPose.ThePose);
+            pose.Rotation.GetAxisAngle(&axis, &angle);
+            angle *= c_div;
+
             float yaw, pitch, roll;
-            quat.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
+            Quatf(axis, angle).GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
+
             double yaw_ = double(yaw);
             if (s.useYawSpring)
             {
@@ -71,9 +80,9 @@ void Rift_Tracker::data(double *data)
             data[Yaw] = yaw_ * -d2r;
             data[Pitch] = double(pitch) * d2r;
             data[Roll] = double(roll) * d2r;
-            data[TX] = double(pose.Position.x) * -1e2;
-            data[TY] = double(pose.Position.y) *  1e2;
-            data[TZ] = double(pose.Position.z) *  1e2;
+            data[TX] = double(pose.Translation.x) * -1e2;
+            data[TY] = double(pose.Translation.y) *  1e2;
+            data[TZ] = double(pose.Translation.z) *  1e2;
         }
     }
 }
diff --git a/tracker-rift-080/ftnoir_tracker_rift_080.cpp b/tracker-rift-080/ftnoir_tracker_rift_080.cpp
index 8522a540..a7d09035 100644
--- a/tracker-rift-080/ftnoir_tracker_rift_080.cpp
+++ b/tracker-rift-080/ftnoir_tracker_rift_080.cpp
@@ -1,16 +1,13 @@
 /* Copyright: "i couldn't care less what anyone does with the 5 lines of code i wrote" - mm0zct */
 #include "ftnoir_tracker_rift_080.h"
 #include "api/plugin-api.hpp"
+#include "compat/util.hpp"
 
 #include <QString>
 
-#include <OVR_CAPI.h>
 #include <Extras/OVR_Math.h>
 #include <OVR_CAPI_0_8_0.h>
 
-#include <cstdio>
-#include <cmath>
-
 using namespace OVR;
 
 Rift_Tracker::Rift_Tracker() : old_yaw(0), hmd(nullptr)
@@ -65,10 +62,23 @@ void Rift_Tracker::data(double *data)
         ovrTrackingState ss = ovr_GetTrackingState(hmd, 0, false);
         if (ss.StatusFlags & ovrStatus_OrientationTracked)
         {
-            auto pose = ss.HeadPose.ThePose;
-            Quatf quat = pose.Orientation;
+            static constexpr float c_mult = 8;
+            static constexpr float c_div = 1/c_mult;
+
+            Vector3f axis;
+            float angle;
+
+            const Posef pose(ss.HeadPose.ThePose);
+            pose.Rotation.GetAxisAngle(&axis, &angle);
+            angle *= c_div;
+
             float yaw, pitch, roll;
-            quat.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll);
+            Quatf(axis, angle).GetYawPitchRoll(&yaw, &pitch, &roll);
+
+            yaw *= c_mult;
+            pitch *= c_mult;
+            roll *= c_mult;
+
             double yaw_ = double(yaw);
             if (s.useYawSpring)
             {
@@ -83,9 +93,9 @@ void Rift_Tracker::data(double *data)
             data[Yaw] = yaw_                   * -d2r;
             data[Pitch] = double(pitch)        *  d2r;
             data[Roll] = double(roll)          *  d2r;
-            data[TX] = double(pose.Position.x) * -1e2;
-            data[TY] = double(pose.Position.y) *  1e2;
-            data[TZ] = double(pose.Position.z) *  1e2;
+            data[TX] = double(pose.Translation.x) * -1e2;
+            data[TY] = double(pose.Translation.y) *  1e2;
+            data[TZ] = double(pose.Translation.z) *  1e2;
         }
     }
 }