1 files changed, 44 insertions, 0 deletions
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index e4b44240272..37c0c05b061 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -535,6 +535,50 @@ 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
  *