Skip to content

Replacing YPR state storaging with Hmd quaternion rotation state #8

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

Replacing YPR state storaging with Hmd quaternion rotation state #8

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
310b73e
Replacing YPR state storageing with Hmd quaternion rotation
pitboxx Sep 14, 2024
537d0f0
Apply reset to HMD Quaternion
pitboxx Sep 15, 2024
6546116
NormalizeQuaternion after hmd rotation
pitboxx Sep 22, 2024
47f77a4
Revert "NormalizeQuaternion after hmd rotation"This reverts commit 65…
pitboxx Oct 5, 2024
e59de30
Fix of HMD Pitch-Roll gimbal lock
pitboxx Oct 5, 2024
7d01b4e
Fix Yaw and Roll offset keys
pitboxx Oct 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
122 changes: 91 additions & 31 deletions OpenVR/samples/driver_arduinohmd/driver_arduinohmd.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
#include <thread>
#include <chrono>
#include <algorithm>
#include <cmath>

#if defined( _WINDOWS )
#include <windows.h>
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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 )
Expand Down Expand Up @@ -698,23 +734,25 @@ 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;
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)
{
Expand All @@ -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

Expand Down