The open source OpenXR runtime
a/math: Add math_quat_to_swing_twist
+37
+9
src/xrt/auxiliary/math/m_api.h
+9
src/xrt/auxiliary/math/m_api.h
···
413
413
void
414
414
math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, struct xrt_quat *result);
415
415
416
+
/*!
417
+
* Converts a quaternion to XY-swing and Z-twist
418
+
*
419
+
* @relates xrt_quat
420
+
* @ingroup aux_math
421
+
*/
422
+
void
423
+
math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist);
424
+
416
425
/*
417
426
*
418
427
* Matrix functions
+28
src/xrt/auxiliary/math/m_base.cpp
+28
src/xrt/auxiliary/math/m_base.cpp
···
484
484
}
485
485
}
486
486
487
+
/*!
488
+
* Converts a quaternion to XY-swing and Z-twist
489
+
*
490
+
* @relates xrt_quat
491
+
* @ingroup aux_math
492
+
*/
493
+
extern "C" void
494
+
math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist)
495
+
{
496
+
Eigen::Quaternionf rot = map_quat(*in);
497
+
498
+
Eigen::Vector3f our_z = rot * (Eigen::Vector3f::UnitZ());
499
+
500
+
Eigen::Quaternionf swing = Eigen::Quaternionf().setFromTwoVectors(Eigen::Vector3f::UnitZ(), our_z);
501
+
502
+
Eigen::Quaternionf twist = swing.inverse() * rot;
503
+
504
+
Eigen::AngleAxisf twist_aax = Eigen::AngleAxisf(twist);
505
+
506
+
Eigen::AngleAxisf swing_aax = Eigen::AngleAxisf(swing);
507
+
508
+
out_swing->x = swing_aax.axis().x() * swing_aax.angle();
509
+
out_swing->y = swing_aax.axis().y() * swing_aax.angle();
510
+
assert(swing_aax.axis().z() < 0.001);
511
+
512
+
*out_twist = twist_aax.axis().z() * twist_aax.angle();
513
+
}
514
+
487
515
/*
488
516
*
489
517
* Exported matrix functions.