diff options
Diffstat (limited to 'source/blender/blenlib')
-rw-r--r-- | source/blender/blenlib/BLI_math_rotation.h | 2 | ||||
-rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 43 |
2 files changed, 45 insertions, 0 deletions
diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h index 7a4ac14970f..1e56b80bcf2 100644 --- a/source/blender/blenlib/BLI_math_rotation.h +++ b/source/blender/blenlib/BLI_math_rotation.h @@ -96,6 +96,8 @@ void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3]); void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4]); +float quat_split_swing_and_twist(const float q[4], int axis, float r_swing[4], float r_twist[4]); + float angle_normalized_qt(const float q[4]); float angle_normalized_qtqt(const float q1[4], const float q2[4]); float angle_qt(const float q[4]); 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 * |