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

source/blender/blenkernel/intern/fcurve.c

@@ -1798,6 +1798,25 @@ void BKE_driver_target_matrix_to_rot_channels(
       quaternion_to_angles(quat, channel);
     }
   }
+  else if (rotation_mode >= DTAR_ROTMODE_SWING_TWIST_X &&
+           rotation_mode <= DTAR_ROTMODE_SWING_TWIST_Z) {
+    int axis = rotation_mode - DTAR_ROTMODE_SWING_TWIST_X;
+    float raw_quat[4], twist;
+
+    mat4_to_quat(raw_quat, mat);
+
+    if (channel == axis + 1) {
+      /* If only the twist angle is needed, skip computing swing. */
+      twist = quat_split_swing_and_twist(raw_quat, axis, NULL, NULL);
+    }
+    else {
+      twist = quat_split_swing_and_twist(raw_quat, axis, quat, NULL);
+
+      quaternion_to_angles(quat, channel);
+    }
+
+    quat[axis + 1] = twist;
+  }
   else {
     BLI_assert(false);
   }

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]);

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
  *

source/blender/makesdna/DNA_anim_types.h

@@ -382,6 +382,12 @@ typedef enum eDriverTarget_RotationMode {
 
   DTAR_ROTMODE_QUATERNION,
 
+  /** Implements the very common Damped Track + child trick to decompose
+   *  rotation into bending followed by twist around the remaining axis. */
+  DTAR_ROTMODE_SWING_TWIST_X,
+  DTAR_ROTMODE_SWING_TWIST_Y,
+  DTAR_ROTMODE_SWING_TWIST_Z,
+
   DTAR_ROTMODE_EULER_MIN = DTAR_ROTMODE_EULER_XYZ,
   DTAR_ROTMODE_EULER_MAX = DTAR_ROTMODE_EULER_ZYX,
 } eDriverTarget_RotationMode;

source/blender/makesrna/intern/rna_fcurve.c

@@ -141,6 +141,21 @@ const EnumPropertyItem rna_enum_driver_target_rotation_mode_items[] = {
     {DTAR_ROTMODE_EULER_ZXY, "ZXY", 0, "ZXY Euler", "Euler using the ZXY rotation order"},
     {DTAR_ROTMODE_EULER_ZYX, "ZYX", 0, "ZYX Euler", "Euler using the ZYX rotation order"},
     {DTAR_ROTMODE_QUATERNION, "QUATERNION", 0, "Quaternion", "Quaternion rotation"},
+    {DTAR_ROTMODE_SWING_TWIST_X,
+     "SWING_TWIST_X",
+     0,
+     "Swing and X Twist",
+     "Decompose into a swing rotation to aim the X axis, followed by twist around it"},
+    {DTAR_ROTMODE_SWING_TWIST_Y,
+     "SWING_TWIST_Y",
+     0,
+     "Swing and Y Twist",
+     "Decompose into a swing rotation to aim the Y axis, followed by twist around it"},
+    {DTAR_ROTMODE_SWING_TWIST_Z,
+     "SWING_TWIST_Z",
+     0,
+     "Swing and Z Twist",
+     "Decompose into a swing rotation to aim the Z axis, followed by twist around it"},
     {0, NULL, 0, NULL, NULL},
 };