mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-29 18:08:29 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Add math_quat_decompose_swing_twist()

Browse source 
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>
This commit is contained in:
Jan Schmidt 2023-08-10 05:03:39 +10:00 committed by Marge Bot
parent 1bfd5ed0cd
commit 5098695a1a
2 changed files with 57 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
src/xrt/auxiliary/math/m_api.h
View file

@ -463,6 +463,25 @@ math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, stru
void
math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, float *out_twist);
/*!
* Decompose a quaternion to swing and twist component rotations around a target
* axis. The swing is always orthogonal to the target axis, and twist rotation is always
* around the axis.
*
* swing * twist gives back the original quat
* (e.g. math_quat_rotate(&swing, &twist, &orig_q))
*
* See https://arxiv.org/pdf/1506.05481.pdf
*
* @relates xrt_quat
* @ingroup aux_math
*/
void
math_quat_decompose_swing_twist(const struct xrt_quat *in,
const struct xrt_vec3 *twist_axis,
struct xrt_quat *swing,
struct xrt_quat *twist);
/*
*
* Matrix functions

38
src/xrt/auxiliary/math/m_base.cpp
View file

@ -550,6 +550,44 @@ math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing,
*out_twist = twist_aax.axis().z() * twist_aax.angle();
}
void
math_quat_decompose_swing_twist(const struct xrt_quat *in,
const struct xrt_vec3 *twist_axis,
struct xrt_quat *swing,
struct xrt_quat *twist)
{
struct xrt_quat twist_inv;
struct xrt_vec3 orig_axis;
float dot;
orig_axis.x = in->x;
orig_axis.y = in->y;
orig_axis.z = in->z;
/* Calculate projection onto the twist axis */
dot = m_vec3_dot(orig_axis, *twist_axis);
struct xrt_vec3 projection = *twist_axis;
math_vec3_scalar_mul(dot, &projection);
twist->x = projection.x;
twist->y = projection.y;
twist->z = projection.z;
twist->w = in->w;
if (math_quat_dot(twist, twist) < FLT_EPSILON) {
/* Singularity - 180 degree rotation and perpendicular
* decomp axis, so twist is the identity quat */
twist->x = twist->y = twist->z = 0.0;
twist->w = 1.0;
} else {
math_quat_normalize(twist);
}
math_quat_invert(twist, &twist_inv);
math_quat_rotate(in, &twist_inv, swing);
math_quat_normalize(swing);
}
/*
*
* Exported matrix functions.