BLI_math: ensure non-negative matrices for mat3_to_quat calculations

Making the callers responsible for this isn't practical as matrices are
often passed indirectly to a functions such as mat3_to_axis_angle,
BKE_object_mat3_to_rot & BKE_pchan_mat3_to_rot.
Or the matrix is combined from other matrices which could be negative.

Given quaternions calculated from negative matrices are completely
invalid and checking only needs to negate matrices with a negative
determinant, move the check into mat3_to_quat and related functions.

Add mat3_normalized_to_quat_fast for cases no error checking on the
input matrix is needed such as blending rotations.

2 files changed, 38 insertions, 29 deletions

diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c
index c4c9b9e3d01..e96b12033a9 100644
--- a/source/blender/blenlib/intern/math_matrix.c
+++ b/source/blender/blenlib/intern/math_matrix.c
@@ -2239,11 +2239,6 @@ void mat4_to_loc_quat(float loc[3], float quat[4], const float wmat[4][4])
   copy_m3_m4(mat3, wmat);
   normalize_m3_m3(mat3_n, mat3);
 
-  /* So scale doesn't interfere with rotation T24291. */
-  if (is_negative_m3(mat3)) {
-    negate_m3(mat3_n);
-  }
-
   mat3_normalized_to_quat(quat, mat3_n);
   copy_v3_v3(loc, wmat[3]);
 }
@@ -2252,7 +2247,7 @@ void mat4_decompose(float loc[3], float quat[4], float size[3], const float wmat
 {
   float rot[3][3];
   mat4_to_loc_rot_size(loc, rot, size, wmat);
-  mat3_normalized_to_quat(quat, rot);
+  mat3_normalized_to_quat_fast(quat, rot);
 }
 
 /**
@@ -2391,8 +2386,8 @@ void blend_m3_m3m3(float out[3][3],
   mat3_to_rot_size(drot, dscale, dst);
   mat3_to_rot_size(srot, sscale, src);
 
-  mat3_normalized_to_quat(dquat, drot);
-  mat3_normalized_to_quat(squat, srot);
+  mat3_normalized_to_quat_fast(dquat, drot);
+  mat3_normalized_to_quat_fast(squat, srot);
 
   /* do blending */
   interp_qt_qtqt(fquat, dquat, squat, srcweight);
@@ -2417,8 +2412,8 @@ void blend_m4_m4m4(float out[4][4],
   mat4_to_loc_rot_size(dloc, drot, dscale, dst);
   mat4_to_loc_rot_size(sloc, srot, sscale, src);
 
-  mat3_normalized_to_quat(dquat, drot);
-  mat3_normalized_to_quat(squat, srot);
+  mat3_normalized_to_quat_fast(dquat, drot);
+  mat3_normalized_to_quat_fast(squat, srot);
 
   /* do blending */
   interp_v3_v3v3(floc, dloc, sloc, srcweight);
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 7ecc271fa2a..bbea95514e9 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -269,13 +269,11 @@ void quat_to_mat4(float m[4][4], const float q[4])
   m[3][3] = 1.0f;
 }
 
-void mat3_normalized_to_quat(float q[4], const float mat[3][3])
+void mat3_normalized_to_quat_fast(float q[4], const float mat[3][3])
 {
   BLI_ASSERT_UNIT_M3(mat);
-  /* Callers must ensure matrices have a positive determinant for valid results, see: T94231. */
-  BLI_assert_msg(!is_negative_m3(mat),
-                 "Matrix 'mat' must not be negative, the resulting quaternion will be invalid. "
-                 "The caller should call negate_m3(mat) if is_negative_m3(mat) returns true.");
+  /* 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];
@@ -336,30 +334,46 @@ void mat3_normalized_to_quat(float q[4], const float mat[3][3])
 
   normalize_qt(q);
 }
-void mat3_to_quat(float q[4], const float mat[3][3])
+
+static void mat3_normalized_to_quat_with_checks(float q[4], float mat[3][3])
 {
-  float unit_mat[3][3];
+  const float det = determinant_m3_array(mat);
+  if (UNLIKELY(!isfinite(det))) {
+    unit_m3(mat);
+  }
+  else if (UNLIKELY(det < 0.0f)) {
+    negate_m3(mat);
+  }
+  mat3_normalized_to_quat_fast(q, mat);
+}
 
-  /* work on a copy */
-  /* this is needed AND a 'normalize_qt' in the end */
-  normalize_m3_m3(unit_mat, mat);
-  mat3_normalized_to_quat(q, unit_mat);
+void mat3_normalized_to_quat(float q[4], const float mat[3][3])
+{
+  float unit_mat_abs[3][3];
+  copy_m3_m3(unit_mat_abs, mat);
+  mat3_normalized_to_quat_with_checks(q, unit_mat_abs);
 }
 
-void mat4_normalized_to_quat(float q[4], const float mat[4][4])
+void mat3_to_quat(float q[4], const float mat[3][3])
 {
-  float mat3[3][3];
+  float unit_mat_abs[3][3];
+  normalize_m3_m3(unit_mat_abs, mat);
+  mat3_normalized_to_quat_with_checks(q, unit_mat_abs);
+}
 
-  copy_m3_m4(mat3, mat);
-  mat3_normalized_to_quat(q, mat3);
+void mat4_normalized_to_quat(float q[4], const float mat[4][4])
+{
+  float unit_mat_abs[3][3];
+  copy_m3_m4(unit_mat_abs, mat);
+  mat3_normalized_to_quat_with_checks(q, unit_mat_abs);
 }
 
 void mat4_to_quat(float q[4], const float mat[4][4])
 {
-  float mat3[3][3];
-
-  copy_m3_m4(mat3, mat);
-  mat3_to_quat(q, mat3);
+  float unit_mat_abs[3][3];
+  copy_m3_m4(unit_mat_abs, mat);
+  normalize_m3(unit_mat_abs);
+  mat3_normalized_to_quat_with_checks(q, unit_mat_abs);
 }
 
 void mat3_to_quat_is_ok(float q[4], const float wmat[3][3])