From d56cbd374b66e0207f862afc0b0d3686f8adec27 Mon Sep 17 00:00:00 2001 From: Adam Date: Tue, 28 Jan 2025 21:41:31 -0600 Subject: [PATCH] Fix `Basis::get_euler` incorrectly simplifying rotations in some cases. --- core/math/basis.cpp | 29 +++++++++++++++++------------ tests/core/math/test_basis.h | 18 ++++++++++++------ 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/core/math/basis.cpp b/core/math/basis.cpp index e5f3431eef8..18959f3c016 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -455,6 +455,11 @@ void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) cons } Vector3 Basis::get_euler(EulerOrder p_order) const { + // This epsilon value results in angles within a +/- 0.04 degree range being simplified/truncated. + // Based on testing, this is the largest the epsilon can be without the angle truncation becoming + // visually noticeable. + const real_t epsilon = 0.00000025; + switch (p_order) { case EulerOrder::XYZ: { // Euler angles in XYZ convention. @@ -466,8 +471,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { Vector3 euler; real_t sy = rows[0][2]; - if (sy < (1.0f - (real_t)CMP_EPSILON)) { - if (sy > -(1.0f - (real_t)CMP_EPSILON)) { + if (sy < (1.0f - epsilon)) { + if (sy > -(1.0f - epsilon)) { // is this a pure Y rotation? if (rows[1][0] == 0 && rows[0][1] == 0 && rows[1][2] == 0 && rows[2][1] == 0 && rows[1][1] == 1) { // return the simplest form (human friendlier in editor and scripts) @@ -501,8 +506,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { Vector3 euler; real_t sz = rows[0][1]; - if (sz < (1.0f - (real_t)CMP_EPSILON)) { - if (sz > -(1.0f - (real_t)CMP_EPSILON)) { + if (sz < (1.0f - epsilon)) { + if (sz > -(1.0f - epsilon)) { euler.x = Math::atan2(rows[2][1], rows[1][1]); euler.y = Math::atan2(rows[0][2], rows[0][0]); euler.z = Math::asin(-sz); @@ -532,8 +537,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { real_t m12 = rows[1][2]; - if (m12 < (1 - (real_t)CMP_EPSILON)) { - if (m12 > -(1 - (real_t)CMP_EPSILON)) { + if (m12 < (1 - epsilon)) { + if (m12 > -(1 - epsilon)) { // is this a pure X rotation? if (rows[1][0] == 0 && rows[0][1] == 0 && rows[0][2] == 0 && rows[2][0] == 0 && rows[0][0] == 1) { // return the simplest form (human friendlier in editor and scripts) @@ -568,8 +573,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { Vector3 euler; real_t sz = rows[1][0]; - if (sz < (1.0f - (real_t)CMP_EPSILON)) { - if (sz > -(1.0f - (real_t)CMP_EPSILON)) { + if (sz < (1.0f - epsilon)) { + if (sz > -(1.0f - epsilon)) { euler.x = Math::atan2(-rows[1][2], rows[1][1]); euler.y = Math::atan2(-rows[2][0], rows[0][0]); euler.z = Math::asin(sz); @@ -596,8 +601,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // -cx*sy sx cx*cy Vector3 euler; real_t sx = rows[2][1]; - if (sx < (1.0f - (real_t)CMP_EPSILON)) { - if (sx > -(1.0f - (real_t)CMP_EPSILON)) { + if (sx < (1.0f - epsilon)) { + if (sx > -(1.0f - epsilon)) { euler.x = Math::asin(sx); euler.y = Math::atan2(-rows[2][0], rows[2][2]); euler.z = Math::atan2(-rows[0][1], rows[1][1]); @@ -624,8 +629,8 @@ Vector3 Basis::get_euler(EulerOrder p_order) const { // -sy cy*sx cy*cx Vector3 euler; real_t sy = rows[2][0]; - if (sy < (1.0f - (real_t)CMP_EPSILON)) { - if (sy > -(1.0f - (real_t)CMP_EPSILON)) { + if (sy < (1.0f - epsilon)) { + if (sy > -(1.0f - epsilon)) { euler.x = Math::atan2(rows[2][1], rows[2][2]); euler.y = Math::asin(-sy); euler.z = Math::atan2(rows[1][0], rows[0][0]); diff --git a/tests/core/math/test_basis.h b/tests/core/math/test_basis.h index f8c5ef279dc..6166986286e 100644 --- a/tests/core/math/test_basis.h +++ b/tests/core/math/test_basis.h @@ -93,9 +93,9 @@ void test_rotation(Vector3 deg_original_euler, EulerOrder rot_order) { Basis res = to_rotation.inverse() * rotation_from_computed_euler; - CHECK_MESSAGE((res.get_column(0) - Vector3(1.0, 0.0, 0.0)).length() <= 0.1, vformat("Fail due to X %s\n", String(res.get_column(0)))); - CHECK_MESSAGE((res.get_column(1) - Vector3(0.0, 1.0, 0.0)).length() <= 0.1, vformat("Fail due to Y %s\n", String(res.get_column(1)))); - CHECK_MESSAGE((res.get_column(2) - Vector3(0.0, 0.0, 1.0)).length() <= 0.1, vformat("Fail due to Z %s\n", String(res.get_column(2)))); + CHECK_MESSAGE((res.get_column(0) - Vector3(1.0, 0.0, 0.0)).length() <= 0.001, vformat("Fail due to X %s\n", String(res.get_column(0)))); + CHECK_MESSAGE((res.get_column(1) - Vector3(0.0, 1.0, 0.0)).length() <= 0.001, vformat("Fail due to Y %s\n", String(res.get_column(1)))); + CHECK_MESSAGE((res.get_column(2) - Vector3(0.0, 0.0, 1.0)).length() <= 0.001, vformat("Fail due to Z %s\n", String(res.get_column(2)))); // Double check `to_rotation` decomposing with XYZ rotation order. const Vector3 euler_xyz_from_rotation = to_rotation.get_euler(EulerOrder::XYZ); @@ -103,9 +103,9 @@ void test_rotation(Vector3 deg_original_euler, EulerOrder rot_order) { res = to_rotation.inverse() * rotation_from_xyz_computed_euler; - CHECK_MESSAGE((res.get_column(0) - Vector3(1.0, 0.0, 0.0)).length() <= 0.1, vformat("Double check with XYZ rot order failed, due to X %s\n", String(res.get_column(0)))); - CHECK_MESSAGE((res.get_column(1) - Vector3(0.0, 1.0, 0.0)).length() <= 0.1, vformat("Double check with XYZ rot order failed, due to Y %s\n", String(res.get_column(1)))); - CHECK_MESSAGE((res.get_column(2) - Vector3(0.0, 0.0, 1.0)).length() <= 0.1, vformat("Double check with XYZ rot order failed, due to Z %s\n", String(res.get_column(2)))); + CHECK_MESSAGE((res.get_column(0) - Vector3(1.0, 0.0, 0.0)).length() <= 0.001, vformat("Double check with XYZ rot order failed, due to X %s\n", String(res.get_column(0)))); + CHECK_MESSAGE((res.get_column(1) - Vector3(0.0, 1.0, 0.0)).length() <= 0.001, vformat("Double check with XYZ rot order failed, due to Y %s\n", String(res.get_column(1)))); + CHECK_MESSAGE((res.get_column(2) - Vector3(0.0, 0.0, 1.0)).length() <= 0.001, vformat("Double check with XYZ rot order failed, due to Z %s\n", String(res.get_column(2)))); INFO(vformat("Rotation order: %s\n.", get_rot_order_name(rot_order))); INFO(vformat("Original Rotation: %s\n", String(deg_original_euler))); @@ -176,6 +176,12 @@ TEST_CASE("[Basis] Euler conversions") { vectors_to_test.push_back(Vector3(120.0, -150.0, -130.0)); vectors_to_test.push_back(Vector3(120.0, 150.0, -130.0)); vectors_to_test.push_back(Vector3(120.0, 150.0, 130.0)); + vectors_to_test.push_back(Vector3(89.9, 0.0, 0.0)); + vectors_to_test.push_back(Vector3(-89.9, 0.0, 0.0)); + vectors_to_test.push_back(Vector3(0.0, 89.9, 0.0)); + vectors_to_test.push_back(Vector3(0.0, -89.9, 0.0)); + vectors_to_test.push_back(Vector3(0.0, 0.0, 89.9)); + vectors_to_test.push_back(Vector3(0.0, 0.0, -89.9)); for (int h = 0; h < euler_order_to_test.size(); h += 1) { for (int i = 0; i < vectors_to_test.size(); i += 1) {