diff options
Diffstat (limited to 'core/math/basis.cpp')
| -rw-r--r-- | core/math/basis.cpp | 80 |
1 files changed, 53 insertions, 27 deletions
diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 7f60b7962c..9fcecd1ba6 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -76,9 +76,11 @@ void Basis::invert() { } void Basis::orthonormalize() { + #ifdef MATH_CHECKS ERR_FAIL_COND(determinant() == 0); #endif + // Gram-Schmidt Process Vector3 x = get_axis(0); @@ -118,16 +120,16 @@ bool Basis::is_diagonal() const { } bool Basis::is_rotation() const { - return Math::is_equal_approx(determinant(), 1) && is_orthogonal(); + return Math::is_equal_approx(determinant(), 1, UNIT_EPSILON) && is_orthogonal(); } bool Basis::is_symmetric() const { - if (!Math::is_equal_approx(elements[0][1], elements[1][0])) + if (!Math::is_equal_approx_ratio(elements[0][1], elements[1][0], UNIT_EPSILON)) return false; - if (!Math::is_equal_approx(elements[0][2], elements[2][0])) + if (!Math::is_equal_approx_ratio(elements[0][2], elements[2][0], UNIT_EPSILON)) return false; - if (!Math::is_equal_approx(elements[1][2], elements[2][1])) + if (!Math::is_equal_approx_ratio(elements[1][2], elements[2][1], UNIT_EPSILON)) return false; return true; @@ -488,6 +490,11 @@ void Basis::set_euler_xyz(const Vector3 &p_euler) { // as the x, y, and z components of a Vector3 respectively. Vector3 Basis::get_euler_yxz() const { + /* checking this is a bad idea, because obtaining from scaled transform is a valid use case +#ifdef MATH_CHECKS + ERR_FAIL_COND(!is_rotation()); +#endif +*/ // Euler angles in YXZ convention. // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix // @@ -496,9 +503,7 @@ Vector3 Basis::get_euler_yxz() const { // cy*sx*sz-cz*sy cy*cz*sx+sy*sz cy*cx Vector3 euler; -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(!is_rotation(), euler); -#endif + real_t m12 = elements[1][2]; if (m12 < 1) { @@ -552,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(a.elements[i][j], b.elements[i][j])) + 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; } } @@ -599,10 +616,16 @@ Basis::operator String() const { } Quat Basis::get_quat() const { + #ifdef MATH_CHECKS - ERR_FAIL_COND_V(!is_rotation(), Quat()); + 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 - real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; + /* Allow getting a quaternion from an unnormalized transform */ + Basis m = *this; + real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; real_t temp[4]; if (trace > 0.0) { @@ -610,23 +633,23 @@ Quat Basis::get_quat() const { temp[3] = (s * 0.5); s = 0.5 / s; - temp[0] = ((elements[2][1] - elements[1][2]) * s); - temp[1] = ((elements[0][2] - elements[2][0]) * s); - temp[2] = ((elements[1][0] - elements[0][1]) * s); + temp[0] = ((m.elements[2][1] - m.elements[1][2]) * s); + temp[1] = ((m.elements[0][2] - m.elements[2][0]) * s); + temp[2] = ((m.elements[1][0] - m.elements[0][1]) * s); } else { - int i = elements[0][0] < elements[1][1] ? - (elements[1][1] < elements[2][2] ? 2 : 1) : - (elements[0][0] < elements[2][2] ? 2 : 0); + int i = m.elements[0][0] < m.elements[1][1] ? + (m.elements[1][1] < m.elements[2][2] ? 2 : 1) : + (m.elements[0][0] < m.elements[2][2] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3; - real_t s = Math::sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0); + real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0); temp[i] = s * 0.5; s = 0.5 / s; - temp[3] = (elements[k][j] - elements[j][k]) * s; - temp[j] = (elements[j][i] + elements[i][j]) * s; - temp[k] = (elements[k][i] + elements[i][k]) * s; + temp[3] = (m.elements[k][j] - m.elements[j][k]) * s; + temp[j] = (m.elements[j][i] + m.elements[i][j]) * s; + temp[k] = (m.elements[k][i] + m.elements[i][k]) * s; } return Quat(temp[0], temp[1], temp[2], temp[3]); @@ -696,9 +719,11 @@ void Basis::set_orthogonal_index(int p_index) { } void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { + /* checking this is a bad idea, because obtaining from scaled transform is a valid use case #ifdef MATH_CHECKS ERR_FAIL_COND(!is_rotation()); #endif +*/ real_t angle, x, y, z; // variables for result real_t epsilon = 0.01; // margin to allow for rounding errors real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees @@ -835,14 +860,15 @@ void Basis::set_diagonal(const Vector3 p_diag) { } Basis Basis::slerp(const Basis &target, const real_t &t) const { -// TODO: implement this directly without using quaternions to make it more efficient -#ifdef MATH_CHECKS - ERR_FAIL_COND_V(!is_rotation(), Basis()); - ERR_FAIL_COND_V(!target.is_rotation(), Basis()); -#endif + //consider scale Quat from(*this); Quat to(target); - return Basis(from.slerp(to, t)); + Basis b(from.slerp(to, t)); + b.elements[0] *= Math::lerp(elements[0].length(), target.elements[0].length(), t); + b.elements[1] *= Math::lerp(elements[1].length(), target.elements[1].length(), t); + b.elements[2] *= Math::lerp(elements[2].length(), target.elements[2].length(), t); + + return b; } |