The open source OpenXR runtime
a/math: Add math_quat_decompose_swing_twist()
Decomposes a quaternion into two orthogonal rotations that
consist of the swing perpendicular to an axis and the
rotation (twist) around that axis.
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2188>
authored by
Jan Schmidt
and committed by
Marge Bot
5098695a
1bfd5ed0
+57
+19
src/xrt/auxiliary/math/m_api.h
+19
src/xrt/auxiliary/math/m_api.h
···
463
463
void
464
464
math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist);
465
465
466
+
/*!
467
+
* Decompose a quaternion to swing and twist component rotations around a target
468
+
* axis. The swing is always orthogonal to the target axis, and twist rotation is always
469
+
* around the axis.
470
+
*
471
+
* swing * twist gives back the original quat
472
+
* (e.g. math_quat_rotate(&swing, &twist, &orig_q))
473
+
*
474
+
* See https://arxiv.org/pdf/1506.05481.pdf
475
+
*
476
+
* @relates xrt_quat
477
+
* @ingroup aux_math
478
+
*/
479
+
void
480
+
math_quat_decompose_swing_twist(const struct xrt_quat *in,
481
+
const struct xrt_vec3 *twist_axis,
482
+
struct xrt_quat *swing,
483
+
struct xrt_quat *twist);
484
+
466
485
/*
467
486
*
468
487
* Matrix functions
+38
src/xrt/auxiliary/math/m_base.cpp
+38
src/xrt/auxiliary/math/m_base.cpp
···
550
550
*out_twist = twist_aax.axis().z() * twist_aax.angle();
551
551
}
552
552
553
+
void
554
+
math_quat_decompose_swing_twist(const struct xrt_quat *in,
555
+
const struct xrt_vec3 *twist_axis,
556
+
struct xrt_quat *swing,
557
+
struct xrt_quat *twist)
558
+
{
559
+
struct xrt_quat twist_inv;
560
+
struct xrt_vec3 orig_axis;
561
+
float dot;
562
+
563
+
orig_axis.x = in->x;
564
+
orig_axis.y = in->y;
565
+
orig_axis.z = in->z;
566
+
567
+
/* Calculate projection onto the twist axis */
568
+
dot = m_vec3_dot(orig_axis, *twist_axis);
569
+
struct xrt_vec3 projection = *twist_axis;
570
+
math_vec3_scalar_mul(dot, &projection);
571
+
572
+
twist->x = projection.x;
573
+
twist->y = projection.y;
574
+
twist->z = projection.z;
575
+
twist->w = in->w;
576
+
577
+
if (math_quat_dot(twist, twist) < FLT_EPSILON) {
578
+
/* Singularity - 180 degree rotation and perpendicular
579
+
* decomp axis, so twist is the identity quat */
580
+
twist->x = twist->y = twist->z = 0.0;
581
+
twist->w = 1.0;
582
+
} else {
583
+
math_quat_normalize(twist);
584
+
}
585
+
586
+
math_quat_invert(twist, &twist_inv);
587
+
math_quat_rotate(in, &twist_inv, swing);
588
+
math_quat_normalize(swing);
589
+
}
590
+
553
591
/*
554
592
*
555
593
* Exported matrix functions.