70 lines
2.2 KiB
C++
70 lines
2.2 KiB
C++
#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,
|
|
static_cast<double>( 1 + matrix.m[0][0] + matrix.m[1][1]
|
|
+ matrix.m[2][2] ) ) )
|
|
/ 2;
|
|
q.x = sqrt( fmax( 0,
|
|
static_cast<double>( 1 + matrix.m[0][0] - matrix.m[1][1]
|
|
- matrix.m[2][2] ) ) )
|
|
/ 2;
|
|
q.y = sqrt( fmax( 0,
|
|
static_cast<double>( 1 - matrix.m[0][0] + matrix.m[1][1]
|
|
- matrix.m[2][2] ) ) )
|
|
/ 2;
|
|
q.z = sqrt( fmax( 0,
|
|
static_cast<double>( 1 - matrix.m[0][0] - matrix.m[1][1]
|
|
+ matrix.m[2][2] ) ) )
|
|
/ 2;
|
|
q.x = copysign( q.x,
|
|
static_cast<double>( matrix.m[2][1] - matrix.m[1][2] ) );
|
|
q.y = copysign( q.y,
|
|
static_cast<double>( matrix.m[0][2] - matrix.m[2][0] ) );
|
|
q.z = copysign( q.z,
|
|
static_cast<double>( 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
|