Fix T101848: Zeroed matrix converted to a quaternion results in rotation

Re-order checks to ensure a zeroed matrix results in a quaternion
without rotation. Also avoid some redundant calculation where the
'trace' was calculated but not used, flip the scaling value early
on instead of negating the quaternion after calculating it.

2 files changed, 59 insertions, 40 deletions

diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index ff45bbee5c9..17e43b545d8 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -275,63 +275,66 @@ void mat3_normalized_to_quat_fast(float q[4], const float mat[3][3])
   /* Caller must ensure matrices aren't negative for valid results, see: T24291, T94231. */
   BLI_assert(!is_negative_m3(mat));
 
-  /* Check the trace of the matrix - bad precision if close to -1. */
-  const float trace = mat[0][0] + mat[1][1] + mat[2][2];
-
-  if (trace > 0) {
-    float s = 2.0f * sqrtf(1.0f + trace);
-
-    q[0] = 0.25f * s;
-
-    s = 1.0f / s;
-
-    q[1] = (mat[1][2] - mat[2][1]) * s;
-    q[2] = (mat[2][0] - mat[0][2]) * s;
-    q[3] = (mat[0][1] - mat[1][0]) * s;
-  }
-  else {
-    /* Find the biggest diagonal element to choose the best formula.
-     * Here trace should also be always >= 0, avoiding bad precision. */
-    if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
-      float s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
-
+  /* Method outlined by Mike Day, ref: https://math.stackexchange.com/a/3183435/220949
+   * with an additional `sqrtf(..)` for higher precision result.
+   * Removing the `sqrt` causes tests to fail unless the precision is set to 1e-6 or larger. */
+
+  if (mat[2][2] < 0.0f) {
+    if (mat[0][0] > mat[1][1]) {
+      const float trace = 1.0f + mat[0][0] - mat[1][1] - mat[2][2];
+      float s = 2.0f * sqrtf(trace);
+      if (mat[1][2] < mat[2][1]) {
+        /* Ensure W is non-negative for a canonical result. */
+        s = -s;
+      }
       q[1] = 0.25f * s;
-
       s = 1.0f / s;
-
       q[0] = (mat[1][2] - mat[2][1]) * s;
-      q[2] = (mat[1][0] + mat[0][1]) * s;
+      q[2] = (mat[0][1] + mat[1][0]) * s;
       q[3] = (mat[2][0] + mat[0][2]) * s;
     }
-    else if (mat[1][1] > mat[2][2]) {
-      float s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
-
+    else {
+      const float trace = 1.0f - mat[0][0] + mat[1][1] - mat[2][2];
+      float s = 2.0f * sqrtf(trace);
+      if (mat[2][0] < mat[0][2]) {
+        /* Ensure W is non-negative for a canonical result. */
+        s = -s;
+      }
       q[2] = 0.25f * s;
-
       s = 1.0f / s;
-
       q[0] = (mat[2][0] - mat[0][2]) * s;
-      q[1] = (mat[1][0] + mat[0][1]) * s;
-      q[3] = (mat[2][1] + mat[1][2]) * s;
+      q[1] = (mat[0][1] + mat[1][0]) * s;
+      q[3] = (mat[1][2] + mat[2][1]) * s;
     }
-    else {
-      float s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
-
+  }
+  else {
+    if (mat[0][0] < -mat[1][1]) {
+      const float trace = 1.0f - mat[0][0] - mat[1][1] + mat[2][2];
+      float s = 2.0f * sqrtf(trace);
+      if (mat[0][1] < mat[1][0]) {
+        /* Ensure W is non-negative for a canonical result. */
+        s = -s;
+      }
       q[3] = 0.25f * s;
-
       s = 1.0f / s;
-
       q[0] = (mat[0][1] - mat[1][0]) * s;
       q[1] = (mat[2][0] + mat[0][2]) * s;
-      q[2] = (mat[2][1] + mat[1][2]) * s;
+      q[2] = (mat[1][2] + mat[2][1]) * s;
     }
-
-    /* Make sure W is non-negative for a canonical result. */
-    if (q[0] < 0) {
-      negate_v4(q);
+    else {
+      /* NOTE(@campbellbarton): A zero matrix will fall through to this block,
+       * needed so a zero scaled matrices to return a quaternion without rotation, see: T101848. */
+      const float trace = 1.0f + mat[0][0] + mat[1][1] + mat[2][2];
+      float s = 2.0f * sqrtf(trace);
+      q[0] = 0.25f * s;
+      s = 1.0f / s;
+      q[1] = (mat[1][2] - mat[2][1]) * s;
+      q[2] = (mat[2][0] - mat[0][2]) * s;
+      q[3] = (mat[0][1] - mat[1][0]) * s;
     }
   }
 
+  BLI_assert(!(q[0] < 0.0f));
   normalize_qt(q);
 }
 
diff --git a/source/blender/blenlib/tests/BLI_math_rotation_test.cc b/source/blender/blenlib/tests/BLI_math_rotation_test.cc
index e37b212e1df..0c8ae38c386 100644
--- a/source/blender/blenlib/tests/BLI_math_rotation_test.cc
+++ b/source/blender/blenlib/tests/BLI_math_rotation_test.cc
@@ -3,6 +3,7 @@
 #include "testing/testing.h"
 
 #include "BLI_math_base.h"
+#include "BLI_math_matrix.h"
 #include "BLI_math_rotation.h"
 #include "BLI_math_rotation.hh"
 #include "BLI_math_vector.hh"
@@ -138,6 +139,21 @@ TEST(math_rotation, quat_to_mat_to_quat_near_0001)
   test_quat_to_mat_to_quat(0.30f, -0.030f, -0.30f, 0.95f);
 }
 
+/* A zeroed matrix converted to a quaternion and back should not add rotation, see: T101848 */
+TEST(math_rotation, quat_to_mat_to_quat_zeroed_matrix)
+{
+  float matrix_zeroed[3][3] = {{0.0f}};
+  float matrix_result[3][3];
+  float matrix_unit[3][3];
+  float out_quat[4];
+
+  unit_m3(matrix_unit);
+  mat3_normalized_to_quat(out_quat, matrix_zeroed);
+  quat_to_mat3(matrix_result, out_quat);
+
+  EXPECT_M3_NEAR(matrix_unit, matrix_result, FLT_EPSILON);
+}
+
 TEST(math_rotation, quat_split_swing_and_twist_negative)
 {
   const float input[4] = {-0.5f, 0, sqrtf(3) / 2, 0};