diff options
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 17 | ||||
| -rw-r--r-- | source/blender/blenlib/tests/BLI_math_rotation_test.cc | 20 |
2 files changed, 36 insertions, 1 deletions
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); +} |