diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_rotation.c')
-rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 43 |
1 files changed, 43 insertions, 0 deletions
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index e4b44240272..5762d164914 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.c @@ -535,6 +535,49 @@ void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q mul_qt_qtqt(q, tquat, q2); } +/** Decompose a quaternion into a swing rotation (quaternion with the selected + * axis component locked at zero), followed by a twist rotation around the axis. + * + * \param q: input quaternion. + * \param axis: twist axis in [0,1,2] + * \param r_swing[out]: if not NULL, receives the swing quaternion. + * \param r_twist[out]: if not NULL, receives the twist quaternion. + * \returns twist angle. + */ +float quat_split_swing_and_twist(const float q[4], int axis, float r_swing[4], float r_twist[4]) +{ + BLI_assert(axis >= 0 && axis <= 2); + + /* Half-twist angle can be computed directly. */ + float t = atan2f(q[axis + 1], q[0]); + + if (r_swing || r_twist) { + float sin_t = sinf(t), cos_t = cosf(t); + + /* Compute swing by multiplying the original quaternion by inverted twist. */ + if (r_swing) { + float twist_inv[4]; + + twist_inv[0] = cos_t; + zero_v3(twist_inv + 1); + twist_inv[axis + 1] = -sin_t; + + mul_qt_qtqt(r_swing, q, twist_inv); + + BLI_assert(fabsf(r_swing[axis + 1]) < BLI_ASSERT_UNIT_EPSILON); + } + + /* Output twist last just in case q ovelaps r_twist. */ + if (r_twist) { + r_twist[0] = cos_t; + zero_v3(r_twist + 1); + r_twist[axis + 1] = sin_t; + } + } + + return 2.0f * t; +} + /* -------------------------------------------------------------------- */ /** \name Quaternion Angle * |