Detect player rotation and flip joints

.gitignore

@@ -5,6 +5,7 @@
 *.slo
 *.lo
 *.o
+*.obj
 
 # Precompiled Headers
 *.gch
@@ -13,6 +14,7 @@
 # Compiled Dynamic libraries
 *.so
 *.dylib
+*.dll
 
 # Fortran module files
 *.mod
@@ -22,8 +24,15 @@
 *.lai
 *.la
 *.a
+*.lib
 
 # Executables
 *.exe
 *.out
 *.app
+
+#Folders
+.vs/
+.vsc/
+objs/
+bin/

driver_kinectV2/CKinectHandler.cpp

@@ -15,6 +15,11 @@ CKinectHandler::CKinectHandler()
     m_frameData->m_tick = 0U;
 
     m_paused = false;
+
+    detectionFOV_2 = DegreeToRadian(90.0f);
+    circleMinRadins= DegreeToRadian(-180.0f);
+    circleMaxRadins = DegreeToRadian(180.0f);
+    radinsDeg180 = DegreeToRadian(180.f);
 }
 
 CKinectHandler::~CKinectHandler()
@@ -95,7 +100,168 @@ void CKinectHandler::SetPaused(bool f_state)
     m_paused = f_state;
 }
 
-void CKinectHandler::Update()
+float CKinectHandler::DegreeToRadian(float degrees)
+{
+    return degrees * (float)M_PI / 180.0f;
+}
+
+vr::HmdQuaternionf_t CKinectHandler::GetRotation(vr::HmdMatrix34_t matrix)
+{
+    vr::HmdQuaternionf_t q;
+    q.w = sqrtf(fmaxf(0, 1 + matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2])) / 2.0f;
+    q.x = sqrtf(fmaxf(0, 1 + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2])) / 2.0f;
+    q.y = sqrtf(fmaxf(0, 1 - matrix.m[0][0] + matrix.m[1][1] - matrix.m[2][2])) / 2.0f;
+    q.z = sqrtf(fmaxf(0, 1 - matrix.m[0][0] - matrix.m[1][1] + matrix.m[2][2])) / 2.0f;
+    q.x = copysignf(q.x, matrix.m[2][1] - matrix.m[1][2]);
+    q.y = copysignf(q.y, matrix.m[0][2] - matrix.m[2][0]);
+    q.z = copysignf(q.z, matrix.m[1][0] - matrix.m[0][1]);
+    return q;
+}
+
+//I didnt't use the glm::yaw function because it doesn't account for the pole it is facing.
+//I am not convering all the values because I dont need them, save a little bit of CPU time.
+//https://www.reddit.com/r/Vive/comments/6toiem/how_to_get_each_axis_rotation_from_vive/
+float CKinectHandler::GetYawFromQuaternion(glm::quat quaternion)
+{
+    //If normalised is one, otherwise is correction factor
+    float unit = (quaternion.x * quaternion.x) +
+        (quaternion.y * quaternion.y) +
+        (quaternion.z * quaternion.z) +
+        (quaternion.w * quaternion.w);
+    float test = quaternion.x * quaternion.w - quaternion.y * quaternion.z;
+
+    if (test > 0.4995f * unit)
+    {
+        //Singularity at north pole
+        return 2.0f * atan2f(quaternion.y, quaternion.x);
+    }
+    else if (test < -0.4995f * unit)
+    {
+        //Singularity at south pole
+        return -2.0f * atan2f(quaternion.y, quaternion.x);
+    }
+    else
+    {
+        return atan2f(2.0f * quaternion.x * quaternion.w + 2.0f * quaternion.y * quaternion.z, 1.0f - 2.0f * (quaternion.z * quaternion.z + quaternion.w * quaternion.w));
+    }
+}
+
+glm::vec3 CKinectHandler::GetEulerFromQuaternion(glm::quat quaternion)
+{
+    float sqw = quaternion.w * quaternion.w;
+    float sqx = quaternion.x * quaternion.x;
+    float sqy = quaternion.y * quaternion.y;
+    float sqz = quaternion.z * quaternion.z;
+    float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
+    float test = quaternion.x * quaternion.w - quaternion.y * quaternion.z;
+    glm::vec3 v;
+
+    if (test > 0.4995f * unit)
+    {
+        //Singularity at north pole
+        v.y = 2.0f * atan2f(quaternion.y, quaternion.x);
+        v.x = (float)M_PI / 2.0f;
+        v.z = 0.0f;
+        return v;
+    }
+    else if (test < -0.4995f * unit)
+    {
+        //Singularity at south pole
+        v.y = -2.0f * atan2f(quaternion.y, quaternion.x);
+        v.x = -(float)M_PI / 2.0f;
+        v.z = 0.0f;
+        return v;
+    }
+    else
+    {
+        v.y = atan2f(2.0f * quaternion.x * quaternion.w + 2.0 * quaternion.y * quaternion.z, 1.0f - 2.0f * (quaternion.z * quaternion.z + quaternion.w * quaternion.w));
+        v.x = asinf(2.0f * (quaternion.x * quaternion.z - quaternion.w * quaternion.y));
+        v.z = atan2f(2.0f * quaternion.x * quaternion.y + 2.0 * quaternion.z * quaternion.w, 1.0f - 2.0f * (quaternion.y * quaternion.y + quaternion.z * quaternion.z));
+        return v;
+    }
+}
+
+float CKinectHandler::WrapAround(float origin, float change, float min, float max)
+{
+    if (origin + change > max)
+    {
+        return WrapAround(fmodf(min + (origin + change), max), 0, min, max);
+    }
+    else
+    {
+        return origin + change;
+    }
+}
+
+bool CKinectHandler::FacingKinect(float kinectYaw, float hmdYaw)
+{
+    float kinectMin = kinectYaw - detectionFOV_2;
+    float kinectMax = kinectYaw + detectionFOV_2;
+
+    if (kinectMax > circleMaxRadins)
+    {
+        float maxRangeOver = detectionFOV_2 - (circleMaxRadins - kinectYaw);
+        return !(hmdYaw > circleMinRadins && hmdYaw < circleMaxRadins || hmdYaw > circleMinRadins && hmdYaw < maxRangeOver);
+    }
+    else if (kinectMin < circleMinRadins)
+    {
+        float minRangeOver = circleMaxRadins - (detectionFOV_2 - kinectYaw);
+        return !(hmdYaw > circleMinRadins && hmdYaw < circleMaxRadins || hmdYaw > minRangeOver && hmdYaw < circleMaxRadins);
+    }
+    else
+    {
+        return !(hmdYaw > kinectMin && hmdYaw < kinectMax);
+    }
+}
+
+//All joints are being used so it is ok to just take all of the left and right joints and flip their values.
+//Make sure to reverse the rotation of all the joints.1
+void CKinectHandler::FlipJoints(Joint *joints, JointOrientation* jointOrientations)
+{
+    FlipJoint(joints[_JointType::JointType_ShoulderLeft], joints[_JointType::JointType_ShoulderRight]);
+    FlipJoint(joints[_JointType::JointType_ElbowLeft], joints[_JointType::JointType_ElbowRight]);
+    FlipJoint(joints[_JointType::JointType_WristLeft], joints[_JointType::JointType_WristRight]);
+    FlipJoint(joints[_JointType::JointType_HipLeft], joints[_JointType::JointType_HipRight]);
+    FlipJoint(joints[_JointType::JointType_KneeLeft], joints[_JointType::JointType_KneeRight]);
+    FlipJoint(joints[_JointType::JointType_AnkleLeft], joints[_JointType::JointType_AnkleRight]);
+    FlipJoint(joints[_JointType::JointType_FootLeft], joints[_JointType::JointType_FootRight]);
+    FlipJoint(joints[_JointType::JointType_HandTipLeft], joints[_JointType::JointType_HandTipRight]);
+    FlipJoint(joints[_JointType::JointType_ThumbLeft], joints[_JointType::JointType_ThumbRight]);
+
+    for (size_t i = 0U; i < _JointType::JointType_Count; i++)
+    {
+        RotateJointOrientation(jointOrientations[i]);
+    }
+}
+
+void CKinectHandler::FlipJoint(Joint &j1, Joint &j2)
+{
+    Joint tmpJoint = j1;
+    tmpJoint.JointType = j2.JointType;
+    j2.JointType = j1.JointType;
+    j1 = j2;
+    j2 = tmpJoint;
+}
+
+void CKinectHandler::RotateJointOrientation(JointOrientation& j)
+{
+    //I don't really know/understand much about quaternions so I am converting these to Euler values and then back to quaternions again.
+    //I don't want to do it this way but I don't understand how to do quaternion multiplication to rotate the device 180* around the z axis.
+    //It also seems that this isnt quite reliable as the emulated pucks seem to have rotation drift though that could be because the kinect has a harder time tracking someone who is turned around.
+    glm::vec3 xyz = GetEulerFromQuaternion(glm::quat(
+        j.Orientation.w,
+        j.Orientation.x,
+        j.Orientation.y,
+        j.Orientation.z
+    ));
+    glm::quat adjustedQuat = glm::quat(glm::vec3(xyz.x, WrapAround(xyz.y, radinsDeg180, circleMinRadins, circleMaxRadins), xyz.z));
+    j.Orientation.w = adjustedQuat.w;
+    j.Orientation.x = adjustedQuat.x;
+    j.Orientation.y = adjustedQuat.y;
+    j.Orientation.z = adjustedQuat.z;
+}
+
+void CKinectHandler::Update(glm::quat m_baseRotation)
 {
     if(m_kinectSensor && m_bodyFrameReader && !m_paused)
     {
@@ -113,6 +279,12 @@ void CKinectHandler::Update()
             IBody *l_bodies[BODY_COUNT] = { nullptr };
             if(l_bodyFrame->GetAndRefreshBodyData(BODY_COUNT, l_bodies) >= S_OK) // Only first visible body
             {
+                vr::TrackedDevicePose_t hmd_pose[1]; //HMD index is always 0.
+                vr::VRServerDriverHost()->GetRawTrackedDevicePoses(0, hmd_pose, 1);
+                vr::HmdQuaternionf_t hmdQuaternion = GetRotation(hmd_pose[0].mDeviceToAbsoluteTracking);
+                float hmdYaw = GetYawFromQuaternion(glm::quat(hmdQuaternion.w, hmdQuaternion.x, hmdQuaternion.y, hmdQuaternion.z));
+                float kinectYaw = GetYawFromQuaternion(m_baseRotation);
+
                 for(size_t i = 0U, j = static_cast<size_t>(BODY_COUNT); i < j; i++)
                 {
                     if(l_bodies[i])
@@ -126,6 +298,14 @@ void CKinectHandler::Update()
                                 JointOrientation l_jointOrientations[_JointType::JointType_Count];
                                 if((l_bodies[i]->GetJoints(_JointType::JointType_Count, l_joints) >= S_OK) && (l_bodies[i]->GetJointOrientations(JointType_Count, l_jointOrientations) >= S_OK))
                                 {
+                                    //Check if headset is facing away from the camera.
+                                    //Take the joints and switch the left DATA with the right.
+                                    //May need to do this for the JointOrientation too?
+                                    if (!FacingKinect(kinectYaw, hmdYaw))
+                                    {
+                                        FlipJoints(l_joints, l_jointOrientations);
+                                    }
+
                                     for(size_t k = 0U; k < _JointType::JointType_Count; k++)
                                     {
                                         m_jointFilters[k]->Update(l_joints[k]);

driver_kinectV2/CKinectHandler.h

@@ -23,9 +23,24 @@ class CKinectHandler final
 
     std::atomic<bool> m_paused;
 
+    float detectionFOV_2;
+    float circleMinRadins;
+    float circleMaxRadins;
+    float radinsDeg180;
+
     CKinectHandler(const CKinectHandler &that) = delete;
     CKinectHandler& operator=(const CKinectHandler &that) = delete;
 
+    float DegreeToRadian(float degrees);
+    vr::HmdQuaternionf_t GetRotation(vr::HmdMatrix34_t matrix);
+    float GetYawFromQuaternion(glm::quat quaternion);
+    glm::vec3 GetEulerFromQuaternion(glm::quat quaternion);
+    float WrapAround(float origin, float change, float min, float max);
+    bool FacingKinect(float kinectYaw, float hmdYaw);
+    void FlipJoints(Joint *joints, JointOrientation *jointOrientations);
+    void FlipJoint(Joint &j1, Joint &j2);
+    void RotateJointOrientation(JointOrientation& j);
+
     void Cleanup();
 public:
     explicit CKinectHandler();
@@ -39,5 +54,5 @@ class CKinectHandler final
     bool IsPaused() const;
     void SetPaused(bool f_state);
 
-    void Update();
+    void Update(glm::quat m_baseRotation);
 };

driver_kinectV2/CServerDriver.cpp

@@ -261,7 +261,7 @@ void CServerDriver::KinectProcess()
         if(l_initialized)
         {
             m_kinectLock.lock();
-            m_kinectHandler->Update();
+            m_kinectHandler->Update(m_baseRotation);
             m_kinectLock.unlock();
         }
         else l_initialized = m_kinectHandler->Initialize();

driver_kinectV2/driver_kinectV2.vcxproj

@@ -79,7 +79,7 @@
     <IncludePath>$(VC_IncludePath);$(WindowsSDK_IncludePath);$(KINECTSDK20_DIR)\inc;</IncludePath>
     <LibraryPath>$(VC_LibraryPath_x64);$(WindowsSDK_LibraryPath_x64);$(KINECTSDK20_DIR)\lib\x64;</LibraryPath>
     <PostBuildEventUseInBuild>false</PostBuildEventUseInBuild>
-    <OutDir>$(SolutionDir)bin\win64\</OutDir>
+    <OutDir>A:\Program Files %28x86%29\Steam\steamapps\common\SteamVR\drivers\kinectV2\bin\win64</OutDir>
     <IntDir>$(SolutionDir)objs\$(ProjectName)\$(Platform)\$(Configuration)\</IntDir>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">

driver_kinectV2/stdafx.h

@@ -21,3 +21,5 @@
 #include "glm/gtc/quaternion.hpp"
 
 #include "pugixml.hpp"
+
+#define M_PI 3.14159265358979323846