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.
2019-01-17 02:32:08 +09:00
parent 0c0a616691
commit 47920bfa74
4 changed files with 71 additions and 18 deletions
@@ -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