diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 8816e3639a3..82b2f7006da 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -557,11 +557,23 @@ void Basis::set_euler_yxz(const Vector3 &p_euler) { *this = ymat * xmat * zmat; } -bool Basis::is_equal_approx(const Basis &a, const Basis &b) const { +bool Basis::is_equal_approx(const Basis &a, const Basis &b,real_t p_epsilon) const { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], UNIT_EPSILON)) + if (!Math::is_equal_approx(a.elements[i][j], b.elements[i][j], p_epsilon)) + return false; + } + } + + return true; +} + +bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b,real_t p_epsilon) const { + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], p_epsilon)) return false; } } @@ -605,12 +617,14 @@ Basis::operator String() const { Quat Basis::get_quat() const { +#ifdef MATH_CHECKS + if (!is_rotation()) { + ERR_EXPLAIN("Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead."); + ERR_FAIL_V(Quat()); + } +#endif /* Allow getting a quaternion from an unnormalized transform */ Basis m = *this; - m.elements[0].normalize(); - m.elements[1].normalize(); - m.elements[2].normalize(); - real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; real_t temp[4]; diff --git a/core/math/basis.h b/core/math/basis.h index 128e56b494f..aa0ddb280fa 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -133,7 +133,8 @@ public: return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; } - bool is_equal_approx(const Basis &a, const Basis &b) const; + bool is_equal_approx(const Basis &a, const Basis &b, real_t p_epsilon=CMP_EPSILON) const; + bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon=UNIT_EPSILON) const; bool operator==(const Basis &p_matrix) const; bool operator!=(const Basis &p_matrix) const; diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 17112d89404..6ac68398274 100644 --- a/core/math/math_funcs.h +++ b/core/math/math_funcs.h @@ -249,11 +249,11 @@ public: static float random(float from, float to); static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); } - static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON) { + static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON, real_t min_epsilon = CMP_EPSILON) { // this is an approximate way to check that numbers are close, as a ratio of their average size // helps compare approximate numbers that may be very big or very small real_t diff = abs(a - b); - if (diff == 0.0) { + if (diff == 0.0 || diff < min_epsilon) { return true; } real_t avg_size = (abs(a) + abs(b)) / 2.0; diff --git a/core/variant_call.cpp b/core/variant_call.cpp index 25a0f3957c0..3812592639d 100644 --- a/core/variant_call.cpp +++ b/core/variant_call.cpp @@ -773,6 +773,8 @@ struct _VariantCall { VCALL_PTR0R(Basis, get_orthogonal_index); VCALL_PTR0R(Basis, orthonormalized); VCALL_PTR2R(Basis, slerp); + VCALL_PTR2R(Basis, is_equal_approx); + VCALL_PTR0R(Basis, get_rotation_quat); VCALL_PTR0R(Transform, inverse); VCALL_PTR0R(Transform, affine_inverse); @@ -1842,6 +1844,8 @@ void register_variant_methods() { ADDFUNC1R(BASIS, VECTOR3, Basis, xform_inv, VECTOR3, "v", varray()); ADDFUNC0R(BASIS, INT, Basis, get_orthogonal_index, varray()); ADDFUNC2R(BASIS, BASIS, Basis, slerp, BASIS, "b", REAL, "t", varray()); + ADDFUNC2R(BASIS, BOOL, Basis, is_equal_approx, BASIS, "b", REAL, "epsilon", varray(CMP_EPSILON)); + ADDFUNC0R(BASIS, QUAT, Basis, get_rotation_quat, varray()); ADDFUNC0R(TRANSFORM, TRANSFORM, Transform, inverse, varray()); ADDFUNC0R(TRANSFORM, TRANSFORM, Transform, affine_inverse, varray());