diff options
| -rw-r--r-- | tracker-rift-025/ftnoir_tracker_rift_025.cpp | 38 | ||||
| -rw-r--r-- | tracker-rift-042/ftnoir_tracker_rift_042.cpp | 32 | ||||
| -rw-r--r-- | tracker-rift-080/ftnoir_tracker_rift_080.cpp | 34 |
3 files changed, 61 insertions, 43 deletions
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 <cstdio> +#include <cmath> + +#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<Axis_Y, Axis_X, Axis_Z>(&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; } } 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; } } } 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; } } } |