SteamFrame/OpenVR-AdvancedSettings-frame
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Create quaternion.h to centralize quaternion math

Browse Source
This commit is contained in:
Kung-vr
2019-02-02 22:36:19 +09:00
parent 95be1a4aab
commit 5832eea951
4 changed files with 73 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/tabcontrollers/MoveCenterTabController.cpp
+9 -22
View File
@@ -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(
2.0
* ( hmdDiffQuaternion.y * hmdDiffQuaternion.w
+ hmdDiffQuaternion.x * hmdDiffQuaternion.z ),
-1.0
+ 2.0
* ( hmdDiffQuaternion.w * hmdDiffQuaternion.w
+ hmdDiffQuaternion.x * hmdDiffQuaternion.x ) );
double hmdYawDiff = utils::quaternionGetYaw( hmdDiffQuaternion );
// 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(
2.0
* ( handDiffQuaternion.y * handDiffQuaternion.w
+ handDiffQuaternion.x * handDiffQuaternion.z ),
-1.0
+ 2.0
* ( handDiffQuaternion.w
* handDiffQuaternion.w
+ handDiffQuaternion.x
* handDiffQuaternion.x ) );
double handYawDiff
= utils::quaternionGetYaw( handDiffQuaternion );
int newRotationAngleDeg = static_cast<int>(
round( handYawDiff * k_radiansToCentidegrees )
src/utils/Matrix.h
-46
View File
@@ -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
+62
View File
@@ -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