Fix gimbal-lock when controller pointed up or down

Compare old and new pose via a relative quaternion rather than relative yaw, then calculate the resulting yaw-difference from the quaternion in one go.

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"

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
+        // Set lastHandQuaternion.w to placeholder value for invalid (-1000.0)
-        // release move shortcut.
+        // 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.
+        // Convert pose matrix to quaternion
-        handYaw = std::atan2( handMatrixAbsolute.m[0][2],
+        handQuaternion = utils::getQuaternion( handMatrixAbsolute );
-                              handMatrixAbsolute.m[2][2] );
     }
 
     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.
+            // Checking if set to -1000.0 placeholder for invalid hand.
-            if ( lastHandYaw < -9.0 )
+            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<int>(
                     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

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.
+    // Set lastHandQuaternion.w to -1000.0 when last hand is invalid.
-    double lastHandYaw = -10.0;
+    vr::HmdQuaternion_t lastHandQuaternion = { -1000.0, 0.0, 0.0, 0.0 };
-    double handYaw;
+    vr::HmdQuaternion_t handQuaternion;
 
     unsigned settingsUpdateCounter = 0;
 

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