diff --git a/src/res/qml/RootPage.qml b/src/res/qml/RootPage.qml index f21e208..a947668 100644 --- a/src/res/qml/RootPage.qml +++ b/src/res/qml/RootPage.qml @@ -418,7 +418,7 @@ MyStackViewPage { // REVERT TO THIS LATER: summaryVersionText.text = applicationVersion // TEMP VERSION: - summaryVersionText.text = "Alpha-Kung v1.3" + summaryVersionText.text = "Alpha-Kung v1.4" if (MoveCenterTabController.trackingUniverse === 0) { summaryPlaySpaceModeText.text = "Sitting" diff --git a/src/tabcontrollers/MoveCenterTabController.cpp b/src/tabcontrollers/MoveCenterTabController.cpp index 1addcf8..027b8c3 100644 --- a/src/tabcontrollers/MoveCenterTabController.cpp +++ b/src/tabcontrollers/MoveCenterTabController.cpp @@ -624,11 +624,11 @@ void MoveCenterTabController::eventLoopTick( emit offsetYChanged( m_offsetY ); emit offsetZChanged( m_offsetZ ); - // Set lastHandYaw to placeholder value for invalid (-10.0) when we - // release move shortcut. + // Set lastHandQuaternion.w to placeholder value for invalid (-1000.0) + // when we release move shortcut. if ( m_rotateHand ) { - lastHandYaw = -10.0; + lastHandQuaternion.w = -1000.0; } return; @@ -647,7 +647,7 @@ void MoveCenterTabController::eventLoopTick( if ( m_rotateHand ) { - lastHandYaw = -10.0; + lastHandQuaternion.w = -1000.0; } } return; @@ -678,7 +678,7 @@ void MoveCenterTabController::eventLoopTick( vr::HmdMatrix34_t handMatrix = pose->mDeviceToAbsoluteTracking; // We need un-rotated coordinates for valid comparison between - // handYaw and lastHandYaw. Set up (un)rotation matrix. + // handQuaternion and lastHandQuaternion. Set up (un)rotation matrix. vr::HmdMatrix34_t handMatrixRotMat; vr::HmdMatrix34_t handMatrixAbsolute; utils::initRotationMatrix( @@ -687,9 +687,8 @@ void MoveCenterTabController::eventLoopTick( // Get handMatrixAbsolute in un-rotated coordinates. utils::matMul33( handMatrixAbsolute, handMatrixRotMat, handMatrix ); - // Convert pose matrix to yaw. - handYaw = std::atan2( handMatrixAbsolute.m[0][2], - handMatrixAbsolute.m[2][2] ); + // Convert pose matrix to quaternion + handQuaternion = utils::getQuaternion( handMatrixAbsolute ); } if ( oldMoveHand == newMoveHand ) @@ -749,17 +748,25 @@ void MoveCenterTabController::eventLoopTick( // Get rotation change of hand. if ( m_rotateHand ) { - // Checking if set to -10.0 placeholder for invalid hand. - if ( lastHandYaw < -9.0 ) + // Checking if set to -1000.0 placeholder for invalid hand. + if ( lastHandQuaternion.w < -900.0 ) { - lastHandYaw = handYaw; + lastHandQuaternion = handQuaternion; } else { - double handYawDiff = handYaw - lastHandYaw; + // Construct a quaternion representing difference between old + // hand and new hand. + vr::HmdQuaternion_t handDiffQuaternion + = utils::multiplyQuaternion( + handQuaternion, + utils::quaternionConjugate( lastHandQuaternion ) ); + + // Calculate yaw from quaternion. + double handYawDiff + = atan2( handDiffQuaternion.y, handDiffQuaternion.w ) * 2.0; - // TODO: Increase angular granularity beyond 1 degree. int newRotationAngleDeg = static_cast( round( handYawDiff * 18000.0 / M_PI ) + m_rotation ); @@ -780,6 +787,6 @@ void MoveCenterTabController::eventLoopTick( m_lastControllerPosition[0] = absoluteControllerPosition[0]; m_lastControllerPosition[1] = absoluteControllerPosition[1]; m_lastControllerPosition[2] = absoluteControllerPosition[2]; - lastHandYaw = handYaw; + lastHandQuaternion = handQuaternion; } } // namespace advsettings diff --git a/src/tabcontrollers/MoveCenterTabController.h b/src/tabcontrollers/MoveCenterTabController.h index 7f33987..36d7d56 100644 --- a/src/tabcontrollers/MoveCenterTabController.h +++ b/src/tabcontrollers/MoveCenterTabController.h @@ -67,9 +67,9 @@ private: bool m_lockYToggle = false; bool m_lockZToggle = false; std::chrono::system_clock::time_point lastMoveButtonClick[2]; - // Set to -10.0 when last hand is invalid. - double lastHandYaw = -10.0; - double handYaw; + // Set lastHandQuaternion.w to -1000.0 when last hand is invalid. + vr::HmdQuaternion_t lastHandQuaternion = { -1000.0, 0.0, 0.0, 0.0 }; + vr::HmdQuaternion_t handQuaternion; unsigned settingsUpdateCounter = 0; diff --git a/src/utils/Matrix.h b/src/utils/Matrix.h index 0eed8d6..8cba324 100644 --- a/src/utils/Matrix.h +++ b/src/utils/Matrix.h @@ -107,4 +107,50 @@ 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