From 3ffc2845d404fa4d21bd2204a485f3317f2eb895 Mon Sep 17 00:00:00 2001 From: Tristan Read <60425965+kOFReadie@users.noreply.github.com> Date: Sun, 5 Sep 2021 18:49:16 +0100 Subject: [PATCH] Detect player rotation and flip joints --- .gitignore | 9 ++ driver_kinectV2/CKinectHandler.cpp | 182 +++++++++++++++++++++++- driver_kinectV2/CKinectHandler.h | 17 ++- driver_kinectV2/CServerDriver.cpp | 2 +- driver_kinectV2/driver_kinectV2.vcxproj | 2 +- driver_kinectV2/stdafx.h | 2 + 6 files changed, 210 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index 3baf41f..80cdbf5 100644 --- a/.gitignore +++ b/.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/ \ No newline at end of file diff --git a/driver_kinectV2/CKinectHandler.cpp b/driver_kinectV2/CKinectHandler.cpp index 361063a..5f3ce10 100644 --- a/driver_kinectV2/CKinectHandler.cpp +++ b/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(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]); diff --git a/driver_kinectV2/CKinectHandler.h b/driver_kinectV2/CKinectHandler.h index b95a1f2..2a7bbe6 100644 --- a/driver_kinectV2/CKinectHandler.h +++ b/driver_kinectV2/CKinectHandler.h @@ -23,9 +23,24 @@ class CKinectHandler final std::atomic 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); }; diff --git a/driver_kinectV2/CServerDriver.cpp b/driver_kinectV2/CServerDriver.cpp index 2c51245..b9f9890 100644 --- a/driver_kinectV2/CServerDriver.cpp +++ b/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(); diff --git a/driver_kinectV2/driver_kinectV2.vcxproj b/driver_kinectV2/driver_kinectV2.vcxproj index bac9310..a980a9c 100644 --- a/driver_kinectV2/driver_kinectV2.vcxproj +++ b/driver_kinectV2/driver_kinectV2.vcxproj @@ -79,7 +79,7 @@ $(VC_IncludePath);$(WindowsSDK_IncludePath);$(KINECTSDK20_DIR)\inc; $(VC_LibraryPath_x64);$(WindowsSDK_LibraryPath_x64);$(KINECTSDK20_DIR)\lib\x64; false - $(SolutionDir)bin\win64\ + A:\Program Files %28x86%29\Steam\steamapps\common\SteamVR\drivers\kinectV2\bin\win64 $(SolutionDir)objs\$(ProjectName)\$(Platform)\$(Configuration)\ diff --git a/driver_kinectV2/stdafx.h b/driver_kinectV2/stdafx.h index e49a386..c98ba30 100644 --- a/driver_kinectV2/stdafx.h +++ b/driver_kinectV2/stdafx.h @@ -21,3 +21,5 @@ #include "glm/gtc/quaternion.hpp" #include "pugixml.hpp" + +#define M_PI 3.14159265358979323846 \ No newline at end of file