From 6c9843e5cba5994e5a050d6dc4d554ff115ec549 Mon Sep 17 00:00:00 2001 From: Relintai Date: Sun, 11 Jun 2023 09:45:23 +0200 Subject: [PATCH] Ported: Test, refactor and fix a bug in Basis.get_axis_angle - fabriceci https://github.com/godotengine/godot/commit/9f1a57d48b296c607b76fdae30a78630065710e6 --- core/math/basis.cpp | 53 ++++++++++++++++--------------- main/tests/test_basis.cpp | 65 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 93 insertions(+), 25 deletions(-) diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 5e79f27b0..02a08d68d 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -914,29 +914,30 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { #ifdef MATH_CHECKS ERR_FAIL_COND(!is_rotation()); #endif -*/ - real_t angle, x, y, z; // variables for result - real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees + */ + + // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm + real_t x, y, z; // Variables for result. + if (Math::is_zero_approx(rows[0][1] - rows[1][0]) && Math::is_zero_approx(rows[0][2] - rows[2][0]) && Math::is_zero_approx(rows[1][2] - rows[2][1])) { + // Singularity found. + // First check for identity matrix which must have +1 for all terms in leading diagonal and zero in other terms. + if (is_diagonal() && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < 3 * CMP_EPSILON)) { + // This singularity is identity matrix so angle = 0. - if ((Math::abs(rows[1][0] - rows[0][1]) < CMP_EPSILON) && (Math::abs(rows[2][0] - rows[0][2]) < CMP_EPSILON) && (Math::abs(rows[2][1] - rows[1][2]) < CMP_EPSILON)) { - // singularity found - // first check for identity matrix which must have +1 for all terms - // in leading diagonaland zero in other terms - if ((Math::abs(rows[1][0] + rows[0][1]) < angle_epsilon) && (Math::abs(rows[2][0] + rows[0][2]) < angle_epsilon) && (Math::abs(rows[2][1] + rows[1][2]) < angle_epsilon) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < angle_epsilon)) { - // this singularity is identity matrix so angle = 0 r_axis = Vector3(0, 1, 0); r_angle = 0; return; } - // otherwise this singularity is angle = 180 - angle = Math_PI; + + // Otherwise this singularity is angle = 180 real_t xx = (rows[0][0] + 1) / 2; real_t yy = (rows[1][1] + 1) / 2; real_t zz = (rows[2][2] + 1) / 2; - real_t xy = (rows[1][0] + rows[0][1]) / 4; - real_t xz = (rows[2][0] + rows[0][2]) / 4; - real_t yz = (rows[2][1] + rows[1][2]) / 4; - if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term + real_t xy = (rows[0][1] + rows[1][0]) / 4; + real_t xz = (rows[0][2] + rows[2][0]) / 4; + real_t yz = (rows[1][2] + rows[2][1]) / 4; + + if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term. if (xx < CMP_EPSILON) { x = 0; y = Math_SQRT12; @@ -946,7 +947,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { y = xy / x; z = xz / x; } - } else if (yy > zz) { // rows[1][1] is the largest diagonal term + } else if (yy > zz) { // rows[1][1] is the largest diagonal term. if (yy < CMP_EPSILON) { x = Math_SQRT12; y = 0; @@ -956,7 +957,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { x = xy / y; z = yz / y; } - } else { // rows[2][2] is the largest diagonal term so base result on this + } else { // rows[2][2] is the largest diagonal term so base result on this. if (zz < CMP_EPSILON) { x = Math_SQRT12; y = Math_SQRT12; @@ -968,23 +969,25 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { } } r_axis = Vector3(x, y, z); - r_angle = angle; + r_angle = Math_PI; return; } - // as we have reached here there are no singularities so we can handle normally - real_t s = Math::sqrt((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise - // acos does clamping. - angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2); - if (angle < 0) { - s = -s; + // As we have reached here there are no singularities so we can handle normally + real_t s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalise. + + if (Math::abs(s) < CMP_EPSILON) { + // Prevent divide by zero, should not happen if matrix is orthogonal and should be caught by singularity test above. + s = 1; } + x = (rows[2][1] - rows[1][2]) / s; y = (rows[0][2] - rows[2][0]) / s; z = (rows[1][0] - rows[0][1]) / s; r_axis = Vector3(x, y, z); - r_angle = angle; + // acos does clamping. + r_angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2); } void Basis::set_quaternion(const Quaternion &p_quat) { diff --git a/main/tests/test_basis.cpp b/main/tests/test_basis.cpp index bebd8f551..1678fde01 100644 --- a/main/tests/test_basis.cpp +++ b/main/tests/test_basis.cpp @@ -315,10 +315,75 @@ void test_euler_conversion() { } } +void check_test(std::string test_case_name, bool condition) { + if (!condition) { + OS::get_singleton()->print("FAILED - %s\n", test_case_name.c_str()); + } else { + OS::get_singleton()->print("PASSED - %s\n", test_case_name.c_str()); + } +} + +void test_set_axis_angle() { + Vector3 axis; + real_t angle; + real_t pi = (real_t)Math_PI; + + // Testing the singularity when the angle is 0°. + Basis identity(1, 0, 0, 0, 1, 0, 0, 0, 1); + identity.get_axis_angle(axis, angle); + check_test("Testing the singularity when the angle is 0.", angle == 0); + + // Testing the singularity when the angle is 180°. + Basis singularityPi(-1, 0, 0, 0, 1, 0, 0, 0, -1); + singularityPi.get_axis_angle(axis, angle); + check_test("Testing the singularity when the angle is 180.", Math::is_equal_approx(angle, pi)); + + // Testing reversing the an axis (of an 30° angle). + float cos30deg = Math::cos(Math::deg2rad((real_t)30.0)); + Basis z_positive(cos30deg, -0.5, 0, 0.5, cos30deg, 0, 0, 0, 1); + Basis z_negative(cos30deg, 0.5, 0, -0.5, cos30deg, 0, 0, 0, 1); + + z_positive.get_axis_angle(axis, angle); + check_test("Testing reversing the an axis (of an 30 angle).", Math::is_equal_approx(angle, Math::deg2rad((real_t)30.0))); + check_test("Testing reversing the an axis (of an 30 angle).", axis == Vector3(0, 0, 1)); + + z_negative.get_axis_angle(axis, angle); + check_test("Testing reversing the an axis (of an 30 angle).", Math::is_equal_approx(angle, Math::deg2rad((real_t)30.0))); + check_test("Testing reversing the an axis (of an 30 angle).", axis == Vector3(0, 0, -1)); + + // Testing a rotation of 90° on x-y-z. + Basis x90deg(1, 0, 0, 0, 0, -1, 0, 1, 0); + x90deg.get_axis_angle(axis, angle); + check_test("Testing a rotation of 90 on x-y-z.", Math::is_equal_approx(angle, pi / (real_t)2)); + check_test("Testing a rotation of 90 on x-y-z.", axis == Vector3(1, 0, 0)); + + Basis y90deg(0, 0, 1, 0, 1, 0, -1, 0, 0); + y90deg.get_axis_angle(axis, angle); + check_test("Testing a rotation of 90 on x-y-z.", axis == Vector3(0, 1, 0)); + + Basis z90deg(0, -1, 0, 1, 0, 0, 0, 0, 1); + z90deg.get_axis_angle(axis, angle); + check_test("Testing a rotation of 90 on x-y-z.", axis == Vector3(0, 0, 1)); + + // Regression test: checks that the method returns a small angle (not 0). + Basis tiny(1, 0, 0, 0, 0.9999995, -0.001, 0, 001, 0.9999995); // The min angle possible with float is 0.001rad. + tiny.get_axis_angle(axis, angle); + check_test("Regression test: checks that the method returns a small angle (not 0).", Math::is_equal_approx(angle, (real_t)0.001, (real_t)0.0001)); + + // Regression test: checks that the method returns an angle which is a number (not NaN) + Basis bugNan(1.00000024, 0, 0.000100001693, 0, 1, 0, -0.000100009143, 0, 1.00000024); + bugNan.get_axis_angle(axis, angle); + check_test("Regression test: checks that the method returns an angle which is a number (not NaN)", !Math::is_nan(angle)); +} + MainLoop *test() { OS::get_singleton()->print("Start euler conversion checks.\n"); test_euler_conversion(); + OS::get_singleton()->print("\n---------------\n"); + OS::get_singleton()->print("Start set axis angle checks.\n"); + test_set_axis_angle(); + return nullptr; }