From 310b73e9dd335e302824b9b2aa2e8240ce030f09 Mon Sep 17 00:00:00 2001 From: pitboxx Date: Sat, 14 Sep 2024 18:33:30 +0700 Subject: [PATCH 1/6] Replacing YPR state storageing with Hmd quaternion rotation --- .../driver_arduinohmd/driver_arduinohmd.cpp | 80 ++++++++++++++++--- 1 file changed, 70 insertions(+), 10 deletions(-) diff --git a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp index 9dd9495..014da15 100644 --- a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp +++ b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #if defined( _WINDOWS ) #include @@ -112,6 +113,7 @@ struct QuaternionF { float w, x, y, z; }; QuaternionF ArduinoIMUQuat = { 0, 0, 0, 0 }, HMDQuatOffset = { 0, 0, 0, 0 }, LastArduinoIMUQuat = { 0, 0, 0, 0 }; +HmdQuaternion_t hmdQuat = HmdQuaternion_Init(1, 0, 0, 0); // Quaternion of HMD state double DegToRad(double f) { return f * (3.14159265358979323846 / 180); @@ -147,6 +149,27 @@ inline vr::HmdQuaternion_t EulerAngleToQuaternion(double Yaw, double Pitch, doub return q; } +inline void RotateQuaternion(HmdQuaternion_t& q, double yaw, double pitch, double roll) { + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + // Quaternions combination to rotate YPR + double combinedW = cy * cp * cr - sy * sp * sr; + double combinedX = cy * cp * sr - sy * sp * cr; + double combinedY = cy * sp * cr + sy * cp * sr; + double combinedZ = -cy * sp * sr + sy * cp * cr; + + // Apply Rotation to target quaternion + q.w = q.w * combinedW - q.x * combinedX - q.y * combinedY - q.z * combinedZ; + q.x = q.w * combinedX + q.x * combinedW + q.y * combinedZ - q.z * combinedY; + q.y = q.w * combinedY - q.x * combinedZ + q.y * combinedW + q.z * combinedX; + q.z = q.w * combinedZ + q.x * combinedY - q.y * combinedX + q.z * combinedW; +} + bool CorrectAngleValue(float Value) { if (Value > -180 && Value < 180) @@ -204,10 +227,14 @@ void ArduinoIMURead() } else if (CorrectAngleValue(ArduinoIMU[0]) && CorrectAngleValue(ArduinoIMU[1]) && CorrectAngleValue(ArduinoIMU[2])) // save last correct values { + yprOffset[0] = ArduinoIMU[0] - LastArduinoIMU[0]; + yprOffset[1] = ArduinoIMU[1] - LastArduinoIMU[1]; + yprOffset[2] = ArduinoIMU[2] - LastArduinoIMU[2]; + LastArduinoIMU[0] = ArduinoIMU[0]; LastArduinoIMU[1] = ArduinoIMU[1]; LastArduinoIMU[2] = ArduinoIMU[2]; - + if (HMDInitCentring == false) if (ArduinoIMU[0] != 0 || ArduinoIMU[1] != 0 || ArduinoIMU[2] != 0) { SetCentering(); @@ -647,6 +674,20 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl *pfTop = -1.0; *pfBottom = 1.0; } + } + + void MultiplyVectorByQuaternion(double vec[3], const vr::HmdQuaternion_t& q) { + double x = vec[0], y = vec[1], z = vec[2]; + double qx = q.x, qy = q.y, qz = q.z, qw = q.w; + + double ix = qw * x + qy * z - qz * y; + double iy = qw * y + qz * x - qx * z; + double iz = qw * z + qx * y - qy * x; + double iw = -qx * x - qy * y - qz * z; + + vec[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; + vec[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; + vec[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; } virtual DistortionCoordinates_t ComputeDistortion( EVREye eEye, float fU, float fV ) @@ -698,15 +739,17 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl if (HMDConnected || ArduinoNotRequire) { + double posOffset[3] = { 0, 0, 0 }; + // Pos - if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) fPos[2] -= StepPos; - if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) fPos[2] += StepPos; + if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) posOffset[2] -= StepPos; + if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) posOffset[2] += StepPos; - if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) fPos[0] -= StepPos; - if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) fPos[0] += StepPos; + if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) posOffset[0] -= StepPos; + if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) posOffset[0] += StepPos; - if ((GetAsyncKeyState(m_hmdDownKey) & 0x8000) != 0) fPos[1] += StepPos; - if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) fPos[1] -= StepPos; + if ((GetAsyncKeyState(m_hmdDownKey) & 0x8000) != 0) posOffset[1] += StepPos; + if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) posOffset[1] -= StepPos; // Yaw fixing if ((GetAsyncKeyState(VK_NUMPAD1) & 0x8000) != 0 && yprOffset[0] < 180) yprOffset[0] += StepRot; @@ -729,9 +772,20 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl // Set head tracking rotation if (ArduinoRotationType == false) { // YPR - pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ), - DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ), - DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 )); + RotateQuaternion( + hmdQuat, + DegToRad(yprOffset[2]), + DegToRad(yprOffset[0]), + DegToRad(yprOffset[1])); + + // Assign updated rotation to HMD + pose.qRotation = hmdQuat; + + // Reset YPR offsets to prevent drifting + yprOffset[0] = 0; + yprOffset[1] = 0; + yprOffset[2] = 0; + } else { // Quaternion // Centered pose.qRotation.w = ArduinoIMUQuat.w * LastArduinoIMUQuat.w - ArduinoIMUQuat.x * LastArduinoIMUQuat.x - ArduinoIMUQuat.y * LastArduinoIMUQuat.y - ArduinoIMUQuat.z * LastArduinoIMUQuat.z; @@ -741,6 +795,12 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl } //Set head position tracking + MultiplyVectorByQuaternion(posOffset, hmdQuat); + + fPos[0] += posOffset[0]; + fPos[1] += posOffset[1]; + fPos[2] += posOffset[2]; + pose.vecPosition[0] = fPos[0]; // X pose.vecPosition[1] = fPos[1]; // Z From 537d0f00b16d388a990eed290bc32ee4ca016606 Mon Sep 17 00:00:00 2001 From: pitboxx Date: Sun, 15 Sep 2024 22:48:03 +0700 Subject: [PATCH 2/6] Apply reset to HMD Quaternion --- .../samples/driver_arduinohmd/driver_arduinohmd.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp index 014da15..620db20 100644 --- a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp +++ b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp @@ -189,9 +189,14 @@ bool CorrectQuatValue(float Value) void SetCentering() { if (ArduinoRotationType == false) { - yprOffset[0] = ArduinoIMU[0]; - yprOffset[1] = ArduinoIMU[1]; - yprOffset[2] = ArduinoIMU[2]; + // Update HMD quaternion with current state of IMU sensor + hmdQuat = HmdQuaternion_Init(1.0, 0, 0, 0); + + RotateQuaternion( + hmdQuat, + DegToRad(ArduinoIMU[2]), + DegToRad(ArduinoIMU[0]), + DegToRad(ArduinoIMU[1])); } else { float length = std::sqrt(ArduinoIMUQuat.x * ArduinoIMUQuat.x + ArduinoIMUQuat.y * ArduinoIMUQuat.y + ArduinoIMUQuat.z * ArduinoIMUQuat.z + ArduinoIMUQuat.w * ArduinoIMUQuat.w); From 65461168085ef8110f1d316409ce5e55a93686a2 Mon Sep 17 00:00:00 2001 From: pitboxx Date: Sun, 22 Sep 2024 22:19:56 +0700 Subject: [PATCH 3/6] NormalizeQuaternion after hmd rotation --- .../driver_arduinohmd/driver_arduinohmd.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp index 620db20..01e6c00 100644 --- a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp +++ b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp @@ -170,6 +170,23 @@ inline void RotateQuaternion(HmdQuaternion_t& q, double yaw, double pitch, doubl q.z = q.w * combinedZ + q.x * combinedY - q.y * combinedX + q.z * combinedW; } +inline void NormalizeQuaternion(HmdQuaternion_t& q) { + double magnitude = sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + + if (magnitude > 0.0) { + q.w /= magnitude; + q.x /= magnitude; + q.y /= magnitude; + q.z /= magnitude; + } + else { + q.w = 1.0; + q.x = 0.0; + q.y = 0.0; + q.z = 0.0; + } +} + bool CorrectAngleValue(float Value) { if (Value > -180 && Value < 180) @@ -197,6 +214,8 @@ void SetCentering() DegToRad(ArduinoIMU[2]), DegToRad(ArduinoIMU[0]), DegToRad(ArduinoIMU[1])); + + NormalizeQuaternion(hmdQuat); } else { float length = std::sqrt(ArduinoIMUQuat.x * ArduinoIMUQuat.x + ArduinoIMUQuat.y * ArduinoIMUQuat.y + ArduinoIMUQuat.z * ArduinoIMUQuat.z + ArduinoIMUQuat.w * ArduinoIMUQuat.w); @@ -782,6 +801,8 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl DegToRad(yprOffset[2]), DegToRad(yprOffset[0]), DegToRad(yprOffset[1])); + + NormalizeQuaternion(hmdQuat); // Assign updated rotation to HMD pose.qRotation = hmdQuat; From 47f77a43e6fcdca5aa523151355bf44d4eeecbb9 Mon Sep 17 00:00:00 2001 From: pitboxx Date: Sun, 6 Oct 2024 00:37:00 +0700 Subject: [PATCH 4/6] Revert "NormalizeQuaternion after hmd rotation"This reverts commit 65461168085ef8110f1d316409ce5e55a93686a2.Revert "Apply reset to HMD Quaternion"This reverts commit 537d0f00b16d388a990eed290bc32ee4ca016606. Revert "Replacing YPR state storageing with Hmd quaternion rotation" This reverts commit 310b73e9dd335e302824b9b2aa2e8240ce030f09. --- .../driver_arduinohmd/driver_arduinohmd.cpp | 112 ++---------------- 1 file changed, 13 insertions(+), 99 deletions(-) diff --git a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp index 01e6c00..9dd9495 100644 --- a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp +++ b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #if defined( _WINDOWS ) #include @@ -113,7 +112,6 @@ struct QuaternionF { float w, x, y, z; }; QuaternionF ArduinoIMUQuat = { 0, 0, 0, 0 }, HMDQuatOffset = { 0, 0, 0, 0 }, LastArduinoIMUQuat = { 0, 0, 0, 0 }; -HmdQuaternion_t hmdQuat = HmdQuaternion_Init(1, 0, 0, 0); // Quaternion of HMD state double DegToRad(double f) { return f * (3.14159265358979323846 / 180); @@ -149,44 +147,6 @@ inline vr::HmdQuaternion_t EulerAngleToQuaternion(double Yaw, double Pitch, doub return q; } -inline void RotateQuaternion(HmdQuaternion_t& q, double yaw, double pitch, double roll) { - double cy = cos(yaw * 0.5); - double sy = sin(yaw * 0.5); - double cp = cos(pitch * 0.5); - double sp = sin(pitch * 0.5); - double cr = cos(roll * 0.5); - double sr = sin(roll * 0.5); - - // Quaternions combination to rotate YPR - double combinedW = cy * cp * cr - sy * sp * sr; - double combinedX = cy * cp * sr - sy * sp * cr; - double combinedY = cy * sp * cr + sy * cp * sr; - double combinedZ = -cy * sp * sr + sy * cp * cr; - - // Apply Rotation to target quaternion - q.w = q.w * combinedW - q.x * combinedX - q.y * combinedY - q.z * combinedZ; - q.x = q.w * combinedX + q.x * combinedW + q.y * combinedZ - q.z * combinedY; - q.y = q.w * combinedY - q.x * combinedZ + q.y * combinedW + q.z * combinedX; - q.z = q.w * combinedZ + q.x * combinedY - q.y * combinedX + q.z * combinedW; -} - -inline void NormalizeQuaternion(HmdQuaternion_t& q) { - double magnitude = sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); - - if (magnitude > 0.0) { - q.w /= magnitude; - q.x /= magnitude; - q.y /= magnitude; - q.z /= magnitude; - } - else { - q.w = 1.0; - q.x = 0.0; - q.y = 0.0; - q.z = 0.0; - } -} - bool CorrectAngleValue(float Value) { if (Value > -180 && Value < 180) @@ -206,16 +166,9 @@ bool CorrectQuatValue(float Value) void SetCentering() { if (ArduinoRotationType == false) { - // Update HMD quaternion with current state of IMU sensor - hmdQuat = HmdQuaternion_Init(1.0, 0, 0, 0); - - RotateQuaternion( - hmdQuat, - DegToRad(ArduinoIMU[2]), - DegToRad(ArduinoIMU[0]), - DegToRad(ArduinoIMU[1])); - - NormalizeQuaternion(hmdQuat); + yprOffset[0] = ArduinoIMU[0]; + yprOffset[1] = ArduinoIMU[1]; + yprOffset[2] = ArduinoIMU[2]; } else { float length = std::sqrt(ArduinoIMUQuat.x * ArduinoIMUQuat.x + ArduinoIMUQuat.y * ArduinoIMUQuat.y + ArduinoIMUQuat.z * ArduinoIMUQuat.z + ArduinoIMUQuat.w * ArduinoIMUQuat.w); @@ -251,14 +204,10 @@ void ArduinoIMURead() } else if (CorrectAngleValue(ArduinoIMU[0]) && CorrectAngleValue(ArduinoIMU[1]) && CorrectAngleValue(ArduinoIMU[2])) // save last correct values { - yprOffset[0] = ArduinoIMU[0] - LastArduinoIMU[0]; - yprOffset[1] = ArduinoIMU[1] - LastArduinoIMU[1]; - yprOffset[2] = ArduinoIMU[2] - LastArduinoIMU[2]; - LastArduinoIMU[0] = ArduinoIMU[0]; LastArduinoIMU[1] = ArduinoIMU[1]; LastArduinoIMU[2] = ArduinoIMU[2]; - + if (HMDInitCentring == false) if (ArduinoIMU[0] != 0 || ArduinoIMU[1] != 0 || ArduinoIMU[2] != 0) { SetCentering(); @@ -698,20 +647,6 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl *pfTop = -1.0; *pfBottom = 1.0; } - } - - void MultiplyVectorByQuaternion(double vec[3], const vr::HmdQuaternion_t& q) { - double x = vec[0], y = vec[1], z = vec[2]; - double qx = q.x, qy = q.y, qz = q.z, qw = q.w; - - double ix = qw * x + qy * z - qz * y; - double iy = qw * y + qz * x - qx * z; - double iz = qw * z + qx * y - qy * x; - double iw = -qx * x - qy * y - qz * z; - - vec[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; - vec[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; - vec[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; } virtual DistortionCoordinates_t ComputeDistortion( EVREye eEye, float fU, float fV ) @@ -763,17 +698,15 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl if (HMDConnected || ArduinoNotRequire) { - double posOffset[3] = { 0, 0, 0 }; - // Pos - if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) posOffset[2] -= StepPos; - if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) posOffset[2] += StepPos; + if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) fPos[2] -= StepPos; + if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) fPos[2] += StepPos; - if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) posOffset[0] -= StepPos; - if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) posOffset[0] += StepPos; + if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) fPos[0] -= StepPos; + if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) fPos[0] += StepPos; - if ((GetAsyncKeyState(m_hmdDownKey) & 0x8000) != 0) posOffset[1] += StepPos; - if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) posOffset[1] -= StepPos; + if ((GetAsyncKeyState(m_hmdDownKey) & 0x8000) != 0) fPos[1] += StepPos; + if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) fPos[1] -= StepPos; // Yaw fixing if ((GetAsyncKeyState(VK_NUMPAD1) & 0x8000) != 0 && yprOffset[0] < 180) yprOffset[0] += StepRot; @@ -796,22 +729,9 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl // Set head tracking rotation if (ArduinoRotationType == false) { // YPR - RotateQuaternion( - hmdQuat, - DegToRad(yprOffset[2]), - DegToRad(yprOffset[0]), - DegToRad(yprOffset[1])); - - NormalizeQuaternion(hmdQuat); - - // Assign updated rotation to HMD - pose.qRotation = hmdQuat; - - // Reset YPR offsets to prevent drifting - yprOffset[0] = 0; - yprOffset[1] = 0; - yprOffset[2] = 0; - + pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ), + DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ), + DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 )); } else { // Quaternion // Centered pose.qRotation.w = ArduinoIMUQuat.w * LastArduinoIMUQuat.w - ArduinoIMUQuat.x * LastArduinoIMUQuat.x - ArduinoIMUQuat.y * LastArduinoIMUQuat.y - ArduinoIMUQuat.z * LastArduinoIMUQuat.z; @@ -821,12 +741,6 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl } //Set head position tracking - MultiplyVectorByQuaternion(posOffset, hmdQuat); - - fPos[0] += posOffset[0]; - fPos[1] += posOffset[1]; - fPos[2] += posOffset[2]; - pose.vecPosition[0] = fPos[0]; // X pose.vecPosition[1] = fPos[1]; // Z From e59de3023e7e15c05192dad1b7bdc72e9c57e479 Mon Sep 17 00:00:00 2001 From: pitboxx Date: Sun, 6 Oct 2024 00:39:19 +0700 Subject: [PATCH 5/6] Fix of HMD Pitch-Roll gimbal lock --- .../driver_arduinohmd/driver_arduinohmd.cpp | 114 +++++++++++++----- 1 file changed, 87 insertions(+), 27 deletions(-) diff --git a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp index 9dd9495..6dae86d 100644 --- a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp +++ b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #if defined( _WINDOWS ) #include @@ -128,23 +129,44 @@ float OffsetYPR(float f, float f2) return f; } -inline vr::HmdQuaternion_t EulerAngleToQuaternion(double Yaw, double Pitch, double Roll) -{ - vr::HmdQuaternion_t q; - // Abbreviations for the various angular functions - double cy = cos(Yaw * 0.5); - double sy = sin(Yaw * 0.5); - double cp = cos(Pitch * 0.5); - double sp = sin(Pitch * 0.5); - double cr = cos(Roll * 0.5); - double sr = sin(Roll * 0.5); - - q.w = cr * cp * cy + sr * sp * sy; - q.x = sr * cp * cy - cr * sp * sy; - q.y = cr * sp * cy + sr * cp * sy; - q.z = cr * cp * sy - sr * sp * cy; - - return q; +void createRollQuaternion(double angle, HmdQuaternion_t& q) { + q.w = cos(angle / 2.0); + q.x = sin(angle / 2.0); + q.y = 0.0; + q.z = 0.0; +} + +void createPitchQuaternion(double angle, HmdQuaternion_t& q) { + q.w = cos(angle / 2.0); + q.x = 0.0; + q.y = sin(angle / 2.0); + q.z = 0.0; +} + +void createYawQuaternion(double angle, HmdQuaternion_t& q) { + q.w = cos(angle / 2.0); + q.x = 0.0; + q.y = 0.0; + q.z = sin(angle / 2.0); +} + +HmdQuaternion_t multiplyQuaternions(const HmdQuaternion_t& q1, const HmdQuaternion_t& q2) { + HmdQuaternion_t result; + result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; + result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y; + result.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x; + result.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w; + return result; +} + +HmdQuaternion_t normalizeQuaternion(const HmdQuaternion_t& q) { + HmdQuaternion_t result = q; + double norm = sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + result.w /= norm; + result.x /= norm; + result.y /= norm; + result.z /= norm; + return result; } bool CorrectAngleValue(float Value) @@ -647,6 +669,20 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl *pfTop = -1.0; *pfBottom = 1.0; } + } + + void MultiplyVectorByQuaternion(double vec[3], const vr::HmdQuaternion_t& q) { + double x = vec[0], y = vec[1], z = vec[2]; + double qx = q.x, qy = q.y, qz = q.z, qw = q.w; + + double ix = qw * x + qy * z - qz * y; + double iy = qw * y + qz * x - qx * z; + double iz = qw * z + qx * y - qy * x; + double iw = -qx * x - qy * y - qz * z; + + vec[0] = ix * qw + iw * -qx + iy * -qz - iz * -qy; + vec[1] = iy * qw + iw * -qy + iz * -qx - ix * -qz; + vec[2] = iz * qw + iw * -qz + ix * -qy - iy * -qx; } virtual DistortionCoordinates_t ComputeDistortion( EVREye eEye, float fU, float fV ) @@ -698,15 +734,17 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl if (HMDConnected || ArduinoNotRequire) { + double posOffset[3] = { 0, 0, 0 }; + // Pos - if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) fPos[2] -= StepPos; - if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) fPos[2] += StepPos; + if ((GetAsyncKeyState(VK_NUMPAD8) & 0x8000) != 0) posOffset[2] -= StepPos; + if ((GetAsyncKeyState(VK_NUMPAD2) & 0x8000) != 0) posOffset[2] += StepPos; - if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) fPos[0] -= StepPos; - if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) fPos[0] += StepPos; + if ((GetAsyncKeyState(VK_NUMPAD4) & 0x8000) != 0) posOffset[0] -= StepPos; + if ((GetAsyncKeyState(VK_NUMPAD6) & 0x8000) != 0) posOffset[0] += StepPos; - if ((GetAsyncKeyState(m_hmdDownKey) & 0x8000) != 0) fPos[1] += StepPos; - if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) fPos[1] -= StepPos; + if ((GetAsyncKeyState(m_hmdDownKey) & 0x8000) != 0) posOffset[1] += StepPos; + if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) posOffset[1] -= StepPos; // Yaw fixing if ((GetAsyncKeyState(VK_NUMPAD1) & 0x8000) != 0 && yprOffset[0] < 180) yprOffset[0] += StepRot; @@ -727,20 +765,42 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl if ( (GetAsyncKeyState(m_centeringKey) & 0x8000) != 0 || ( (GetAsyncKeyState(VK_CONTROL) & 0x8000) != 0 && (GetAsyncKeyState(VK_MENU) & 0x8000) != 0 && (GetAsyncKeyState('R') & 0x8000) != 0) ) SetCentering(); + HmdQuaternion_t hmdQuat = HmdQuaternion_Init(1, 0, 0, 0); + // Set head tracking rotation if (ArduinoRotationType == false) { // YPR - pose.qRotation = EulerAngleToQuaternion(DegToRad( OffsetYPR(ArduinoIMU[2], yprOffset[2]) ), - DegToRad( OffsetYPR(ArduinoIMU[0], yprOffset[0]) * -1 ), - DegToRad( OffsetYPR(ArduinoIMU[1], yprOffset[1]) * -1 )); + + HmdQuaternion_t yawQuaternion; + createPitchQuaternion(DegToRad(OffsetYPR(ArduinoIMU[0], yprOffset[0])), yawQuaternion); + HmdQuaternion_t rollQuaternion; + createRollQuaternion(DegToRad(OffsetYPR(ArduinoIMU[2], yprOffset[2])) * -1, rollQuaternion); + HmdQuaternion_t pitchQuaternion; + createYawQuaternion(DegToRad(OffsetYPR(ArduinoIMU[1], yprOffset[1])), pitchQuaternion); + + hmdQuat = multiplyQuaternions(hmdQuat, yawQuaternion); + hmdQuat = multiplyQuaternions(hmdQuat, rollQuaternion); + hmdQuat = multiplyQuaternions(hmdQuat, pitchQuaternion); + hmdQuat = normalizeQuaternion(hmdQuat); + + // Assign updated rotation to HMD + pose.qRotation = hmdQuat; + + MultiplyVectorByQuaternion(posOffset, hmdQuat); } else { // Quaternion // Centered pose.qRotation.w = ArduinoIMUQuat.w * LastArduinoIMUQuat.w - ArduinoIMUQuat.x * LastArduinoIMUQuat.x - ArduinoIMUQuat.y * LastArduinoIMUQuat.y - ArduinoIMUQuat.z * LastArduinoIMUQuat.z; pose.qRotation.x = ArduinoIMUQuat.w * LastArduinoIMUQuat.x + ArduinoIMUQuat.x * LastArduinoIMUQuat.w + ArduinoIMUQuat.y * LastArduinoIMUQuat.z - ArduinoIMUQuat.z * LastArduinoIMUQuat.y; pose.qRotation.y = ArduinoIMUQuat.w * LastArduinoIMUQuat.y - ArduinoIMUQuat.x * LastArduinoIMUQuat.z + ArduinoIMUQuat.y * LastArduinoIMUQuat.w + ArduinoIMUQuat.z * LastArduinoIMUQuat.x; pose.qRotation.z = ArduinoIMUQuat.w * LastArduinoIMUQuat.z + ArduinoIMUQuat.x * LastArduinoIMUQuat.y - ArduinoIMUQuat.y * LastArduinoIMUQuat.x + ArduinoIMUQuat.z * LastArduinoIMUQuat.w; + + MultiplyVectorByQuaternion(posOffset, pose.qRotation); } - //Set head position tracking + // Moving HMD position in the direction of the view + fPos[0] += posOffset[0]; + fPos[1] += posOffset[1]; + fPos[2] += posOffset[2]; + pose.vecPosition[0] = fPos[0]; // X pose.vecPosition[1] = fPos[1]; // Z From 7d01b4e31d0e1ca911f5c6a30f237e0595d29d1f Mon Sep 17 00:00:00 2001 From: pitboxx Date: Sun, 20 Oct 2024 22:53:23 +0700 Subject: [PATCH 6/6] Fix Yaw and Roll offset keys --- OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp index 6dae86d..523aa9b 100644 --- a/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp +++ b/OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp @@ -747,12 +747,12 @@ class CDeviceDriver : public vr::ITrackedDeviceServerDriver, public vr::IVRDispl if ((GetAsyncKeyState(m_hmdUpKey) & 0x8000) != 0) posOffset[1] -= StepPos; // Yaw fixing - if ((GetAsyncKeyState(VK_NUMPAD1) & 0x8000) != 0 && yprOffset[0] < 180) yprOffset[0] += StepRot; - if ((GetAsyncKeyState(VK_NUMPAD3) & 0x8000) != 0 && yprOffset[0] > -180) yprOffset[0] -= StepRot; + if ((GetAsyncKeyState(VK_NUMPAD1) & 0x8000) != 0 && yprOffset[0] < 180) yprOffset[0] -= StepRot; + if ((GetAsyncKeyState(VK_NUMPAD3) & 0x8000) != 0 && yprOffset[0] > -180) yprOffset[0] += StepRot; // Roll fixing - if ((GetAsyncKeyState(VK_NUMPAD7) & 0x8000) != 0 && yprOffset[2] < 180) yprOffset[2] += StepRot; - if ((GetAsyncKeyState(VK_NUMPAD9) & 0x8000) != 0 && yprOffset[2] > -180) yprOffset[2] -= StepRot; + if ((GetAsyncKeyState(VK_NUMPAD7) & 0x8000) != 0 && yprOffset[1] < 180) yprOffset[1] -= StepRot; + if ((GetAsyncKeyState(VK_NUMPAD9) & 0x8000) != 0 && yprOffset[1] > -180) yprOffset[1] += StepRot; if ((GetAsyncKeyState(m_hmdResetKey) & 0x8000) != 0) {