Create quaternion.h to centralize quaternion math

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 \

src/tabcontrollers/MoveCenterTabController.cpp

@@ -2,6 +2,7 @@
 #include <QQuickWindow>
 #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(
+            double hmdYawDiff = utils::quaternionGetYaw( hmdDiffQuaternion );
-                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;
@@ -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(
+                    double handYawDiff
-                        2.0
+                        = utils::quaternionGetYaw( handDiffQuaternion );
-                            * ( handDiffQuaternion.y * handDiffQuaternion.w
-                                + handDiffQuaternion.x * handDiffQuaternion.z ),
-                        -1.0
-                            + 2.0
-                                  * ( handDiffQuaternion.w
-                                          * handDiffQuaternion.w
-                                      + handDiffQuaternion.x
-                                            * handDiffQuaternion.x ) );
 
                     int newRotationAngleDeg = static_cast<int>(
                         round( handYawDiff * k_radiansToCentidegrees )

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

src/utils/quaternion.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <openvr.h>
+//#include <cmath>
+
+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