Drivers: support decomposing rotation into swing followed by twist.

In order to correctly drive corrective shape keys from a freely
rotating organic joint it is very often found necessary to
decompose the rotation into separate bending and twisting
motions. This type of decomposition cannot be reproduced by
any Euler order or a single quaternion.

Instead this is done by using a helper bone with a Damped Track
constraint aimed at the tail of the control to pick up the bending,
and its helper child with Copy Transforms to separate the twist.

Requiring two additional bones to drive a shape key or a correction
bone seems inconvenient, so this implements the necessary math as new
options in the recently introduced Rotation Mode dropdown of the
Transform Channel driver variable type. The data is also accessible
as a Transformation constraint input.

The output is in the form of Quaternion-derived 'pseudo-angles',
which for `Swing and Y Twist` would represent the following:

* W: true bend angle, independent of bend direction.
* Y: true twist angle.
* X, Z: pseudo-angles representing the proportion of bending around X/Z.

Reviewers: brecht

Differential Revision: https://developer.blender.org/D5651

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
  *