Fix precision issues and a bug in vec_roll_to_mat3_normalized.

When the input vector gets close to -Y, y and theta becomes totally
unreliable. It is thus necessary to compute the result in a different
way based on x and z. The code already had a special case, but:

- The threshold for using the special case was way too low.
- The special case was not precise enough to extend the threshold.
- The special case math had a sign error, resulting in a jump.

This adds tests for the computation precision and fixes the issues
by adjusting the threshold, and replacing the special case with one
based on a quadratic Taylor expansion of sqrt instead of linear.

Replacing the special case fixes the bug and results in a compatibility
break, requiring versioning for the roll of affected bones.

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

2 files changed, 88 insertions, 24 deletions

diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index a266718dcfc..0fa4c6e47e8 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -2227,39 +2227,47 @@ void mat3_vec_to_roll(const float mat[3][3], const float vec[3], float *r_roll)
  * </pre>
  *
  * When y is close to -1, computing 1 / (1 + y) will cause severe numerical instability,
- * so we ignore it and normalize M instead.
+ * so we use a different approach based on x and z as inputs.
  * 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:
+ * Since x and z are both close to 0, we apply the binomial expansion to the second order:
+ * `y = -sqrt(1 - (x^2 + z^2)) = -1 + (x^2 + z^2) / 2 + (x^2 + z^2)^2 / 8`, which allows
+ * eliminating the problematic `1` constant.
+ *
+ * A first order expansion allows simplifying to this, but second order is more precise:
  * <pre>
  *                        ┌  z^2 - x^2,  -2 * x * z ┐
  * M* = 1 / (x^2 + z^2) * │                         │
  *                        └ -2 * x * z,   x^2 - z^2 ┘
  * </pre>
