From b5e67864360d98145c67130bfdb3c4c56963c787 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 1 Oct 2016 16:49:34 +0200
Subject: tracker/rift-{025,042,080}: fix gimbal lock

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

Issue: #464
Reported-by: @Jerrow
---
 tracker-rift-025/ftnoir_tracker_rift_025.cpp | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

(limited to 'tracker-rift-025')

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);
 
-- 
cgit v1.2.3