Fix T83196: bad matrix to quaternion precision near 180 degrees rotation.

Adjust the threshold for switching from the base case to trace > 0,
based on very similar example code from www.euclideanspace.com to
avoid float precision issues when trace is close to -1.

Also, remove conversions to and from double, because using double
here doesn't really have benefit, especially with the new threshold.

Finally, add quaternion-matrix-quaternion round trip tests with
full coverage for all 4 branches.

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

1 files changed, 38 insertions, 28 deletions

diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index c8e84cee0a0..a0ee16bee76 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -320,47 +320,57 @@ void quat_to_mat4(float m[4][4], const float q[4])
 
 void mat3_normalized_to_quat(float q[4], const float mat[3][3])
 {
-  double tr, s;
-
   BLI_ASSERT_UNIT_M3(mat);
 
-  tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]);
+  /* 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;
 
-  if (tr > (double)1e-4f) {
-    s = sqrt(tr);
-    q[0] = (float)s;
-    s = 1.0 / (4.0 * s);
-    q[1] = (float)((double)(mat[1][2] - mat[2][1]) * s);
-    q[2] = (float)((double)(mat[2][0] - mat[0][2]) * s);
-    q[3] = (float)((double)(mat[0][1] - mat[1][0]) * 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]) {
-      s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
-      q[1] = (float)(0.25 * s);
+      float s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]);
+
+      q[1] = 0.25f * s;
 
-      s = 1.0 / s;
-      q[0] = (float)((double)(mat[1][2] - mat[2][1]) * s);
-      q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s);
-      q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s);
+      s = 1.0f / s;
+
+      q[0] = (mat[1][2] - mat[2][1]) * s;
+      q[2] = (mat[1][0] + mat[0][1]) * s;
+      q[3] = (mat[2][0] + mat[0][2]) * s;
     }
     else if (mat[1][1] > mat[2][2]) {
-      s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
-      q[2] = (float)(0.25 * s);
+      float s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]);
+
+      q[2] = 0.25f * s;
+
+      s = 1.0f / s;
 
-      s = 1.0 / s;
-      q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s);
-      q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s);
-      q[3] = (float)((double)(mat[2][1] + mat[1][2]) * 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;
     }
     else {
-      s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
-      q[3] = (float)(0.25 * s);
+      float s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]);
+
+      q[3] = 0.25f * s;
+
+      s = 1.0f / s;
 
-      s = 1.0 / s;
-      q[0] = (float)((double)(mat[0][1] - mat[1][0]) * s);
-      q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s);
-      q[2] = (float)((double)(mat[2][1] + mat[1][2]) * 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;
     }
   }