diff --git a/src/utils/Matrix.h b/src/utils/Matrix.h index 8cba324..1185a33 100644 --- a/src/utils/Matrix.h +++ b/src/utils/Matrix.h @@ -111,21 +111,28 @@ 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] ) ) + q.w = sqrt( fmax( 0, + static_cast( 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] ) ) + q.x = sqrt( fmax( 0, + static_cast( 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] ) ) + q.y = sqrt( fmax( 0, + static_cast( 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] ) ) + q.z = sqrt( fmax( 0, + static_cast( 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] ); + q.x = copysign( q.x, + static_cast( matrix.m[2][1] - matrix.m[1][2] ) ); + q.y = copysign( q.y, + static_cast( matrix.m[0][2] - matrix.m[2][0] ) ); + q.z = copysign( q.z, + static_cast( matrix.m[1][0] - matrix.m[0][1] ) ); return q; }