From b5e67864360d98145c67130bfdb3c4c56963c787 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik 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-042/ftnoir_tracker_rift_042.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) (limited to 'tracker-rift-042') 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(&yaw, &pitch, &roll); + Quatf(axis, angle).GetEulerAngles(&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; } } } -- cgit v1.2.3