diff options
-rw-r--r-- | source/blender/blenlib/CMakeLists.txt | 1 | ||||
-rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 66 | ||||
-rw-r--r-- | source/blender/blenlib/tests/BLI_math_rotation_test.cc | 128 |
3 files changed, 167 insertions, 28 deletions
diff --git a/source/blender/blenlib/CMakeLists.txt b/source/blender/blenlib/CMakeLists.txt index bcc8c181af4..9736d8f9b3b 100644 --- a/source/blender/blenlib/CMakeLists.txt +++ b/source/blender/blenlib/CMakeLists.txt @@ -400,6 +400,7 @@ if(WITH_GTESTS) tests/BLI_math_color_test.cc tests/BLI_math_geom_test.cc tests/BLI_math_matrix_test.cc + tests/BLI_math_rotation_test.cc tests/BLI_math_solvers_test.cc tests/BLI_math_vector_test.cc tests/BLI_memiter_test.cc 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; } } diff --git a/source/blender/blenlib/tests/BLI_math_rotation_test.cc b/source/blender/blenlib/tests/BLI_math_rotation_test.cc new file mode 100644 index 00000000000..1b1c4ef24a1 --- /dev/null +++ b/source/blender/blenlib/tests/BLI_math_rotation_test.cc @@ -0,0 +1,128 @@ +/* Apache License, Version 2.0 */ + +#include "testing/testing.h" + +#include "BLI_math_rotation.h" + +#include <math.h> + +/* Test that quaternion converts to itself via matrix. */ +static void test_quat_to_mat_to_quat(float w, float x, float y, float z) +{ + float in_quat[4] = {w, x, y, z}; + float norm_quat[4], matrix[3][3], out_quat[4]; + + normalize_qt_qt(norm_quat, in_quat); + quat_to_mat3(matrix, norm_quat); + mat3_normalized_to_quat(out_quat, matrix); + + /* The expected result is flipped (each orientation corresponds to 2 quats) */ + if (w < 0) { + mul_qt_fl(norm_quat, -1); + } + + EXPECT_V4_NEAR(norm_quat, out_quat, FLT_EPSILON); +} + +TEST(math_rotation, quat_to_mat_to_quat_rot180) +{ + test_quat_to_mat_to_quat(1, 0, 0, 0); + test_quat_to_mat_to_quat(0, 1, 0, 0); + test_quat_to_mat_to_quat(0, 0, 1, 0); + test_quat_to_mat_to_quat(0, 0, 0, 1); +} + +TEST(math_rotation, quat_to_mat_to_quat_rot180n) +{ + test_quat_to_mat_to_quat(-1.000f, 0, 0, 0); + test_quat_to_mat_to_quat(-1e-20f, -1, 0, 0); + test_quat_to_mat_to_quat(-1e-20f, 0, -1, 0); + test_quat_to_mat_to_quat(-1e-20f, 0, 0, -1); +} + +TEST(math_rotation, quat_to_mat_to_quat_rot90) +{ + const float s2 = 1 / sqrtf(2); + test_quat_to_mat_to_quat(s2, s2, 0, 0); + test_quat_to_mat_to_quat(s2, -s2, 0, 0); + test_quat_to_mat_to_quat(s2, 0, s2, 0); + test_quat_to_mat_to_quat(s2, 0, -s2, 0); + test_quat_to_mat_to_quat(s2, 0, 0, s2); + test_quat_to_mat_to_quat(s2, 0, 0, -s2); +} + +TEST(math_rotation, quat_to_mat_to_quat_rot90n) +{ + const float s2 = 1 / sqrtf(2); + test_quat_to_mat_to_quat(-s2, s2, 0, 0); + test_quat_to_mat_to_quat(-s2, -s2, 0, 0); + test_quat_to_mat_to_quat(-s2, 0, s2, 0); + test_quat_to_mat_to_quat(-s2, 0, -s2, 0); + test_quat_to_mat_to_quat(-s2, 0, 0, s2); + test_quat_to_mat_to_quat(-s2, 0, 0, -s2); +} + +TEST(math_rotation, quat_to_mat_to_quat_bad_T83196) +{ + test_quat_to_mat_to_quat(0.0032f, 0.9999f, -0.0072f, -0.0100f); + test_quat_to_mat_to_quat(0.0058f, 0.9999f, -0.0090f, -0.0101f); + test_quat_to_mat_to_quat(0.0110f, 0.9998f, -0.0140f, -0.0104f); + test_quat_to_mat_to_quat(0.0142f, 0.9997f, -0.0192f, -0.0107f); + test_quat_to_mat_to_quat(0.0149f, 0.9996f, -0.0212f, -0.0107f); +} + +TEST(math_rotation, quat_to_mat_to_quat_near_1000) +{ + test_quat_to_mat_to_quat(0.9999f, 0.01f, -0.001f, -0.01f); + test_quat_to_mat_to_quat(0.9999f, 0.02f, -0.002f, -0.02f); + test_quat_to_mat_to_quat(0.9999f, 0.03f, -0.003f, -0.03f); + test_quat_to_mat_to_quat(0.9999f, 0.04f, -0.004f, -0.04f); + test_quat_to_mat_to_quat(0.9999f, 0.05f, -0.005f, -0.05f); + test_quat_to_mat_to_quat(0.999f, 0.10f, -0.010f, -0.10f); + test_quat_to_mat_to_quat(0.99f, 0.15f, -0.015f, -0.15f); + test_quat_to_mat_to_quat(0.98f, 0.20f, -0.020f, -0.20f); + test_quat_to_mat_to_quat(0.97f, 0.25f, -0.025f, -0.25f); + test_quat_to_mat_to_quat(0.95f, 0.30f, -0.030f, -0.30f); +} + +TEST(math_rotation, quat_to_mat_to_quat_near_0100) +{ + test_quat_to_mat_to_quat(0.01f, 0.9999f, -0.001f, -0.01f); + test_quat_to_mat_to_quat(0.02f, 0.9999f, -0.002f, -0.02f); + test_quat_to_mat_to_quat(0.03f, 0.9999f, -0.003f, -0.03f); + test_quat_to_mat_to_quat(0.04f, 0.9999f, -0.004f, -0.04f); + test_quat_to_mat_to_quat(0.05f, 0.9999f, -0.005f, -0.05f); + test_quat_to_mat_to_quat(0.10f, 0.999f, -0.010f, -0.10f); + test_quat_to_mat_to_quat(0.15f, 0.99f, -0.015f, -0.15f); + test_quat_to_mat_to_quat(0.20f, 0.98f, -0.020f, -0.20f); + test_quat_to_mat_to_quat(0.25f, 0.97f, -0.025f, -0.25f); + test_quat_to_mat_to_quat(0.30f, 0.95f, -0.030f, -0.30f); +} + +TEST(math_rotation, quat_to_mat_to_quat_near_0010) +{ + test_quat_to_mat_to_quat(0.01f, -0.001f, 0.9999f, -0.01f); + test_quat_to_mat_to_quat(0.02f, -0.002f, 0.9999f, -0.02f); + test_quat_to_mat_to_quat(0.03f, -0.003f, 0.9999f, -0.03f); + test_quat_to_mat_to_quat(0.04f, -0.004f, 0.9999f, -0.04f); + test_quat_to_mat_to_quat(0.05f, -0.005f, 0.9999f, -0.05f); + test_quat_to_mat_to_quat(0.10f, -0.010f, 0.999f, -0.10f); + test_quat_to_mat_to_quat(0.15f, -0.015f, 0.99f, -0.15f); + test_quat_to_mat_to_quat(0.20f, -0.020f, 0.98f, -0.20f); + test_quat_to_mat_to_quat(0.25f, -0.025f, 0.97f, -0.25f); + test_quat_to_mat_to_quat(0.30f, -0.030f, 0.95f, -0.30f); +} + +TEST(math_rotation, quat_to_mat_to_quat_near_0001) +{ + test_quat_to_mat_to_quat(0.01f, -0.001f, -0.01f, 0.9999f); + test_quat_to_mat_to_quat(0.02f, -0.002f, -0.02f, 0.9999f); + test_quat_to_mat_to_quat(0.03f, -0.003f, -0.03f, 0.9999f); + test_quat_to_mat_to_quat(0.04f, -0.004f, -0.04f, 0.9999f); + test_quat_to_mat_to_quat(0.05f, -0.005f, -0.05f, 0.9999f); + test_quat_to_mat_to_quat(0.10f, -0.010f, -0.10f, 0.999f); + test_quat_to_mat_to_quat(0.15f, -0.015f, -0.15f, 0.99f); + test_quat_to_mat_to_quat(0.20f, -0.020f, -0.20f, 0.98f); + 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); +} |