diff options
Diffstat (limited to 'core/math/basis.cpp')
| -rw-r--r-- | core/math/basis.cpp | 26 |
1 files changed, 20 insertions, 6 deletions
diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 8816e3639a..9fcecd1ba6 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]; |