From 499a0e3edec663f51669f94c68ca53f3f380a799 Mon Sep 17 00:00:00 2001 From: Kung-vr Date: Thu, 24 Jan 2019 00:58:15 +0900 Subject: [PATCH] Fix rotation counter stats during rotation drag --- src/res/qml/RootPage.qml | 4 +- src/res/qml/StatisticsPage.qml | 4 +- .../MoveCenterTabController.cpp | 83 +++++++++++- src/tabcontrollers/MoveCenterTabController.h | 6 + .../StatisticsTabController.cpp | 127 +----------------- 5 files changed, 97 insertions(+), 127 deletions(-) diff --git a/src/res/qml/RootPage.qml b/src/res/qml/RootPage.qml index a947668..1158b85 100644 --- a/src/res/qml/RootPage.qml +++ b/src/res/qml/RootPage.qml @@ -489,9 +489,9 @@ MyStackViewPage { function updateStatistics() { var rotations = StatisticsTabController.hmdRotations if (rotations > 0) { - summaryHmdRotationsText.text = rotations.toFixed(1) + " CCW" + summaryHmdRotationsText.text = rotations.toFixed(2) + " CCW" } else { - summaryHmdRotationsText.text = -rotations.toFixed(1) + " CW" + summaryHmdRotationsText.text = -rotations.toFixed(2) + " CW" } summaryDroppedFramesText.text = StatisticsTabController.droppedFrames summaryReprojectedFramesText.text = StatisticsTabController.reprojectedFrames diff --git a/src/res/qml/StatisticsPage.qml b/src/res/qml/StatisticsPage.qml index f0907a4..5d2773e 100644 --- a/src/res/qml/StatisticsPage.qml +++ b/src/res/qml/StatisticsPage.qml @@ -198,9 +198,9 @@ MyStackViewPage { statsHmdMovedText.text = StatisticsTabController.hmdDistanceMoved.toFixed(1) + " m" var rotations = StatisticsTabController.hmdRotations if (rotations > 0) { - statsHmdRotationText.text = rotations.toFixed(1) + " CCW" + statsHmdRotationText.text = rotations.toFixed(2) + " CCW" } else { - statsHmdRotationText.text = -rotations.toFixed(1) + " CW" + statsHmdRotationText.text = -rotations.toFixed(2) + " CW" } statsLeftControllerSpeedText.text = " " + StatisticsTabController.leftControllerMaxSpeed.toFixed(1) + " m/s" statsRightControllerSpeedText.text = " " + StatisticsTabController.rightControllerMaxSpeed.toFixed(1) + " m/s" diff --git a/src/tabcontrollers/MoveCenterTabController.cpp b/src/tabcontrollers/MoveCenterTabController.cpp index 027b8c3..69e5ab4 100644 --- a/src/tabcontrollers/MoveCenterTabController.cpp +++ b/src/tabcontrollers/MoveCenterTabController.cpp @@ -510,6 +510,16 @@ void MoveCenterTabController::reset() emit rotationChanged( m_rotation ); } +double MoveCenterTabController::getHmdYawTotal() +{ + return m_hmdYawTotal; +} + +void MoveCenterTabController::resetHmdYawTotal() +{ + m_hmdYawTotal = 0.0; +} + bool isMoveShortCutPressed( vr::ETrackedControllerRole hand ) { auto handId @@ -618,6 +628,67 @@ void MoveCenterTabController::eventLoopTick( } auto oldMoveHand = m_activeMoveController; auto newMoveHand = getMoveShortcutHand(); + double angle = m_rotation * M_PI / 18000.0; + + // hmd rotation tracking stuff: + // Check if hmd is tracking ok and do everything + if ( devicePoses[0].bPoseIsValid + && devicePoses[0].eTrackingResult == vr::TrackingResult_Running_OK ) + { + // Get hmd pose matrix (in rotated coordinates) + vr::HmdMatrix34_t hmdMatrix = devicePoses[0].mDeviceToAbsoluteTracking; + + // Set up (un)rotation matrix + vr::HmdMatrix34_t hmdMatrixRotMat; + vr::HmdMatrix34_t hmdMatrixAbsolute; + utils::initRotationMatrix( + hmdMatrixRotMat, 1, static_cast( angle ) ); + + // Get hmdMatrixAbsolute in un-rotated coordinates. + utils::matMul33( hmdMatrixAbsolute, hmdMatrixRotMat, hmdMatrix ); + + // Convert pose matrix to quaternion + hmdQuaternion = utils::getQuaternion( hmdMatrixAbsolute ); + + // Get rotation change of hmd + // Checking if set to -1000.0 placeholder for invalid hmd pose. + if ( lastHmdQuaternion.w < -900.0 ) + { + lastHmdQuaternion = hmdQuaternion; + } + + else + { + // Construct a quaternion representing difference between old + // hmd pose and new hmd pose. + vr::HmdQuaternion_t hmdDiffQuaternion = utils::multiplyQuaternion( + hmdQuaternion, + utils::quaternionConjugate( 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 ) ); + + // Apply yaw difference to m_hmdYawTotal. + m_hmdYawTotal += hmdYawDiff; + lastHmdQuaternion = hmdQuaternion; + } + + // end check if hmdpose is ok + } + else + { + // set lastHmdQuaternion.w to placeholder -1000.0 for invalid + lastHmdQuaternion.w = -1000.0; + } + // end hmd rotating tracking stuff + if ( newMoveHand == vr::TrackedControllerRole_Invalid ) { emit offsetXChanged( m_offsetX ); @@ -663,7 +734,6 @@ void MoveCenterTabController::eventLoopTick( pose->mDeviceToAbsoluteTracking.m[1][3], pose->mDeviceToAbsoluteTracking.m[2][3] }; - double angle = m_rotation * M_PI / 18000.0; rotateCoordinates( relativeControllerPosition, -angle ); float absoluteControllerPosition[] = { static_cast( relativeControllerPosition[0] ) + m_offsetX, @@ -764,8 +834,15 @@ void MoveCenterTabController::eventLoopTick( utils::quaternionConjugate( lastHandQuaternion ) ); // Calculate yaw from quaternion. - double handYawDiff - = atan2( handDiffQuaternion.y, handDiffQuaternion.w ) * 2.0; + 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 ) ); int newRotationAngleDeg = static_cast( round( handYawDiff * 18000.0 / M_PI ) + m_rotation ); diff --git a/src/tabcontrollers/MoveCenterTabController.h b/src/tabcontrollers/MoveCenterTabController.h index 36d7d56..473eafd 100644 --- a/src/tabcontrollers/MoveCenterTabController.h +++ b/src/tabcontrollers/MoveCenterTabController.h @@ -70,6 +70,10 @@ private: // 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; + // Set lastHmdQuaternion.w to -1000.0 when last hmd pose is invalid. + vr::HmdQuaternion_t lastHmdQuaternion = { -1000.0, 0.0, 0.0, 0.0 }; + vr::HmdQuaternion_t hmdQuaternion; + double m_hmdYawTotal = 0.0; unsigned settingsUpdateCounter = 0; @@ -95,6 +99,8 @@ public: bool lockXToggle() const; bool lockYToggle() const; bool lockZToggle() const; + double getHmdYawTotal(); + void resetHmdYawTotal(); public slots: int trackingUniverse() const; diff --git a/src/tabcontrollers/StatisticsTabController.cpp b/src/tabcontrollers/StatisticsTabController.cpp index a0d3c9f..ecf2592 100644 --- a/src/tabcontrollers/StatisticsTabController.cpp +++ b/src/tabcontrollers/StatisticsTabController.cpp @@ -77,126 +77,12 @@ void StatisticsTabController::eventLoopTick( m_rightControllerMaxSpeed = rightSpeed; } - // Hmd Rotation // - /* - | Intrinsic y-x'-z" rotation matrix: - | cr*cy+sp*sr*sy | cr*sp*sy-cy*sr | cp*sy | - | cp*sr | cp*cr |-sp | - | cy*sp*sr-cr*sy | cr*cy*sp+sr*sy | cp*cy | + // HMD Rotation // + double playspaceHmdYawTotal + = parent->m_moveCenterTabController.getHmdYawTotal(); + m_hmdRotation = static_cast( playspaceHmdYawTotal / ( 2.0 * M_PI ) ); - yaw = atan2(cp*sy, cp*cy) [pi, -pi], CCW - pitch = -asin(-sp) [pi/2, -pi/2] - roll = atan2(cp*sr, cp*cr) [pi, -pi], CW - */ - - double yawRaw = 0.0; - double yaw = 0.0; - bool rotationChanged = false; - if ( devicePoses[0].bPoseIsValid - && devicePoses[0].eTrackingResult == vr::TrackingResult_Running_OK ) - { - yawRaw = std::atan2( m[0][2], m[2][2] ); - if ( yawRaw < 0.0f ) - { - yawRaw += 2 * M_PI; // map to [0, 2*pi], CCW - } - yaw = yawRaw - rotationOffset; - if ( yaw < 0.0 ) - { - yaw = 2 * M_PI + yaw; - } - if ( rotationResetFlag ) - { - rotationDir = 0; - rotationCounter = 0; - rotationOffset = yawRaw; - rotationResetFlag = false; - lastYaw = -1.0f; - rotationChanged = true; - } - else if ( lastYaw < 0.0f && yaw > 0.0 ) - { - lastYaw = yaw; - rotationChanged = true; - if ( yaw <= M_PI ) - { - rotationDir = 1; - } - else - { - rotationDir = -1; - } - } - else if ( std::abs( lastYaw - yaw ) >= 0.01 && yaw > 0.0 ) - { - auto diff = yaw - lastYaw; - int mode = 0; - if ( std::abs( diff ) > M_PI ) - { - if ( diff < -M_PI ) - { // CCW overflow - mode = 1; - } - else if ( diff > M_PI ) - { // CW overflow - mode = -1; - } - } - else - { - if ( lastYaw < 0.0 && yaw > 0.0 ) - { - mode = 1; - } - else if ( lastYaw > 0.0 && yaw < 0.0 ) - { - mode = -1; - } - } - if ( mode > 0 ) - { - if ( rotationCounter == 0 && rotationDir <= 0 ) - { - rotationDir = 1; - } - else - { - rotationCounter++; - } - } - else if ( mode < 0 ) - { - if ( rotationCounter == 0 && rotationDir >= 0 ) - { - rotationDir = -1; - } - else - { - rotationCounter--; - } - } - lastYaw = yaw; - rotationChanged = true; - } - } - else - { - lastYaw = -1; - } - - if ( rotationChanged ) - { - m_hmdRotation = ( float ) rotationCounter; - if ( rotationDir > 0 ) - { - m_hmdRotation += yaw / ( 2 * M_PI ); - } - else if ( rotationDir < 0 ) - { - m_hmdRotation += -1.0f + yaw / ( 2 * M_PI ); - } - } - if ( lastPosTimer == 0 ) + if ( lastPosTimer <= 0 ) { lastPosTimer = 10; } @@ -273,7 +159,8 @@ void StatisticsTabController::statsDistanceResetClicked() void StatisticsTabController::statsRotationResetClicked() { - rotationResetFlag = true; + // rotationResetFlag = true; + parent->m_moveCenterTabController.resetHmdYawTotal(); } void StatisticsTabController::statsLeftControllerSpeedResetClicked()