From 3a1458031b50fce43419a8b80924cc9439248c73 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Thu, 9 Jun 2016 11:09:23 +0200 Subject: tracker/rift-{025,042,080}: fix float/double promotions The change was only read and compile-tested. --- tracker-rift-025/ftnoir_tracker_rift_025.cpp | 38 ++++++++++++++++------------ 1 file changed, 22 insertions(+), 16 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 4b938763..aab8d530 100644 --- a/tracker-rift-025/ftnoir_tracker_rift_025.cpp +++ b/tracker-rift-025/ftnoir_tracker_rift_025.cpp @@ -3,6 +3,11 @@ #include "opentrack/plugin-api.hpp" #include "OVR.h" #include +#include + +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif using namespace OVR; @@ -65,27 +70,28 @@ void Rift_Tracker::data(double *data) if (pSFusion != NULL && pSensor != NULL) { Quatf hmdOrient = pSFusion->GetOrientation(); - double newHeadPose[6]; - float yaw = 0; - float pitch = 0; - float roll = 0; + float yaw = 0, pitch = 0, roll = 0; + hmdOrient.GetEulerAngles(&yaw, &pitch , &roll); - newHeadPose[Pitch] = pitch; - newHeadPose[Roll] = roll; - newHeadPose[Yaw] = yaw; + + double yaw_ = yaw; + if (s.useYawSpring) { - newHeadPose[Yaw] = old_yaw*s.persistence + (yaw-old_yaw); - if(newHeadPose[Yaw] > s.deadzone) - newHeadPose[Yaw] -= s.constant_drift; - if(newHeadPose[Yaw] < -s.deadzone) - newHeadPose[Yaw] += s.constant_drift; - old_yaw=yaw; + yaw_ = old_yaw*s.persistence + (yaw_-old_yaw); + if (yaw_ > s.deadzone) + yaw_ -= s.constant_drift; + if (yaw_ < -s.deadzone) + yaw_ += s.constant_drift; + old_yaw = yaw_; } - data[Yaw] = newHeadPose[Yaw] * 57.295781f; - data[Pitch] = newHeadPose[Pitch] * 57.295781f; - data[Roll] = newHeadPose[Roll] * 57.295781f; + + static constexpr double r2d = 180 / M_PI; + + data[Yaw] = yaw_ * r2d; + data[Pitch] = double(pitch) * r2d; + data[Roll] = double(roll) * r2d; } } -- cgit v1.2.3