diff options
Diffstat (limited to 'tracker-rift-080/ftnoir_tracker_rift_080.cpp')
| -rw-r--r-- | tracker-rift-080/ftnoir_tracker_rift_080.cpp | 34 |
1 files changed, 20 insertions, 14 deletions
diff --git a/tracker-rift-080/ftnoir_tracker_rift_080.cpp b/tracker-rift-080/ftnoir_tracker_rift_080.cpp index d5d6cbd6..c1a07407 100644 --- a/tracker-rift-080/ftnoir_tracker_rift_080.cpp +++ b/tracker-rift-080/ftnoir_tracker_rift_080.cpp @@ -6,6 +6,10 @@ #include "OVR_CAPI_0_8_0.h" #include <cstdio> +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif + using namespace OVR; Rift_Tracker::Rift_Tracker() : old_yaw(0), hmd(nullptr) @@ -43,27 +47,29 @@ void Rift_Tracker::data(double *data) if (hmd) { ovrTrackingState ss = ovr_GetTrackingState(hmd, 0, false); - if(ss.StatusFlags & ovrStatus_OrientationTracked) { + if (ss.StatusFlags & ovrStatus_OrientationTracked) + { auto pose = ss.HeadPose.ThePose; Quatf quat = pose.Orientation; float yaw, pitch, roll; quat.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&yaw, &pitch, &roll); + double yaw_ = yaw; if (s.useYawSpring) { - 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; + 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_; } - constexpr float d2r = 57.295781f; - data[Yaw] = yaw * -d2r; - data[Pitch] = pitch * d2r; - data[Roll] = roll * d2r; - data[TX] = pose.Position.x * -1e2; - data[TY] = pose.Position.y * 1e2; - data[TZ] = pose.Position.z * 1e2; + static constexpr double d2r = 180 / M_PI; + 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; } } } |