[Bf-blender-cvs] [9c0df8e2753] master: Fix weird Swing+Twist decomposition with noncanonical quaternions.

[Alexander Gavrilov]

Commit: 9c0df8e27533657f49919e45d46c85c847048d8f
Author: Alexander Gavrilov
Date:   Sun Dec 13 21:41:08 2020 +0300
Branches: master
https://developer.blender.org/rB9c0df8e27533657f49919e45d46c85c847048d8f

Fix weird Swing+Twist decomposition with noncanonical quaternions.

It turns out that after the fix to T83196 (rB814b2787cadd) the matrix
to quaternion conversion can produce noncanonical results in large
areas of the rotation space, when previously this was limited to
way smaller areas. This in turn causes Swing+Twist math to produce
angles beyond 180 degrees, e.g. outputting a -120..240 range.

This fixes both issues, ensuring that conversion outputs a canonical
result, and decomposition canonifies its input.

This was reported in chat by @jpbouza.

===================================================================

M	source/blender/blenlib/intern/math_rotation.c
M	source/blender/blenlib/tests/BLI_math_rotation_test.cc

===================================================================

diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index a0ee16bee76..19828e69638 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -372,6 +372,11 @@ void mat3_normalized_to_quat(float q[4], const float mat[3][3])
       q[1] = (mat[2][0] + mat[0][2]) * s;
       q[2] = (mat[2][1] + mat[1][2]) * s;
     }
+
+    /* Make sure w is nonnegative for a canonical result. */
+    if (q[0] < 0) {
+      negate_v4(q);
+    }
   }
 
   normalize_qt(q);
@@ -556,10 +561,20 @@ void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q
  * \param r_twist: if not NULL, receives the twist quaternion.
  * \returns twist angle.
  */
-float quat_split_swing_and_twist(const float q[4], int axis, float r_swing[4], float r_twist[4])
+float quat_split_swing_and_twist(const float q_in[4], int axis, float r_swing[4], float r_twist[4])
 {
   BLI_assert(axis >= 0 && axis <= 2);
 
+  /* The calculation requires a canonical quaternion. */
+  float q[4];
+
+  if (q_in[0] < 0) {
+    negate_v4_v4(q, q_in);
+  }
+  else {
+    copy_v4_v4(q, q_in);
+  }
+
   /* Half-twist angle can be computed directly. */
   float t = atan2f(q[axis + 1], q[0]);
 
diff --git a/source/blender/blenlib/tests/BLI_math_rotation_test.cc b/source/blender/blenlib/tests/BLI_math_rotation_test.cc
index 02257ba83dd..90fd4f8c2e6 100644
--- a/source/blender/blenlib/tests/BLI_math_rotation_test.cc
+++ b/source/blender/blenlib/tests/BLI_math_rotation_test.cc
@@ -71,6 +71,12 @@ TEST(math_rotation, quat_to_mat_to_quat_bad_T83196)
   test_quat_to_mat_to_quat(0.0149f, 0.9996f, -0.0212f, -0.0107f);
 }
 
+TEST(math_rotation, quat_to_mat_to_quat_bad_negative)
+{
+  /* This shouldn't produce a negative q[0]. */
+  test_quat_to_mat_to_quat(0.5f - 1e-6f, 0, -sqrtf(3) / 2 - 1e-6f, 0);
+}
+
 TEST(math_rotation, quat_to_mat_to_quat_near_1000)
 {
   test_quat_to_mat_to_quat(0.9999f, 0.01f, -0.001f, -0.01f);
@@ -126,3 +132,17 @@ TEST(math_rotation, quat_to_mat_to_quat_near_0001)
   test_quat_to_mat_to_quat(0.25f, -0.025f, -0.25f, 0.97f);
   test_quat_to_mat_to_quat(0.30f, -0.030f, -0.30f, 0.95f);
 }
+
+TEST(math_rotation, quat_split_swing_and_twist_negative)
+{
+  const float input[4] = {-0.5f, 0, sqrtf(3) / 2, 0};
+  const float expected_swing[4] = {1.0f, 0, 0, 0};
+  const float expected_twist[4] = {0.5f, 0, -sqrtf(3) / 2, 0};
+  float swing[4], twist[4];
+
+  float twist_angle = quat_split_swing_and_twist(input, 1, swing, twist);
+
+  EXPECT_NEAR(twist_angle, -M_PI * 2 / 3, FLT_EPSILON);
+  EXPECT_V4_NEAR(swing, expected_swing, FLT_EPSILON);
+  EXPECT_V4_NEAR(twist, expected_twist, FLT_EPSILON);
+}