Better code for (bone axis + roll) to mat

See T39470 and D436. Code by @tippisum, with some minor edits by @mont29.

Tested with various rigs, including Rigify, CGcookie flex rig, and gooseberry/pataz caterpillar.

Riggers, please test it, no change expected in behaviour.

Reviewers: aligorith

CC: tippisum

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

1 files changed, 78 insertions, 34 deletions

diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index 60ec4817f81..845781f9abc 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -1414,6 +1414,7 @@ void BKE_rotMode_change_values(float quat[4], float eul[3], float axis[3], float
  * pose_mat(b)= arm_mat(b) * chan_mat(b)
  *
  * *************************************************************************** */
+
 /* Computes vector and roll based on a rotation.
  * "mat" must contain only a rotation, and no scaling. */
 void mat3_to_vec_roll(float mat[3][3], float r_vec[3], float *r_roll)
@@ -1433,52 +1434,95 @@ void mat3_to_vec_roll(float mat[3][3], float r_vec[3], float *r_roll)
 	}
 }
 
-/* Calculates the rest matrix of a bone based
- * On its vector and a roll around that vector */
+/* Calculates the rest matrix of a bone based on its vector and a roll around that vector. */
+/* Given v = (v.x, v.y, v.z) our (normalized) bone vector, we want the rotation matrix M
+ * from the Y axis (so that M * (0, 1, 0) = v).
+ *   -> The rotation axis a lays on XZ plane, and it is orthonormal to v, hence to the projection of v onto XZ plane.
+ *   -> a = (v.z, 0, -v.x)
+ * We know a is eigenvector of M (so M * a = a).
+ * Finally, we have w, such that M * w = (0, 1, 0) (i.e. the vector that will be aligned with Y axis once transformed).
+ * We know w is symmetric to v by the Y axis.
+ *   -> w = (-v.x, v.y, -v.z)
+ *
+ * Solving this, we get (x, y and z being the components of v):
+ *     ┌ (x^2 * y + z^2) / (x^2 + z^2),   x,   x * z * (y - 1) / (x^2 + z^2) ┐
+ * M = │  x * (y^2 - 1)  / (x^2 + z^2),   y,    z * (y^2 - 1)  / (x^2 + z^2) │
+ *     └ x * z * (y - 1) / (x^2 + z^2),   z,   (x^2 + z^2 * y) / (x^2 + z^2) ┘
+ *
+ * This is stable as long as v (the bone) is not too much aligned with +/-Y (i.e. x and z components
+ * are not too close to 0).
+ *
+ * Since v is normalized, we have x^2 + y^2 + z^2 = 1, hence x^2 + z^2 = 1 - y^2 = (1 - y)(1 + y).
+ * This allows to simplifies M like this:
+ *     ┌ 1 - x^2 / (1 + y),   x,     -x * z / (1 + y) ┐
+ * M = │                -x,   y,                   -z │
+ *     └  -x * z / (1 + y),   z,    1 - z^2 / (1 + y) ┘
+ *
+ * Written this way, we see the case v = +Y is no more a singularity. The only one remaining is the bone being
+ * aligned with -Y.
+ *
+ * Let's handle the asymptotic behavior when bone vector is reaching the limit of y = -1. Each of the four corner
+ * elements can vary from -1 to 1, depending on the axis a chosen for doing the rotation. And the "rotation" here
+ * is in fact established by mirroring XZ plane by that given axis, then inversing the Y-axis.
+ * For sufficiently small x and z, and with y approaching -1, all elements but the four corner ones of M
+ * will degenerate. So let's now focus on these corner elements.
+ *
+ * We rewrite M so that it only contains its four corner elements, and combine the 1 / (1 + y) factor:
+ *                    ┌ 1 + y - x^2,        -x * z ┐
+ * M* = 1 / (1 + y) * │                            │
+ *                    └      -x * z,   1 + y - z^2 ┘
+ *
+ * When y is close to -1, computing 1 / (1 + y) will cause severe numerical instability, so we ignore it and
+ * normalize M instead. We know y^2 = 1 - (x^2 + z^2), and y < 0, hence y = -sqrt(1 - (x^2 + z^2)).
+ * Since x and z are both close to 0, we apply the binomial expansion to the first order:
+ * y = -sqrt(1 - (x^2 + z^2)) = -1 + (x^2 + z^2) / 2. Which gives:
+ *                        ┌  z^2 - x^2,  -2 * x * z ┐
+ * M* = 1 / (x^2 + z^2) * │                         │
+ *                        └ -2 * x * z,   x^2 - z^2 ┘
+ */
 void vec_roll_to_mat3(const float vec[3], const float roll, float mat[3][3])
 {
-	float nor[3], axis[3], target[3] = {0, 1, 0};
+	float nor[3];
 	float theta;
 	float rMatrix[3][3], bMatrix[3][3];
 
 	normalize_v3_v3(nor, vec);
 
-	/* Find Axis & Amount for bone matrix */
-	cross_v3_v3v3(axis, target, nor);
+	theta = 1 + nor[1];
 
-	/* was 0.0000000000001, caused bug [#23954], smaller values give unstable
-	 * roll when toggling editmode.
-	 *
-	 * was 0.00001, causes bug [#27675], with 0.00000495,
-	 * so a value inbetween these is needed.
+	/* With old algo, 1.0e-13f caused T23954 and T31333, 1.0e-6f caused T27675 and T30438,
+	 * so using 1.0e-9f as best compromise.
 	 *
-	 * was 0.000001, causes bug [#30438] (which is same as [#27675, imho).
-	 * Resetting it to org value seems to cause no more [#23954]...
-	 *
-	 * was 0.0000000000001, caused bug [#31333], smaller values give unstable
-	 * roll when toggling editmode again...
-	 * No good value here, trying 0.000000001 as best compromise. :/
+	 * New algo is supposed much more precise, since less complex computations are performed,
+	 * but it uses two different threshold values...
 	 */
-	if (len_squared_v3(axis) > 1.0e-9f) {
-		/* if nor is *not* a multiple of target ... */
-		normalize_v3(axis);
-
-		theta = angle_normalized_v3v3(target, nor);
-
-		/* Make Bone matrix*/
-		axis_angle_normalized_to_mat3(bMatrix, axis, theta);
+	if (theta > 1.0e-9f) {
+		/* nor is *not* -Y.
+		 * We got these values for free... so be happy with it... ;)
+		 */
+		bMatrix[0][1] = -nor[0];
+		bMatrix[1][0] = nor[0];
+		bMatrix[1][1] = nor[1];
+		bMatrix[1][2] = nor[2];
+		bMatrix[2][1] = -nor[2];
+		if (theta > 1.0e-5f) {
+			/* If nor is far enough from -Y, apply the general case. */
+			bMatrix[0][0] = 1 - nor[0] * nor[0] / theta;
+			bMatrix[2][2] = 1 - nor[2] * nor[2] / theta;
+			bMatrix[2][0] = bMatrix[0][2] = -nor[0] * nor[2] / theta;
+		}
+		else {
+			/* If nor is too close to -Y, apply the special case. */
+			theta = nor[0] * nor[0] + nor[2] * nor[2];
+			bMatrix[0][0] = (nor[0] + nor[2]) * (nor[0] - nor[2]) / theta;
+			bMatrix[2][2] = -bMatrix[0][0];
+			bMatrix[2][0] = bMatrix[0][2] = 2.0f * nor[0] * nor[2] / theta;
+		}
 	}
 	else {
-		/* if nor is a multiple of target ... */
-		float updown;
-
-		/* point same direction, or opposite? */
-		updown = (dot_v3v3(target, nor) > 0) ? 1.0f : -1.0f;
-
-		/* I think this should work... */
-		bMatrix[0][0] = updown; bMatrix[0][1] = 0.0;    bMatrix[0][2] = 0.0;
-		bMatrix[1][0] = 0.0;    bMatrix[1][1] = updown; bMatrix[1][2] = 0.0;
-		bMatrix[2][0] = 0.0;    bMatrix[2][1] = 0.0;    bMatrix[2][2] = 1.0;
+		/* If nor is -Y, simple symmetry by Z axis. */
+		unit_m3(bMatrix);
+		bMatrix[0][0] = bMatrix[1][1] = -1.0;
 	}
 
 	/* Make Roll matrix */