From ffc3ec4083fe6af9bf8835494d9dc68d834d148b Mon Sep 17 00:00:00 2001 From: Martin Date: Sun, 19 Jul 2020 10:21:56 +0200 Subject: [PATCH] Split the code into different files --- ...tion_pose.cpp => MotionPoseController.cpp} | 239 ++++-------------- motionPose/MotionPoseController.h | 85 +++++++ motionPose/ServerDriver.cpp | 30 +++ motionPose/ServerDriver.h | 22 ++ motionPose/driver_motionPose.cpp | 18 ++ motionPose/driver_motionPose.h | 28 ++ motionPose/motionPose.vcxproj | 15 +- motionPose/motionPose.vcxproj.filters | 19 +- motionPose/openvr_math.h | 167 ++++++++---- 9 files changed, 370 insertions(+), 253 deletions(-) rename motionPose/{motion_pose.cpp => MotionPoseController.cpp} (52%) create mode 100644 motionPose/MotionPoseController.h create mode 100644 motionPose/ServerDriver.cpp create mode 100644 motionPose/ServerDriver.h create mode 100644 motionPose/driver_motionPose.cpp create mode 100644 motionPose/driver_motionPose.h diff --git a/motionPose/motion_pose.cpp b/motionPose/MotionPoseController.cpp similarity index 52% rename from motionPose/motion_pose.cpp rename to motionPose/MotionPoseController.cpp index e2ffed2..865c2d8 100644 --- a/motionPose/motion_pose.cpp +++ b/motionPose/MotionPoseController.cpp @@ -1,84 +1,10 @@ -#include -#include "openvr_math.h" +#include "MotionPoseController.h" #include "driverlog.h" -#include -#include -#include -#include - -#include -#include - -#if defined( _WINDOWS ) -#include -#endif - -#include -#include -#include "WatchdogProvider.h" - -using namespace vr; - - -#if defined(_WIN32) -#define HMD_DLL_EXPORT extern "C" __declspec( dllexport ) -#define HMD_DLL_IMPORT extern "C" __declspec( dllimport ) -#elif defined(__GNUC__) || defined(COMPILER_GCC) || defined(__APPLE__) -#define HMD_DLL_EXPORT extern "C" __attribute__((visibility("default"))) -#define HMD_DLL_IMPORT extern "C" -#else -#error "Unsupported Platform." -#endif - -inline HmdQuaternion_t HmdQuaternion_Init(double w, double x, double y, double z) +//class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver +namespace driver { - HmdQuaternion_t quat; - quat.w = w; - quat.x = x; - quat.y = y; - quat.z = z; - return quat; -} - -inline HmdVector3d_t HmdVector3d_t_Init(double x, double y, double z) -{ - HmdVector3d_t vec; - vec.v[0] = x; - vec.v[1] = y; - vec.v[2] = z; - return vec; -} - - -//----------------------------------------------------------------------------- -// Purpose: -//----------------------------------------------------------------------------- -class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver -{ - - double rigX; - double rigY; - double rigZ; - double rigPitch; - double rigRoll; - double rigYaw; - double rigHeave; - double rigSurge; - double rigSway; - double yawComp; - - char* mmfFile; - DriverPose_t pose; - int hmdDeviceId; - bool hmdValid = false; - bool moverConnected = false; - TrackedDevicePose_t poses[10]; - HANDLE hMapFile = NULL; - HmdVector3d_t rigPos; - -public: - CMotionPoseControllerDriver() + CMotionPoseControllerDriver::CMotionPoseControllerDriver() { m_unObjectId = vr::k_unTrackedDeviceIndexInvalid; m_ulPropertyContainer = vr::k_ulInvalidPropertyContainer; @@ -109,33 +35,33 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver rigPos = { 0 }; } - virtual ~CMotionPoseControllerDriver() + CMotionPoseControllerDriver::~CMotionPoseControllerDriver() { } -#define BUF_SIZE 256 + #define BUF_SIZE 256 - - virtual EVRInitError Activate(vr::TrackedDeviceIndex_t unObjectId) + vr::EVRInitError CMotionPoseControllerDriver::Activate(vr::TrackedDeviceIndex_t unObjectId) { DriverLog("Activate motionPoseController\n"); m_unObjectId = unObjectId; m_ulPropertyContainer = vr::VRProperties()->TrackedDeviceToPropertyContainer(m_unObjectId); - vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, Prop_ModelNumber_String, m_sModelNumber.c_str()); - vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, Prop_RenderModelName_String, "locator"); + vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, vr::Prop_ModelNumber_String, m_sModelNumber.c_str()); + vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, vr::Prop_RenderModelName_String, "locator"); // return a constant that's not 0 (invalid) or 1 (reserved for Oculus) - vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, Prop_CurrentUniverseId_Uint64, 2); + vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, 2); - vr::VRProperties()->SetInt32Property(m_ulPropertyContainer, Prop_ControllerRoleHint_Int32, TrackedControllerRole_OptOut); - vr::VRProperties()->SetInt32Property(m_ulPropertyContainer, Prop_DeviceClass_Int32, TrackedDeviceClass_GenericTracker); + vr::VRProperties()->SetInt32Property(m_ulPropertyContainer, vr::Prop_ControllerRoleHint_Int32, vr::TrackedControllerRole_OptOut); + vr::VRProperties()->SetInt32Property(m_ulPropertyContainer, vr::Prop_DeviceClass_Int32, vr::TrackedDeviceClass_GenericTracker); - return VRInitError_None; + return vr::VRInitError_None; } - void initalizeMoverMmf() { + void CMotionPoseControllerDriver::initalizeMoverMmf() + { moverConnected = false; TCHAR szName[] = TEXT("Local\\motionRigPose"); @@ -148,50 +74,53 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver { // DriverLog("could not open mmf motionRigPose\n"); } - else { + else + { // DriverLog("successfully opened mmf motionRigPose\n"); - mmfFile = (char*)MapViewOfFile(hMapFile, // handle to map object - FILE_MAP_ALL_ACCESS, // read/write permission - 0, - 0, - BUF_SIZE); - if (strncmp(mmfFile, "posedata", 8) == 0) { + mmfFile = (char*)MapViewOfFile(hMapFile, // handle to map object + FILE_MAP_ALL_ACCESS, // read/write permission + 0, + 0, + BUF_SIZE); + if (strncmp(mmfFile, "posedata", 8) == 0) + { DriverLog("successfully receiving posedata\n"); moverConnected = true; } - else { -// DriverLog("no valid posedata\n"); + else + { + // DriverLog("no valid posedata\n"); } } } - virtual void Deactivate() + void CMotionPoseControllerDriver::Deactivate() { m_unObjectId = vr::k_unTrackedDeviceIndexInvalid; } - virtual void EnterStandby() + void CMotionPoseControllerDriver::EnterStandby() { } - void* GetComponent(const char* pchComponentNameAndVersion) + void* CMotionPoseControllerDriver::GetComponent(const char* pchComponentNameAndVersion) { // override this to add a component to a driver return NULL; } - virtual void PowerOff() + void CMotionPoseControllerDriver::PowerOff() { } /** debug request from a client */ - virtual void DebugRequest(const char* pchRequest, char* pchResponseBuffer, uint32_t unResponseBufferSize) + void CMotionPoseControllerDriver::DebugRequest(const char* pchRequest, char* pchResponseBuffer, uint32_t unResponseBufferSize) { if (unResponseBufferSize >= 1) pchResponseBuffer[0] = 0; } - virtual DriverPose_t GetPose() + vr::DriverPose_t CMotionPoseControllerDriver::GetPose() { // DriverLog("virtualPoseController::GetPose\n"); @@ -200,7 +129,7 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver pose.shouldApplyHeadModel = false; pose.willDriftInYaw = false; - HmdQuaternion_t yaw = vrmath::quaternionFromRotationY(yawComp); + vr::HmdQuaternion_t yaw = vrmath::quaternionFromRotationY(yawComp); // Turn Right/Left if ((GetAsyncKeyState('E') & 0x8000) != 0) yawComp += 0.005; @@ -219,13 +148,15 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver if ((GetAsyncKeyState('X') & 0x8000) != 0) rigPos = rigPos + vrmath::quaternionRotateVector(yaw, HmdVector3d_t_Init(0, 0, 0.003)); // reset - if ((GetAsyncKeyState('P') & 0x8000) != 0) { + if ((GetAsyncKeyState('P') & 0x8000) != 0) + { rigPos = { 0 }; yawComp = 0; } - if (moverConnected && strncmp(mmfFile, "posedata", 8) == 0) { - pose.result = TrackingResult_Running_OK; + if (moverConnected && strncmp(mmfFile, "posedata", 8) == 0) + { + pose.result = vr::TrackingResult_Running_OK; // double* poseData = (double*)mmfFile; rigSway = poseData[1]; @@ -258,8 +189,9 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver return pose; } - else { - pose.result = TrackingResult_Calibrating_InProgress; + else + { + pose.result = vr::TrackingResult_Calibrating_InProgress; pose.deviceIsConnected = false; initalizeMoverMmf(); @@ -271,11 +203,7 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver } - - - - - void RunFrame() + void CMotionPoseControllerDriver::RunFrame() { // Your driver would read whatever hardware state is associated with its input components and pass that // in to UpdateBooleanComponent. This could happen in RunFrame or on a thread of your own that's reading USB @@ -289,86 +217,11 @@ class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver if (event.trackedDeviceIndex == this->m_unObjectId && event.eventType == EVREventType::VREvent_PropertyChanged) { if (event.data.property.prop == Prop_RenderModelName_String) { std::string renderModel = vr::VRProperties()->GetStringProperty(event.data.property.container, Prop_RenderModelName_String); - vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, Prop_RenderModelName_String, renderModel.data()); + vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, Prop_RenderModelName_String, renderModel.data()); } } }*/ - vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_unObjectId, GetPose(), sizeof(DriverPose_t)); - } - - std::string GetSerialNumber() const { return m_sSerialNumber; } - -private: - vr::TrackedDeviceIndex_t m_unObjectId; - vr::PropertyContainerHandle_t m_ulPropertyContainer; - - std::string m_sSerialNumber; - std::string m_sModelNumber; - - -}; - -class CServerDriver_MotionPose : public IServerTrackedDeviceProvider -{ -public: - virtual EVRInitError Init(vr::IVRDriverContext* pDriverContext); - virtual void Cleanup(); - virtual const char* const* GetInterfaceVersions() { return vr::k_InterfaceVersions; } - virtual void RunFrame(); - virtual bool ShouldBlockStandbyMode() { return false; } - virtual void EnterStandby() {} - virtual void LeaveStandby() {} - -private: - CMotionPoseControllerDriver* m_pController = nullptr; -}; - -CServerDriver_MotionPose g_motionPoseDriver; - - -EVRInitError CServerDriver_MotionPose::Init(vr::IVRDriverContext* pDriverContext) -{ - VR_INIT_SERVER_DRIVER_CONTEXT(pDriverContext); - InitDriverLog(vr::VRDriverLog()); - - m_pController = new CMotionPoseControllerDriver(); - vr::VRServerDriverHost()->TrackedDeviceAdded(m_pController->GetSerialNumber().c_str(), vr::TrackedDeviceClass_Controller, m_pController); - - return VRInitError_None; -} - -void CServerDriver_MotionPose::Cleanup() -{ - CleanupDriverLog(); - delete m_pController; - m_pController = NULL; -} - - -void CServerDriver_MotionPose::RunFrame() -{ - if (m_pController) - { - m_pController->RunFrame(); - } -} - -vrmotioncompensation::driver::WatchdogProvider watchdogProvider; - -HMD_DLL_EXPORT void* HmdDriverFactory(const char* pInterfaceName, int* pReturnCode) -{ - if (0 == strcmp(IServerTrackedDeviceProvider_Version, pInterfaceName)) - { - return &g_motionPoseDriver; - } - else if (std::strcmp(IVRWatchdogProvider_Version, pInterfaceName) == 0) - { - return &watchdogProvider; + vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_unObjectId, GetPose(), sizeof(vr::DriverPose_t)); } - - if (pReturnCode) - *pReturnCode = VRInitError_Init_InterfaceNotFound; - - return NULL; -} +}; \ No newline at end of file diff --git a/motionPose/MotionPoseController.h b/motionPose/MotionPoseController.h new file mode 100644 index 0000000..b82cde3 --- /dev/null +++ b/motionPose/MotionPoseController.h @@ -0,0 +1,85 @@ +#pragma once + +#include +#include "openvr_driver.h" +#include "openvr_math.h" + +namespace driver +{ + class CMotionPoseControllerDriver : public vr::ITrackedDeviceServerDriver + { + #define BUF_SIZE 256 + + double rigX; + double rigY; + double rigZ; + double rigPitch; + double rigRoll; + double rigYaw; + double rigHeave; + double rigSurge; + double rigSway; + double yawComp; + + char* mmfFile; + vr::DriverPose_t pose; + int hmdDeviceId; + bool hmdValid = false; + bool moverConnected = false; + vr::TrackedDevicePose_t poses[10]; + HANDLE hMapFile = NULL; + vr::HmdVector3d_t rigPos; + + public: + CMotionPoseControllerDriver(); + + virtual ~CMotionPoseControllerDriver(); + + inline vr::HmdQuaternion_t HmdQuaternion_Init(double w, double x, double y, double z) + { + vr::HmdQuaternion_t quat; + quat.w = w; + quat.x = x; + quat.y = y; + quat.z = z; + return quat; + } + + inline vr::HmdVector3d_t HmdVector3d_t_Init(double x, double y, double z) + { + vr::HmdVector3d_t vec; + vec.v[0] = x; + vec.v[1] = y; + vec.v[2] = z; + return vec; + } + + virtual vr::EVRInitError Activate(vr::TrackedDeviceIndex_t unObjectId); + + void initalizeMoverMmf(); + + virtual void Deactivate(); + + virtual void EnterStandby(); + + void* GetComponent(const char* pchComponentNameAndVersion); + + virtual void PowerOff(); + + /** debug request from a client */ + virtual void DebugRequest(const char* pchRequest, char* pchResponseBuffer, uint32_t unResponseBufferSize); + + virtual vr::DriverPose_t GetPose(); + + void RunFrame(); + + std::string GetSerialNumber() const { return m_sSerialNumber; } + + private: + vr::TrackedDeviceIndex_t m_unObjectId; + vr::PropertyContainerHandle_t m_ulPropertyContainer; + + std::string m_sSerialNumber; + std::string m_sModelNumber; + }; +} \ No newline at end of file diff --git a/motionPose/ServerDriver.cpp b/motionPose/ServerDriver.cpp new file mode 100644 index 0000000..89a205f --- /dev/null +++ b/motionPose/ServerDriver.cpp @@ -0,0 +1,30 @@ +#include "ServerDriver.h" + +//----------------------------------------------------------------------------- +// Purpose: +//----------------------------------------------------------------------------- +vr::EVRInitError CServerDriver_MotionPose::Init(vr::IVRDriverContext* pDriverContext) +{ + VR_INIT_SERVER_DRIVER_CONTEXT(pDriverContext); + InitDriverLog(vr::VRDriverLog()); + + m_pController = new driver::CMotionPoseControllerDriver(); + vr::VRServerDriverHost()->TrackedDeviceAdded(m_pController->GetSerialNumber().c_str(), vr::TrackedDeviceClass_Controller, m_pController); + + return vr::VRInitError_None; +} + +void CServerDriver_MotionPose::Cleanup() +{ + CleanupDriverLog(); + delete m_pController; + m_pController = NULL; +} + +void CServerDriver_MotionPose::RunFrame() +{ + if (m_pController) + { + m_pController->RunFrame(); + } +} \ No newline at end of file diff --git a/motionPose/ServerDriver.h b/motionPose/ServerDriver.h new file mode 100644 index 0000000..de7e174 --- /dev/null +++ b/motionPose/ServerDriver.h @@ -0,0 +1,22 @@ +#pragma once + +#include "openvr_driver.h" +//#include "WatchdogProvider.h" +#include "openvr_math.h" +#include "driverlog.h" +#include "MotionPoseController.h" + +class CServerDriver_MotionPose : public vr::IServerTrackedDeviceProvider +{ +public: + virtual vr::EVRInitError Init(vr::IVRDriverContext* pDriverContext); + virtual void Cleanup(); + virtual const char* const* GetInterfaceVersions() { return vr::k_InterfaceVersions; } + virtual void RunFrame(); + virtual bool ShouldBlockStandbyMode() { return false; } + virtual void EnterStandby() {} + virtual void LeaveStandby() {} + +private: + driver::CMotionPoseControllerDriver* m_pController = nullptr; +}; \ No newline at end of file diff --git a/motionPose/driver_motionPose.cpp b/motionPose/driver_motionPose.cpp new file mode 100644 index 0000000..81ebe9b --- /dev/null +++ b/motionPose/driver_motionPose.cpp @@ -0,0 +1,18 @@ +#include "driver_motionPose.h" + +HMD_DLL_EXPORT void* HmdDriverFactory(const char* pInterfaceName, int* pReturnCode) +{ + if (0 == strcmp(vr::IServerTrackedDeviceProvider_Version, pInterfaceName)) + { + return &g_motionPoseDriver; + } + else if (std::strcmp(vr::IVRWatchdogProvider_Version, pInterfaceName) == 0) + { + return &watchdogProvider; + } + + if (pReturnCode) + *pReturnCode = vr::VRInitError_Init_InterfaceNotFound; + + return NULL; +} \ No newline at end of file diff --git a/motionPose/driver_motionPose.h b/motionPose/driver_motionPose.h new file mode 100644 index 0000000..3c952c6 --- /dev/null +++ b/motionPose/driver_motionPose.h @@ -0,0 +1,28 @@ +#pragma once + +/* + We wont be using any cryptography, DDE, RPC, etc.. so exclude these from the build process, not necessary but speeds up the build a bit +*/ +#define WIN32_LEAN_AND_MEAN +#include +#include "ServerDriver.h" +#include "WatchdogProvider.h" + +/* + Obviously we need to include the openvr_driver.h file so we can do the stuff +*/ +#include +#define HMD_DLL_EXPORT extern "C" __declspec(dllexport) +using namespace vr; + +/* + Other helpful includes +*/ +#include +#include +#include + +// Our driver factory function +HMD_DLL_EXPORT void* HmdDriverFactory(const char* pInterfaceName, int* pReturnCode); +CServerDriver_MotionPose g_motionPoseDriver; +vrmotioncompensation::driver::WatchdogProvider watchdogProvider; \ No newline at end of file diff --git a/motionPose/motionPose.vcxproj b/motionPose/motionPose.vcxproj index d1965c3..8e1d51a 100644 --- a/motionPose/motionPose.vcxproj +++ b/motionPose/motionPose.vcxproj @@ -19,12 +19,17 @@ + - + + + + + @@ -93,6 +98,7 @@ false $(SolutionDir)\bin\drivers\motionPose\bin\win64\ + openvr\headers;$(IncludePath) @@ -151,8 +157,10 @@ true NDEBUG;MOTIONPOSE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) true - Use - pch.h + NotUsing + + + openvr\headers;%(AdditionalIncludeDirectories) Windows @@ -160,6 +168,7 @@ true true false + %(AdditionalDependencies) diff --git a/motionPose/motionPose.vcxproj.filters b/motionPose/motionPose.vcxproj.filters index d74f870..87e905f 100644 --- a/motionPose/motionPose.vcxproj.filters +++ b/motionPose/motionPose.vcxproj.filters @@ -18,10 +18,16 @@ Source Files - + Source Files - + + Source Files + + + Source Files + + Source Files @@ -35,5 +41,14 @@ Source Files + + Source Files + + + Source Files + + + Source Files + \ No newline at end of file diff --git a/motionPose/openvr_math.h b/motionPose/openvr_math.h index 2f8dd0e..992ccbb 100644 --- a/motionPose/openvr_math.h +++ b/motionPose/openvr_math.h @@ -1,7 +1,10 @@ +#pragma once + #include -inline vr::HmdQuaternion_t operator+(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) { +inline vr::HmdQuaternion_t operator+(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) +{ return { lhs.w + rhs.w, lhs.x + rhs.x, @@ -11,7 +14,8 @@ inline vr::HmdQuaternion_t operator+(const vr::HmdQuaternion_t& lhs, const vr::H } -inline vr::HmdQuaternion_t operator-(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) { +inline vr::HmdQuaternion_t operator-(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) +{ return{ lhs.w - rhs.w, lhs.x - rhs.x, @@ -21,7 +25,8 @@ inline vr::HmdQuaternion_t operator-(const vr::HmdQuaternion_t& lhs, const vr::H } -inline vr::HmdQuaternion_t operator*(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) { +inline vr::HmdQuaternion_t operator*(const vr::HmdQuaternion_t& lhs, const vr::HmdQuaternion_t& rhs) +{ return { (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), @@ -31,7 +36,8 @@ inline vr::HmdQuaternion_t operator*(const vr::HmdQuaternion_t& lhs, const vr::H } -inline vr::HmdVector3d_t operator+(const vr::HmdVector3d_t& lhs, const vr::HmdVector3d_t& rhs) { +inline vr::HmdVector3d_t operator+(const vr::HmdVector3d_t& lhs, const vr::HmdVector3d_t& rhs) +{ return { lhs.v[0] + rhs.v[0], lhs.v[1] + rhs.v[1], @@ -39,7 +45,8 @@ inline vr::HmdVector3d_t operator+(const vr::HmdVector3d_t& lhs, const vr::HmdVe }; } -inline vr::HmdVector3d_t operator+(const vr::HmdVector3d_t& lhs, const double(&rhs)[3]) { +inline vr::HmdVector3d_t operator+(const vr::HmdVector3d_t& lhs, const double(&rhs)[3]) +{ return{ lhs.v[0] + rhs[0], lhs.v[1] + rhs[1], @@ -47,7 +54,8 @@ inline vr::HmdVector3d_t operator+(const vr::HmdVector3d_t& lhs, const double(&r }; } -inline vr::HmdVector3d_t operator-(const vr::HmdVector3d_t& lhs, const vr::HmdVector3d_t& rhs) { +inline vr::HmdVector3d_t operator-(const vr::HmdVector3d_t& lhs, const vr::HmdVector3d_t& rhs) +{ return{ lhs.v[0] - rhs.v[0], lhs.v[1] - rhs.v[1], @@ -55,7 +63,8 @@ inline vr::HmdVector3d_t operator-(const vr::HmdVector3d_t& lhs, const vr::HmdVe }; } -inline vr::HmdVector3d_t operator-(const vr::HmdVector3d_t& lhs, const double(&rhs)[3]) { +inline vr::HmdVector3d_t operator-(const vr::HmdVector3d_t& lhs, const double(&rhs)[3]) +{ return{ lhs.v[0] - rhs[0], lhs.v[1] - rhs[1], @@ -64,7 +73,8 @@ inline vr::HmdVector3d_t operator-(const vr::HmdVector3d_t& lhs, const double(&r } -inline vr::HmdVector3d_t operator*(const vr::HmdVector3d_t& lhs, const double rhs) { +inline vr::HmdVector3d_t operator*(const vr::HmdVector3d_t& lhs, const double rhs) +{ return{ lhs.v[0] * rhs, lhs.v[1] * rhs, @@ -73,7 +83,8 @@ inline vr::HmdVector3d_t operator*(const vr::HmdVector3d_t& lhs, const double rh } -inline vr::HmdVector3d_t operator/(const vr::HmdVector3d_t& lhs, const double rhs) { +inline vr::HmdVector3d_t operator/(const vr::HmdVector3d_t& lhs, const double rhs) +{ return{ lhs.v[0] / rhs, lhs.v[1] / rhs, @@ -82,13 +93,16 @@ inline vr::HmdVector3d_t operator/(const vr::HmdVector3d_t& lhs, const double rh } -namespace vrmath { +namespace vrmath +{ - template int signum(T v) { + template int signum(T v) + { return (v > (T)0) ? 1 : ((v < (T)0) ? -1 : 0); } - inline vr::HmdQuaternion_t quaternionFromRotationAxis(double rot, double ux, double uy, double uz) { + inline vr::HmdQuaternion_t quaternionFromRotationAxis(double rot, double ux, double uy, double uz) + { auto ha = rot / 2; return{ std::cos(ha), @@ -98,7 +112,8 @@ namespace vrmath { }; } - inline vr::HmdQuaternion_t quaternionFromRotationX(double rot) { + inline vr::HmdQuaternion_t quaternionFromRotationX(double rot) + { auto ha = rot / 2; return{ std::cos(ha), @@ -108,7 +123,8 @@ namespace vrmath { }; } - inline vr::HmdQuaternion_t quaternionFromRotationY(double rot) { + inline vr::HmdQuaternion_t quaternionFromRotationY(double rot) + { auto ha = rot / 2; return{ std::cos(ha), @@ -118,7 +134,8 @@ namespace vrmath { }; } - inline vr::HmdQuaternion_t quaternionFromRotationZ(double rot) { + inline vr::HmdQuaternion_t quaternionFromRotationZ(double rot) + { auto ha = rot / 2; return{ std::cos(ha), @@ -128,41 +145,49 @@ namespace vrmath { }; } - inline vr::HmdQuaternion_t quaternionFromYawPitchRoll(double yaw, double pitch, double roll) { + inline vr::HmdQuaternion_t quaternionFromYawPitchRoll(double yaw, double pitch, double roll) + { return quaternionFromRotationY(yaw) * quaternionFromRotationX(pitch) * quaternionFromRotationZ(roll); } - inline vr::HmdQuaternion_t quaternionFromYawRollPitch(double yaw, double roll, double pitch) { + inline vr::HmdQuaternion_t quaternionFromYawRollPitch(double yaw, double roll, double pitch) + { return quaternionFromRotationY(yaw) * quaternionFromRotationZ(roll) * quaternionFromRotationX(pitch); } - inline vr::HmdQuaternion_t quaternionFromRotationMatrix(const vr::HmdMatrix34_t& mat) { + inline vr::HmdQuaternion_t quaternionFromRotationMatrix(const vr::HmdMatrix34_t& mat) + { auto a = mat.m; vr::HmdQuaternion_t q; double trace = a[0][0] + a[1][1] + a[2][2]; - if (trace > 0) { + if (trace > 0) + { double s = 0.5 / sqrt(trace + 1.0); q.w = 0.25 / s; q.x = (a[1][2] - a[2][1]) * s; q.y = (a[2][0] - a[0][2]) * s; q.z = (a[0][1] - a[1][0]) * s; } - else { - if (a[0][0] > a[1][1] && a[0][0] > a[2][2]) { + else + { + if (a[0][0] > a[1][1] && a[0][0] > a[2][2]) + { double s = 2.0 * sqrt(1.0 + a[0][0] - a[1][1] - a[2][2]); q.w = (a[1][2] - a[2][1]) / s; q.x = 0.25 * s; q.y = (a[1][0] + a[0][1]) / s; q.z = (a[2][0] + a[0][2]) / s; } - else if (a[1][1] > a[2][2]) { + else if (a[1][1] > a[2][2]) + { double s = 2.0 * sqrt(1.0 + a[1][1] - a[0][0] - a[2][2]); q.w = (a[2][0] - a[0][2]) / s; q.x = (a[1][0] + a[0][1]) / s; q.y = 0.25 * s; q.z = (a[2][1] + a[1][2]) / s; } - else { + else + { double s = 2.0 * sqrt(1.0 + a[2][2] - a[0][0] - a[1][1]); q.w = (a[0][1] - a[1][0]) / s; q.x = (a[2][0] + a[0][2]) / s; @@ -176,7 +201,8 @@ namespace vrmath { return q; } - inline vr::HmdQuaternion_t quaternionConjugate(const vr::HmdQuaternion_t& quat) { + inline vr::HmdQuaternion_t quaternionConjugate(const vr::HmdQuaternion_t& quat) + { return { quat.w, -quat.x, @@ -185,64 +211,80 @@ namespace vrmath { }; } - inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const vr::HmdVector3d_t& vector, bool reverse = false) { - if (reverse) { + inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const vr::HmdVector3d_t& vector, bool reverse = false) + { + if (reverse) + { vr::HmdQuaternion_t pin = { 0.0, vector.v[0], vector.v[1] , vector.v[2] }; auto pout = vrmath::quaternionConjugate(quat) * pin * quat; return { pout.x, pout.y, pout.z }; } - else { + else + { vr::HmdQuaternion_t pin = { 0.0, vector.v[0], vector.v[1] , vector.v[2] }; auto pout = quat * pin * vrmath::quaternionConjugate(quat); return { pout.x, pout.y, pout.z }; } } - inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const vr::HmdQuaternion_t& quatInv, const vr::HmdVector3d_t& vector, bool reverse = false) { - if (reverse) { + inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const vr::HmdQuaternion_t& quatInv, const vr::HmdVector3d_t& vector, bool reverse = false) + { + if (reverse) + { vr::HmdQuaternion_t pin = { 0.0, vector.v[0], vector.v[1] , vector.v[2] }; auto pout = quatInv * pin * quat; return{ pout.x, pout.y, pout.z }; } - else { + else + { vr::HmdQuaternion_t pin = { 0.0, vector.v[0], vector.v[1] , vector.v[2] }; auto pout = quat * pin * quatInv; return{ pout.x, pout.y, pout.z }; } } - inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const double(&vector)[3], bool reverse = false) { - if (reverse) { + inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const double(&vector)[3], bool reverse = false) + { + if (reverse) + { vr::HmdQuaternion_t pin = { 0.0, vector[0], vector[1] , vector[2] }; auto pout = vrmath::quaternionConjugate(quat) * pin * quat; return{ pout.x, pout.y, pout.z }; } - else { + else + { vr::HmdQuaternion_t pin = { 0.0, vector[0], vector[1] , vector[2] }; auto pout = quat * pin * vrmath::quaternionConjugate(quat); return{ pout.x, pout.y, pout.z }; } } - inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const vr::HmdQuaternion_t& quatInv, const double(&vector)[3], bool reverse = false) { - if (reverse) { + inline vr::HmdVector3d_t quaternionRotateVector(const vr::HmdQuaternion_t& quat, const vr::HmdQuaternion_t& quatInv, const double(&vector)[3], bool reverse = false) + { + if (reverse) + { vr::HmdQuaternion_t pin = { 0.0, vector[0], vector[1] , vector[2] }; auto pout = quatInv * pin * quat; return{ pout.x, pout.y, pout.z }; } - else { + else + { vr::HmdQuaternion_t pin = { 0.0, vector[0], vector[1] , vector[2] }; auto pout = quat * pin * quatInv; return{ pout.x, pout.y, pout.z }; } } - inline vr::HmdMatrix34_t matMul33(const vr::HmdMatrix34_t& a, const vr::HmdMatrix34_t& b) { + inline vr::HmdMatrix34_t matMul33(const vr::HmdMatrix34_t& a, const vr::HmdMatrix34_t& b) + { vr::HmdMatrix34_t result; - for (unsigned i = 0; i < 3; i++) { - for (unsigned j = 0; j < 3; j++) { + for (unsigned i = 0; i < 3; i++) + { + for (unsigned j = 0; j < 3; j++) + { result.m[i][j] = 0.0f; - for (unsigned k = 0; k < 3; k++) { + for (unsigned k = 0; k < 3; k++) + { result.m[i][j] += a.m[i][k] * b.m[k][j]; } } @@ -250,54 +292,69 @@ namespace vrmath { return result; } - inline vr::HmdVector3_t matMul33(const vr::HmdMatrix34_t& a, const vr::HmdVector3_t& b) { + inline vr::HmdVector3_t matMul33(const vr::HmdMatrix34_t& a, const vr::HmdVector3_t& b) + { vr::HmdVector3_t result; - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < 3; i++) + { result.v[i] = 0.0f; - for (unsigned k = 0; k < 3; k++) { + for (unsigned k = 0; k < 3; k++) + { result.v[i] += a.m[i][k] * b.v[k]; }; } return result; } - inline vr::HmdVector3d_t matMul33(const vr::HmdMatrix34_t& a, const vr::HmdVector3d_t& b) { + inline vr::HmdVector3d_t matMul33(const vr::HmdMatrix34_t& a, const vr::HmdVector3d_t& b) + { vr::HmdVector3d_t result; - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < 3; i++) + { result.v[i] = 0.0f; - for (unsigned k = 0; k < 3; k++) { + for (unsigned k = 0; k < 3; k++) + { result.v[i] += a.m[i][k] * b.v[k]; }; } return result; } - inline vr::HmdVector3_t matMul33(const vr::HmdVector3_t& a, const vr::HmdMatrix34_t& b) { + inline vr::HmdVector3_t matMul33(const vr::HmdVector3_t& a, const vr::HmdMatrix34_t& b) + { vr::HmdVector3_t result; - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < 3; i++) + { result.v[i] = 0.0f; - for (unsigned k = 0; k < 3; k++) { + for (unsigned k = 0; k < 3; k++) + { result.v[i] += a.v[k] * b.m[k][i]; }; } return result; } - inline vr::HmdVector3d_t matMul33(const vr::HmdVector3d_t& a, const vr::HmdMatrix34_t& b) { + inline vr::HmdVector3d_t matMul33(const vr::HmdVector3d_t& a, const vr::HmdMatrix34_t& b) + { vr::HmdVector3d_t result; - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < 3; i++) + { result.v[i] = 0.0f; - for (unsigned k = 0; k < 3; k++) { + for (unsigned k = 0; k < 3; k++) + { result.v[i] += a.v[k] * b.m[k][i]; }; } return result; } - inline vr::HmdMatrix34_t transposeMul33(const vr::HmdMatrix34_t& a) { + inline vr::HmdMatrix34_t transposeMul33(const vr::HmdMatrix34_t& a) + { vr::HmdMatrix34_t result; - for (unsigned i = 0; i < 3; i++) { - for (unsigned k = 0; k < 3; k++) { + for (unsigned i = 0; i < 3; i++) + { + for (unsigned k = 0; k < 3; k++) + { result.m[i][k] = a.m[k][i]; } }