From 5832eea951c7419b16ffa6ffa61e7a352bc985c5 Mon Sep 17 00:00:00 2001 From: Kung-vr Date: Sat, 2 Feb 2019 22:36:19 +0900 Subject: [PATCH] Create quaternion.h to centralize quaternion math --- advancedSettings.pro | 3 +- .../MoveCenterTabController.cpp | 31 +++------- src/utils/Matrix.h | 46 -------------- src/utils/quaternion.h | 62 +++++++++++++++++++ 4 files changed, 73 insertions(+), 69 deletions(-) create mode 100644 src/utils/quaternion.h diff --git a/advancedSettings.pro b/advancedSettings.pro index a9485ea..fd7a986 100644 --- a/advancedSettings.pro +++ b/advancedSettings.pro @@ -95,7 +95,8 @@ HEADERS += src/overlaycontroller.h \ src/ivrinput/ivrinput_action.h \ src/ivrinput/ivrinput_manifest.h \ src/ivrinput/ivrinput_action_set.h \ - src/ivrinput/ivrinput.h + src/ivrinput/ivrinput.h \ + src/utils/quaternion.h win32 { SOURCES += src/tabcontrollers/audiomanager/AudioManagerWindows.cpp \ diff --git a/src/tabcontrollers/MoveCenterTabController.cpp b/src/tabcontrollers/MoveCenterTabController.cpp index 0edecc8..8f30822 100644 --- a/src/tabcontrollers/MoveCenterTabController.cpp +++ b/src/tabcontrollers/MoveCenterTabController.cpp @@ -2,6 +2,7 @@ #include #include "../overlaycontroller.h" #include "../utils/Matrix.h" +#include "../utils/quaternion.h" void rotateCoordinates( double coordinates[3], double angle ) { @@ -895,7 +896,7 @@ void MoveCenterTabController::eventLoopTick( utils::matMul33( hmdMatrixAbsolute, hmdMatrixRotMat, hmdMatrix ); // Convert pose matrix to quaternion - m_hmdQuaternion = utils::getQuaternion( hmdMatrixAbsolute ); + m_hmdQuaternion = utils::quaternionFromHmdMatrix34( hmdMatrixAbsolute ); // Get rotation change of hmd // Checking for invalid quaternion using < because == isn't guaranteed @@ -909,19 +910,12 @@ void MoveCenterTabController::eventLoopTick( { // Construct a quaternion representing difference between old // hmd pose and new hmd pose. - vr::HmdQuaternion_t hmdDiffQuaternion = utils::multiplyQuaternion( + vr::HmdQuaternion_t hmdDiffQuaternion = utils::quaternionMultiply( m_hmdQuaternion, utils::quaternionConjugate( m_lastHmdQuaternion ) ); // Calculate yaw from quaternion. - double hmdYawDiff = atan2( - 2.0 - * ( hmdDiffQuaternion.y * hmdDiffQuaternion.w - + hmdDiffQuaternion.x * hmdDiffQuaternion.z ), - -1.0 - + 2.0 - * ( hmdDiffQuaternion.w * hmdDiffQuaternion.w - + hmdDiffQuaternion.x * hmdDiffQuaternion.x ) ); + double hmdYawDiff = utils::quaternionGetYaw( hmdDiffQuaternion ); // Apply yaw difference to m_hmdYawTotal. m_hmdYawTotal += hmdYawDiff; @@ -1076,7 +1070,8 @@ void MoveCenterTabController::eventLoopTick( utils::matMul33( handMatrixAbsolute, handMatrixRotMat, handMatrix ); // Convert pose matrix to quaternion - m_handQuaternion = utils::getQuaternion( handMatrixAbsolute ); + m_handQuaternion + = utils::quaternionFromHmdMatrix34( handMatrixAbsolute ); if ( m_lastRotateHand == m_activeTurnHand ) { @@ -1094,22 +1089,14 @@ void MoveCenterTabController::eventLoopTick( // Construct a quaternion representing difference // between old hand and new hand. vr::HmdQuaternion_t handDiffQuaternion - = utils::multiplyQuaternion( + = utils::quaternionMultiply( m_handQuaternion, utils::quaternionConjugate( m_lastHandQuaternion ) ); // Calculate yaw from quaternion. - double handYawDiff = atan2( - 2.0 - * ( handDiffQuaternion.y * handDiffQuaternion.w - + handDiffQuaternion.x * handDiffQuaternion.z ), - -1.0 - + 2.0 - * ( handDiffQuaternion.w - * handDiffQuaternion.w - + handDiffQuaternion.x - * handDiffQuaternion.x ) ); + double handYawDiff + = utils::quaternionGetYaw( handDiffQuaternion ); int newRotationAngleDeg = static_cast( round( handYawDiff * k_radiansToCentidegrees ) diff --git a/src/utils/Matrix.h b/src/utils/Matrix.h index 8cba324..0eed8d6 100644 --- a/src/utils/Matrix.h +++ b/src/utils/Matrix.h @@ -107,50 +107,4 @@ inline vr::HmdVector3_t& matMul33( vr::HmdVector3_t& result, return result; } -inline vr::HmdQuaternion_t getQuaternion( 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; - q.x = sqrt( - fmax( 0, 1 + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2] ) ) - / 2; - q.y = sqrt( - fmax( 0, 1 - matrix.m[0][0] + matrix.m[1][1] - matrix.m[2][2] ) ) - / 2; - q.z = sqrt( - fmax( 0, 1 - matrix.m[0][0] - matrix.m[1][1] + matrix.m[2][2] ) ) - / 2; - q.x = copysign( q.x, matrix.m[2][1] - matrix.m[1][2] ); - q.y = copysign( q.y, matrix.m[0][2] - matrix.m[2][0] ); - q.z = copysign( q.z, matrix.m[1][0] - matrix.m[0][1] ); - return q; -} - -inline vr::HmdQuaternion_t multiplyQuaternion( 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 ), - ( lhs.w * rhs.y ) + ( lhs.y * rhs.w ) + ( lhs.z * rhs.x ) - - ( lhs.x * rhs.z ), - ( lhs.w * rhs.z ) + ( lhs.z * rhs.w ) + ( lhs.x * rhs.y ) - - ( lhs.y * rhs.x ) }; -} - -inline vr::HmdQuaternion_t - quaternionConjugate( const vr::HmdQuaternion_t& quat ) -{ - return { - quat.w, - -quat.x, - -quat.y, - -quat.z, - }; -} - } // end namespace utils diff --git a/src/utils/quaternion.h b/src/utils/quaternion.h new file mode 100644 index 0000000..7e2ecff --- /dev/null +++ b/src/utils/quaternion.h @@ -0,0 +1,62 @@ +#pragma once + +#include +//#include + +namespace utils +{ +inline vr::HmdQuaternion_t quaternionFromHmdMatrix34( 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; + q.x = sqrt( + fmax( 0, 1 + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2] ) ) + / 2; + q.y = sqrt( + fmax( 0, 1 - matrix.m[0][0] + matrix.m[1][1] - matrix.m[2][2] ) ) + / 2; + q.z = sqrt( + fmax( 0, 1 - matrix.m[0][0] - matrix.m[1][1] + matrix.m[2][2] ) ) + / 2; + q.x = copysign( q.x, matrix.m[2][1] - matrix.m[1][2] ); + q.y = copysign( q.y, matrix.m[0][2] - matrix.m[2][0] ); + q.z = copysign( q.z, matrix.m[1][0] - matrix.m[0][1] ); + return q; +} + +inline vr::HmdQuaternion_t quaternionMultiply( 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 ), + ( lhs.w * rhs.y ) + ( lhs.y * rhs.w ) + ( lhs.z * rhs.x ) + - ( lhs.x * rhs.z ), + ( lhs.w * rhs.z ) + ( lhs.z * rhs.w ) + ( lhs.x * rhs.y ) + - ( lhs.y * rhs.x ) }; +} + +inline vr::HmdQuaternion_t + quaternionConjugate( const vr::HmdQuaternion_t& quat ) +{ + return { + quat.w, + -quat.x, + -quat.y, + -quat.z, + }; +} + +inline double quaternionGetYaw( const vr::HmdQuaternion_t& quat ) +{ + double yawResult + = atan2( 2.0 * ( quat.y * quat.w + quat.x * quat.z ), + ( 2.0 * ( quat.w * quat.w + quat.x * quat.x ) ) - 1.0 ); + return yawResult; +} + +} // namespace utils