+ *
+ * P.S. In the end, this basically is a heavily optimized version of Damped Track +Y.
  */
 void vec_roll_to_mat3_normalized(const float nor[3], const float roll, float r_mat[3][3])
 {
-  const float SAFE_THRESHOLD = 1.0e-5f;     /* theta above this value has good enough precision. */
-  const float CRITICAL_THRESHOLD = 1.0e-9f; /* above this is safe under certain conditions. */
+  const float SAFE_THRESHOLD = 6.1e-3f;     /* theta above this value has good enough precision. */
+  const float CRITICAL_THRESHOLD = 2.5e-4f; /* true singularity if xz distance is below this. */
   const float THRESHOLD_SQUARED = CRITICAL_THRESHOLD * CRITICAL_THRESHOLD;
 
   const float x = nor[0];
   const float y = nor[1];
   const float z = nor[2];
 
-  const float theta = 1.0f + y;          /* remapping Y from [-1,+1] to [0,2]. */
-  const float theta_alt = x * x + z * z; /* Helper value for matrix calculations.*/
+  float theta = 1.0f + y;                /* remapping Y from [-1,+1] to [0,2]. */
+  const float theta_alt = x * x + z * z; /* squared distance from origin in x,z plane. */
   float rMatrix[3][3], bMatrix[3][3];
 
   BLI_ASSERT_UNIT_V3(nor);
 
-  /* When theta is close to zero (nor is aligned close to negative Y Axis),
+  /* Determine if the input is far enough from the true singularity of this type of
+   * transformation at (0,-1,0), where roll becomes 0/0 undefined without a limit.
+   *
+   * When theta is close to zero (nor is aligned close to negative Y Axis),
    * we have to check we do have non-null X/Z components as well.
    * Also, due to float precision errors, nor can be (0.0, -0.99999994, 0.0) which results
    * in theta being close to zero. This will cause problems when theta is used as divisor.
    */
-  if (theta > SAFE_THRESHOLD || (theta > CRITICAL_THRESHOLD && theta_alt > THRESHOLD_SQUARED)) {
+  if (theta > SAFE_THRESHOLD || theta_alt > THRESHOLD_SQUARED) {
     /* nor is *not* aligned to negative Y-axis (0,-1,0). */
 
     bMatrix[0][1] = -x;
@@ -2268,18 +2276,15 @@ void vec_roll_to_mat3_normalized(const float nor[3], const float roll, float r_m
     bMatrix[1][2] = z;
     bMatrix[2][1] = -z;
 
-    if (theta > SAFE_THRESHOLD) {
-      /* nor differs significantly from negative Y axis (0,-1,0): apply the general case. */
-      bMatrix[0][0] = 1 - x * x / theta;
-      bMatrix[2][2] = 1 - z * z / theta;
-      bMatrix[2][0] = bMatrix[0][2] = -x * z / theta;
-    }
-    else {
-      /* nor is close to negative Y axis (0,-1,0): apply the special case. */
-      bMatrix[0][0] = (x + z) * (x - z) / -theta_alt;
-      bMatrix[2][2] = -bMatrix[0][0];
-      bMatrix[2][0] = bMatrix[0][2] = 2.0f * x * z / theta_alt;
+    if (theta <= SAFE_THRESHOLD) {
+      /* When nor is close to negative Y axis (0,-1,0) the theta precision is very bad,
+       * so recompute it from x and z instead, using the series expansion for sqrt. */
+      theta = theta_alt * 0.5f + theta_alt * theta_alt * 0.125f;
     }
+
+    bMatrix[0][0] = 1 - x * x / theta;
+    bMatrix[2][2] = 1 - z * z / theta;
+    bMatrix[2][0] = bMatrix[0][2] = -x * z / theta;
   }
   else {
     /* nor is very close to negative Y axis (0,-1,0): use simple symmetry by Z axis. */
diff --git a/source/blender/blenkernel/intern/armature_test.cc b/source/blender/blenkernel/intern/armature_test.cc
index 8ebb91ffc74..3d22351e9a6 100644
--- a/source/blender/blenkernel/intern/armature_test.cc
+++ b/source/blender/blenkernel/intern/armature_test.cc
@@ -185,12 +185,12 @@ static double find_flip_boundary(double x, double z)
 
 TEST(vec_roll_to_mat3_normalized, FlippedBoundary1)
 {
-  EXPECT_NEAR(find_flip_boundary(0, 1), 2.40e-4, 0.01e-4);
+  EXPECT_NEAR(find_flip_boundary(0, 1), 2.50e-4, 0.01e-4);
 }
 
 TEST(vec_roll_to_mat3_normalized, FlippedBoundary2)
 {
-  EXPECT_NEAR(find_flip_boundary(1, 1), 3.39e-4, 0.01e-4);
+  EXPECT_NEAR(find_flip_boundary(1, 1), 2.50e-4, 0.01e-4);
 }
 
 /* Test cases close to the -Y axis. */
@@ -218,9 +218,9 @@ TEST(vec_roll_to_mat3_normalized, Flipped3)
 {
   /* If normalized_vector is in a critical range close to -Y, apply the special case. */
   const float input[3] = {2.5e-4f, -0.999999881f, 2.5e-4f}; /* Corner Case. */
-  const float expected_roll_mat[3][3] = {{0.000000f, -2.5e-4f, 1.000000f},
+  const float expected_roll_mat[3][3] = {{0.000000f, -2.5e-4f, -1.000000f},
                                          {2.5e-4f, -0.999999881f, 2.5e-4f},
-                                         {1.000000f, -2.5e-4f, 0.000000f}};
+                                         {-1.000000f, -2.5e-4f, 0.000000f}};
   test_vec_roll_to_mat3_normalized(input, 0.0f, expected_roll_mat, false);
 }
 
@@ -304,6 +304,65 @@ TEST(vec_roll_to_mat3_normalized, Roll1)
   test_vec_roll_to_mat3_normalized(input, float(M_PI * 0.5), expected_roll_mat);
 }
 
+/** Test that the matrix is orthogonal for an input close to -Y. */
+static double test_vec_roll_to_mat3_orthogonal(double s, double x, double z)
+{
+  const float input[3] = {float(x), float(s * sqrt(1 - x * x - z * z)), float(z)};
+
+  return test_vec_roll_to_mat3_normalized(input, 0.0f, NULL);
+}
+
+/** Test that the matrix is orthogonal for a range of inputs close to -Y. */
+static void test_vec_roll_to_mat3_orthogonal(double s, double x1, double x2, double y1, double y2)
+{
+  const int count = 5000;
+  double delta = 0;
+  double tmax = 0;
+
+  for (int i = 0; i <= count; i++) {
+    double t = double(i) / count;
+    double det = test_vec_roll_to_mat3_orthogonal(s, interpd(x2, x1, t), interpd(y2, y1, t));
+
+    /* Find and report maximum error in the matrix determinant. */
+    double curdelta = abs(det - 1);
+    if (curdelta > delta) {
+      delta = curdelta;
+      tmax = t;
+    }
+  }
+
+  printf("             Max determinant error %.10f at %f.\n", delta, tmax);
+}
+
+#define TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(name, s, x1, x2, y1, y2) \
+  TEST(vec_roll_to_mat3_normalized, name) \
+  { \
+    test_vec_roll_to_mat3_orthogonal(s, x1, x2, y1, y2); \
+  }
+
+/* Moving from -Y towards X. */
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_000_005, -1, 0, 0, 3e-4, 0.005)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_000_010, -1, 0, 0, 0.005, 0.010)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_000_050, -1, 0, 0, 0.010, 0.050)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_000_100, -1, 0, 0, 0.050, 0.100)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_000_200, -1, 0, 0, 0.100, 0.200)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_000_300, -1, 0, 0, 0.200, 0.300)
+
+/* Moving from -Y towards X and Y. */
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_005_005, -1, 3e-4, 0.005, 3e-4, 0.005)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_010_010, -1, 0.005, 0.010, 0.005, 0.010)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_050_050, -1, 0.010, 0.050, 0.010, 0.050)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_100_100, -1, 0.050, 0.100, 0.050, 0.100)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoN_200_200, -1, 0.100, 0.200, 0.100, 0.200)
+
+/* Moving from +Y towards X. */
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoP_000_005, 1, 0, 0, 0, 0.005)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoP_000_100, 1, 0, 0, 0.005, 0.100)
+
+/* Moving from +Y towards X and Y. */
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoP_005_005, 1, 0, 0.005, 0, 0.005)
+TEST_VEC_ROLL_TO_MAT3_ORTHOGONAL(OrthoP_100_100, 1, 0.005, 0.100, 0.005, 0.100)
+
 class BKE_armature_find_selected_bones_test : public testing::Test {
  protected:
   bArmature arm;