From 52a069294a8c7d3ea0edae4bf8d77b1cea9290b9 Mon Sep 17 00:00:00 2001 From: danwillm <39023874+danwillm@users.noreply.github.com> Date: Sat, 13 Mar 2021 22:32:27 +0000 Subject: [PATCH 1/2] refactor to include const references instead of pass by value, refactor controller pose --- .../include/Comm/SerialCommunicationManager.h | 2 +- LucidGloves/include/ControllerDriver.h | 2 +- LucidGloves/include/ControllerPose.h | 3 -- LucidGloves/include/Quaternion.h | 11 ++--- LucidGloves/src/ControllerDriver.cpp | 4 +- LucidGloves/src/ControllerPose.cpp | 44 +++++++++---------- LucidGloves/src/DeviceFactory.cpp | 2 +- LucidGloves/src/Quaternion.cpp | 23 +++++++--- 8 files changed, 49 insertions(+), 42 deletions(-) diff --git a/LucidGloves/include/Comm/SerialCommunicationManager.h b/LucidGloves/include/Comm/SerialCommunicationManager.h index be63609d..eeac46df 100644 --- a/LucidGloves/include/Comm/SerialCommunicationManager.h +++ b/LucidGloves/include/Comm/SerialCommunicationManager.h @@ -22,7 +22,7 @@ struct VRSerialConfiguration_t { class SerialManager : public ICommunicationManager { public: - SerialManager(VRSerialConfiguration_t configuration) : m_configuration(configuration) {}; + SerialManager(VRSerialConfiguration_t configuration) : m_configuration(configuration), m_isConnected(false), m_hSerial(0), m_errors(0) {}; //connect to the device using serial void Connect(); //start a thread that listens for updates from the device and calls the callback with data diff --git a/LucidGloves/include/ControllerDriver.h b/LucidGloves/include/ControllerDriver.h index af842bca..aed512d1 100644 --- a/LucidGloves/include/ControllerDriver.h +++ b/LucidGloves/include/ControllerDriver.h @@ -43,7 +43,7 @@ class too. Those comment blocks have some good information. **/ class ControllerDriver : public vr::ITrackedDeviceServerDriver { public: - ControllerDriver(const VRDeviceConfiguration_t settings); + ControllerDriver(const VRDeviceConfiguration_t &settings); /** Initialize your controller here. Give OpenVR information about your controller and set up handles to inform OpenVR when diff --git a/LucidGloves/include/ControllerPose.h b/LucidGloves/include/ControllerPose.h index 2913a781..e29a038d 100644 --- a/LucidGloves/include/ControllerPose.h +++ b/LucidGloves/include/ControllerPose.h @@ -17,9 +17,6 @@ class ControllerPose { //This member variable is the id of the controller we are "shadowing" and receiving positioning from. short int m_shadowControllerId = -1; - //This is the pose we send back for OpenVR to process as our pose for the driver. - //This represents the orientation and rotation, as well as angular velocity, etc. - vr::DriverPose_t m_pose; vr::HmdQuaternion_t m_offsetQuaternion; VRDeviceConfiguration_t m_configuration; diff --git a/LucidGloves/include/Quaternion.h b/LucidGloves/include/Quaternion.h index 0f24a4cd..5abb0056 100644 --- a/LucidGloves/include/Quaternion.h +++ b/LucidGloves/include/Quaternion.h @@ -8,13 +8,14 @@ double DegToRad(int degrees); double RadToDeg(double rad); //get the quaternion for roation from a matrix -vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t matrix); -vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t matrix); +vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t& matrix); +vr::HmdVector3_t GetPosition(const vr::HmdMatrix34_t& matrix); +vr::HmdVector3_t CombinePosition(vr::HmdMatrix34_t& matrix, vr::HmdVector3_t& vec); //returns the result of multiplying two quaternions, effectively applying a roatation on a quaternion -vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t q, const vr::HmdQuaternion_t r); +vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t& q, const vr::HmdQuaternion_t& r); vr::HmdQuaternion_t QuaternionFromAngle(const double& xx, const double& yy, const double& zz, const double& a); vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z); -vr::HmdMatrix33_t Get33Matrix(const vr::HmdMatrix34_t matrix); -vr::HmdVector3_t MultiplyMatrix(vr::HmdMatrix33_t matrix, const vr::HmdVector3_t vector); \ No newline at end of file +vr::HmdMatrix33_t GetRotationMatrix(const vr::HmdMatrix34_t& matrix); +vr::HmdVector3_t MultiplyMatrix(vr::HmdMatrix33_t& matrix, const vr::HmdVector3_t& vector); diff --git a/LucidGloves/src/ControllerDriver.cpp b/LucidGloves/src/ControllerDriver.cpp index 157175b2..51729d01 100644 --- a/LucidGloves/src/ControllerDriver.cpp +++ b/LucidGloves/src/ControllerDriver.cpp @@ -1,7 +1,7 @@ #include "ControllerDriver.h" -ControllerDriver::ControllerDriver(const VRDeviceConfiguration_t configuration) - : m_configuration(configuration) { +ControllerDriver::ControllerDriver(const VRDeviceConfiguration_t &configuration) + : m_configuration(configuration), m_driverId(-1) { //copy a default bone transform to our hand transform for use in finger positioning later std::copy( diff --git a/LucidGloves/src/ControllerPose.cpp b/LucidGloves/src/ControllerPose.cpp index 8d60f8b5..633ea604 100644 --- a/LucidGloves/src/ControllerPose.cpp +++ b/LucidGloves/src/ControllerPose.cpp @@ -2,43 +2,43 @@ #include "ControllerPose.h" ControllerPose::ControllerPose(vr::ETrackedControllerRole shadowDeviceOfRole, std::string thisDeviceManufacturer, VRDeviceConfiguration_t configuration, uint32_t driverId) : m_configuration(configuration), m_shadowDeviceOfRole(shadowDeviceOfRole), m_driverId(driverId), m_thisDeviceManufacturer(thisDeviceManufacturer) { - - m_pose.poseIsValid = true; - m_pose.result = vr::TrackingResult_Running_OK; - m_pose.deviceIsConnected = true; - m_pose.qWorldFromDriverRotation.w = 1; - m_pose.qDriverFromHeadRotation.w = 1; - //This will be configurable in settings - m_pose.poseTimeOffset = configuration.poseOffset; const vr::HmdVector3_t angleOffset = m_configuration.angleOffsetVector; m_offsetQuaternion = EulerToQuaternion(DegToRad(angleOffset.v[0]), DegToRad(angleOffset.v[1]), DegToRad(angleOffset.v[2])); + DebugDriverLog("Offset calculated! {%.2f, %.2f, %.2f, %.2f}", m_offsetQuaternion.w, m_offsetQuaternion.x, m_offsetQuaternion.y, m_offsetQuaternion.z); } vr::DriverPose_t ControllerPose::UpdatePose() { vr::DriverPose_t newPose = { 0 }; + newPose.qWorldFromDriverRotation.w = 1; + newPose.qDriverFromHeadRotation.w = 1; + if (m_shadowControllerId != -1) { vr::TrackedDevicePose_t trackedDevicePoses[vr::k_unMaxTrackedDeviceCount]; vr::VRServerDriverHost()->GetRawTrackedDevicePoses(0, trackedDevicePoses, vr::k_unMaxTrackedDeviceCount); if (trackedDevicePoses[m_shadowControllerId].bPoseIsValid) { - newPose.qWorldFromDriverRotation.w = 1; - newPose.qDriverFromHeadRotation.w = 1; - const vr::HmdMatrix34_t shadowControllerMatrix = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking; - - ////As we need to account for rotation for the offset, multiply - const vr::HmdVector3_t vectorOffset = MultiplyMatrix(Get33Matrix(shadowControllerMatrix), m_configuration.offsetVector); + //get the matrix that represents the position of the controller that we are shadowing + vr::HmdMatrix34_t controllerMatrix = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking; + + //get only the rotation (3x3 matrix), as the 3x4 matrix also includes position + vr::HmdMatrix33_t controllerRotationMatrix = GetRotationMatrix(controllerMatrix); + + //multiply the rotation matrix by the offset vector set that is the offset of the controller relative to the hand + vr::HmdVector3_t vectorOffset = MultiplyMatrix(controllerRotationMatrix, m_configuration.offsetVector); + + //combine these positions to get the resultant position + vr::HmdVector3_t newControllerPosition = CombinePosition(controllerMatrix, vectorOffset); - newPose.vecPosition[0] = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking.m[0][3] + vectorOffset.v[0]; - newPose.vecPosition[1] = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking.m[1][3] + vectorOffset.v[1]; - newPose.vecPosition[2] = trackedDevicePoses[m_shadowControllerId].mDeviceToAbsoluteTracking.m[2][3] + vectorOffset.v[2]; //- forward + newPose.vecPosition[0] = newControllerPosition.v[0]; + newPose.vecPosition[1] = newControllerPosition.v[1]; + newPose.vecPosition[2] = newControllerPosition.v[2]; - //vr::HmdQuaternion_t offset_quaternion = QuaternionFromAngle(1, 0, 0, DegToRad(-45)); - - //merge rotation - newPose.qRotation = MultiplyQuaternion(GetRotation(shadowControllerMatrix), m_offsetQuaternion); + //Multiply rotation quaternions together, as the controller may be rotated relative to the hand + newPose.qRotation = MultiplyQuaternion(GetRotation(controllerMatrix), m_offsetQuaternion); + //Copy other values from the controller that we want for this device newPose.vecAngularVelocity[0] = trackedDevicePoses[m_shadowControllerId].vAngularVelocity.v[0]; newPose.vecAngularVelocity[1] = trackedDevicePoses[m_shadowControllerId].vAngularVelocity.v[1]; newPose.vecAngularVelocity[2] = trackedDevicePoses[m_shadowControllerId].vAngularVelocity.v[2]; @@ -49,11 +49,11 @@ vr::DriverPose_t ControllerPose::UpdatePose() { newPose.poseIsValid = true; newPose.deviceIsConnected = true; + newPose.result = vr::TrackingResult_Running_OK; } else { - //DebugDriverLog("pose %d is not valid", m_shadowControllerId); newPose.poseIsValid = false; newPose.deviceIsConnected = true; newPose.result = vr::TrackingResult_Uninitialized; diff --git a/LucidGloves/src/DeviceFactory.cpp b/LucidGloves/src/DeviceFactory.cpp index 8d21c464..073a1d8a 100644 --- a/LucidGloves/src/DeviceFactory.cpp +++ b/LucidGloves/src/DeviceFactory.cpp @@ -20,7 +20,7 @@ This method returns an instance of your provider that OpenVR uses. HMD_DLL_EXPORT void* HmdDriverFactory(const char* interfaceName, int* returnCode) { - if (0 == strcmp(vr::IServerTrackedDeviceProvider_Version, interfaceName)) { + if (0 == strcmp(vr::IServerTrackedDeviceProvider_Version, interfaceName)) { return &deviceProvider; } DriverLog("HmdDriverFactory called for %s", interfaceName); diff --git a/LucidGloves/src/Quaternion.cpp b/LucidGloves/src/Quaternion.cpp index d247709a..156af553 100644 --- a/LucidGloves/src/Quaternion.cpp +++ b/LucidGloves/src/Quaternion.cpp @@ -7,7 +7,7 @@ double RadToDeg(double rad) { return rad * 180 / M_PI; } -static vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t matrix) { +vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t& matrix) { vr::HmdVector3_t vector; vector.v[0] = matrix.m[0][3]; @@ -17,7 +17,17 @@ static vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t matrix) { return vector; } -vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t matrix) { +vr::HmdVector3_t CombinePosition(vr::HmdMatrix34_t& matrix, vr::HmdVector3_t& vec) { + vr::HmdVector3_t vector; + + vector.v[0] = matrix.m[0][3] + vec.v[0]; + vector.v[1] = matrix.m[1][3] + vec.v[1]; + vector.v[2] = matrix.m[2][3] + vec.v[2]; + + return vector; +} + +vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t& matrix) { vr::HmdQuaternion_t q; q.w = sqrt(fmax(0, 1 + matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2])) / 2; @@ -32,7 +42,7 @@ vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t matrix) { return q; } -vr::HmdMatrix33_t Get33Matrix(const vr::HmdMatrix34_t matrix) { +vr::HmdMatrix33_t GetRotationMatrix(const vr::HmdMatrix34_t& matrix) { vr::HmdMatrix33_t result = { { {matrix.m[0][0], matrix.m[0][1], matrix.m[0][2]}, {matrix.m[1][0], matrix.m[1][1], matrix.m[1][2]}, @@ -41,7 +51,7 @@ vr::HmdMatrix33_t Get33Matrix(const vr::HmdMatrix34_t matrix) { return result; } -vr::HmdVector3_t MultiplyMatrix(const vr::HmdMatrix33_t matrix, const vr::HmdVector3_t vector) { +vr::HmdVector3_t MultiplyMatrix(const vr::HmdMatrix33_t& matrix, const vr::HmdVector3_t& vector) { vr::HmdVector3_t result; result.v[0] = matrix.m[0][0] * vector.v[0] + matrix.m[0][1] * vector.v[1] + matrix.m[0][2] * vector.v[2]; @@ -72,15 +82,14 @@ vr::HmdQuaternion_t QuaternionFromAngle(const double& xx, const double& yy, cons return quat; } -vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z) -{ +vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z) { vr::HmdQuaternion_t quat0 = QuaternionFromAngle(1, 0, 0, x); vr::HmdQuaternion_t quat1 = MultiplyQuaternion(quat0, QuaternionFromAngle(0, 1, 0, y)); vr::HmdQuaternion_t quat2 = MultiplyQuaternion(quat1, QuaternionFromAngle(0, 0, 1, z)); return quat2; } -vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t q, const vr::HmdQuaternion_t r) { +vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t& q, const vr::HmdQuaternion_t& r) { vr::HmdQuaternion_t result; result.w = (r.w * q.w - r.x * q.x - r.y * q.y - r.z * q.z); From b6832be7b84565dbf76802847b8afbe3f0a71a0a Mon Sep 17 00:00:00 2001 From: danwillm <39023874+danwillm@users.noreply.github.com> Date: Sun, 14 Mar 2021 09:23:58 +0000 Subject: [PATCH 2/2] added extra const --- LucidGloves/include/Comm/SerialCommunicationManager.h | 3 +-- LucidGloves/include/Quaternion.h | 4 ++-- LucidGloves/src/Quaternion.cpp | 4 ++-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/LucidGloves/include/Comm/SerialCommunicationManager.h b/LucidGloves/include/Comm/SerialCommunicationManager.h index eeac46df..1bfdfc0f 100644 --- a/LucidGloves/include/Comm/SerialCommunicationManager.h +++ b/LucidGloves/include/Comm/SerialCommunicationManager.h @@ -46,5 +46,4 @@ class SerialManager : public ICommunicationManager { std::thread m_serialThread; VRSerialConfiguration_t m_configuration; -}; - +}; \ No newline at end of file diff --git a/LucidGloves/include/Quaternion.h b/LucidGloves/include/Quaternion.h index 5abb0056..73460c0c 100644 --- a/LucidGloves/include/Quaternion.h +++ b/LucidGloves/include/Quaternion.h @@ -10,7 +10,7 @@ double RadToDeg(double rad); //get the quaternion for roation from a matrix vr::HmdQuaternion_t GetRotation(const vr::HmdMatrix34_t& matrix); vr::HmdVector3_t GetPosition(const vr::HmdMatrix34_t& matrix); -vr::HmdVector3_t CombinePosition(vr::HmdMatrix34_t& matrix, vr::HmdVector3_t& vec); +vr::HmdVector3_t CombinePosition(const vr::HmdMatrix34_t& matrix, const vr::HmdVector3_t& vec); //returns the result of multiplying two quaternions, effectively applying a roatation on a quaternion vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t& q, const vr::HmdQuaternion_t& r); @@ -18,4 +18,4 @@ vr::HmdQuaternion_t MultiplyQuaternion(const vr::HmdQuaternion_t& q, const vr::H vr::HmdQuaternion_t QuaternionFromAngle(const double& xx, const double& yy, const double& zz, const double& a); vr::HmdQuaternion_t EulerToQuaternion(const double& x, const double& y, const double& z); vr::HmdMatrix33_t GetRotationMatrix(const vr::HmdMatrix34_t& matrix); -vr::HmdVector3_t MultiplyMatrix(vr::HmdMatrix33_t& matrix, const vr::HmdVector3_t& vector); +vr::HmdVector3_t MultiplyMatrix(const vr::HmdMatrix33_t& matrix, const vr::HmdVector3_t& vector); diff --git a/LucidGloves/src/Quaternion.cpp b/LucidGloves/src/Quaternion.cpp index 156af553..d1b14b57 100644 --- a/LucidGloves/src/Quaternion.cpp +++ b/LucidGloves/src/Quaternion.cpp @@ -7,7 +7,7 @@ double RadToDeg(double rad) { return rad * 180 / M_PI; } -vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t& matrix) { +vr::HmdVector3_t GetPosition(const vr::HmdMatrix34_t& matrix) { vr::HmdVector3_t vector; vector.v[0] = matrix.m[0][3]; @@ -17,7 +17,7 @@ vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t& matrix) { return vector; } -vr::HmdVector3_t CombinePosition(vr::HmdMatrix34_t& matrix, vr::HmdVector3_t& vec) { +vr::HmdVector3_t CombinePosition(const vr::HmdMatrix34_t& matrix, const vr::HmdVector3_t& vec) { vr::HmdVector3_t vector; vector.v[0] = matrix.m[0][3] + vec.v[0];