diff options
Diffstat (limited to 'tracker-rift-042')
-rw-r--r-- | tracker-rift-042/ftnoir_tracker_rift_042.cpp | 32 |
1 files changed, 19 insertions, 13 deletions
diff --git a/tracker-rift-042/ftnoir_tracker_rift_042.cpp b/tracker-rift-042/ftnoir_tracker_rift_042.cpp index bee19ee6..853dfcf9 100644 --- a/tracker-rift-042/ftnoir_tracker_rift_042.cpp +++ b/tracker-rift-042/ftnoir_tracker_rift_042.cpp @@ -5,6 +5,11 @@ #include "Kernel/OVR_Math.h" #include <cstdio> #include <cstring> +#include <cmath> + +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif using namespace OVR; @@ -51,22 +56,23 @@ void Rift_Tracker::data(double *data) 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 double d2r = 57.295781; - 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; } } } |