diff --git a/core/math/basis.cpp b/core/math/basis.cpp index 5931ecb257..48bba25dc2 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.cpp @@ -54,7 +54,8 @@ void Basis::invert() { } void Basis::orthonormalize() { - // Gram-Schmidt Process + // Orthonormalizable check is done in Vector3 class below. + // Gram-Schmidt Process: Vector3 x = get_column(0); Vector3 y = get_column(1); diff --git a/core/math/plane.cpp b/core/math/plane.cpp index 74783b2a69..b23514f007 100644 --- a/core/math/plane.cpp +++ b/core/math/plane.cpp @@ -38,9 +38,15 @@ void Plane::set_normal(const Vector3 &p_normal) { } void Plane::normalize() { +#ifdef MATH_CHECKS + if (!is_finite()) { + WARN_PRINT("Plane cannot be normalized, the distance and the normal should be finite."); + } +#endif // MATH_CHECKS + real_t l = normal.length(); if (l == 0) { - *this = Plane(0, 0, 0, 0); + zero(); return; } normal /= l; diff --git a/core/math/plane.h b/core/math/plane.h index 6bce4e773f..a440e8c4c0 100644 --- a/core/math/plane.h +++ b/core/math/plane.h @@ -45,6 +45,11 @@ struct [[nodiscard]] Plane { void set_normal(const Vector3 &p_normal); _FORCE_INLINE_ Vector3 get_normal() const { return normal; } + void zero() { + normal.zero(); + d = 0; + } + void normalize(); Plane normalized() const; diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 536682de8d..e4ae4027da 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -63,6 +63,11 @@ real_t Quaternion::length() const { } void Quaternion::normalize() { +#ifdef MATH_CHECKS + if (!is_finite()) { + WARN_PRINT("Quaternion cannot be normalized, the elements should be finite."); + } +#endif // MATH_CHECKS *this /= length(); } diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp index b5a971e7d9..611b6f0851 100644 --- a/core/math/transform_2d.cpp +++ b/core/math/transform_2d.cpp @@ -145,7 +145,8 @@ void Transform2D::translate_local(const Vector2 &p_translation) { } void Transform2D::orthonormalize() { - // Gram-Schmidt Process + // Orthonormalizable check is done in Vector2 class below. + // Gram-Schmidt Process: Vector2 x = columns[0]; Vector2 y = columns[1]; diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp index b4d02408ac..a5b825be03 100644 --- a/core/math/transform_3d.cpp +++ b/core/math/transform_3d.cpp @@ -150,6 +150,7 @@ Transform3D Transform3D::translated_local(const Vector3 &p_translation) const { } void Transform3D::orthonormalize() { + // Orthonormalizable check is done in Basis class below. basis.orthonormalize(); } diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp index 7a51c13945..581214e6e1 100644 --- a/core/math/vector2.cpp +++ b/core/math/vector2.cpp @@ -50,8 +50,18 @@ real_t Vector2::length_squared() const { } void Vector2::normalize() { - real_t l = x * x + y * y; - if (l != 0) { + if (!is_finite()) { +#ifdef MATH_CHECKS + WARN_PRINT("Vector2 cannot be normalized, the elements must be finite. Making (0, 0) as a fallback."); +#endif // MATH_CHECKS + zero(); + return; + } + + real_t l = length_squared(); + if (l == 0) { + zero(); + } else { l = Math::sqrt(l); x /= l; y /= l; diff --git a/core/math/vector2.h b/core/math/vector2.h index 98207474a3..5bb064523b 100644 --- a/core/math/vector2.h +++ b/core/math/vector2.h @@ -91,6 +91,8 @@ struct [[nodiscard]] Vector2 { real_t length_squared() const; Vector2 limit_length(real_t p_len = 1.0) const; + void zero() { x = y = 0; } + Vector2 min(const Vector2 &p_vector2) const { return Vector2(MIN(x, p_vector2.x), MIN(y, p_vector2.y)); } diff --git a/core/math/vector3.h b/core/math/vector3.h index b0f29e0d56..f83431b59c 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -546,14 +546,22 @@ real_t Vector3::length_squared() const { } void Vector3::normalize() { - real_t lengthsq = length_squared(); - if (lengthsq == 0) { - x = y = z = 0; + if (!is_finite()) { +#ifdef MATH_CHECKS + WARN_PRINT("Vector3 cannot be normalized, the elements must be finite. Making (0, 0, 0) as a fallback."); +#endif // MATH_CHECKS + zero(); + return; + } + + real_t l = length_squared(); + if (l == 0) { + zero(); } else { - real_t length = Math::sqrt(lengthsq); - x /= length; - y /= length; - z /= length; + l = Math::sqrt(l); + x /= l; + y /= l; + z /= l; } } diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp index e4fa1769c0..adf6053735 100644 --- a/core/math/vector4.cpp +++ b/core/math/vector4.cpp @@ -79,15 +79,23 @@ real_t Vector4::length() const { } void Vector4::normalize() { - real_t lengthsq = length_squared(); - if (lengthsq == 0) { - x = y = z = w = 0; + if (!is_finite()) { +#ifdef MATH_CHECKS + WARN_PRINT("Vector4 cannot be normalized, the elements must be finite. Making (0, 0, 0, 0) as a fallback."); +#endif // MATH_CHECKS + zero(); + return; + } + + real_t l = length_squared(); + if (l == 0) { + zero(); } else { - real_t length = Math::sqrt(lengthsq); - x /= length; - y /= length; - z /= length; - w /= length; + l = Math::sqrt(l); + x /= l; + y /= l; + z /= l; + w /= l; } } diff --git a/core/math/vector4.h b/core/math/vector4.h index edfe28b077..6b2fe70656 100644 --- a/core/math/vector4.h +++ b/core/math/vector4.h @@ -98,6 +98,8 @@ struct [[nodiscard]] Vector4 { Vector4 normalized() const; bool is_normalized() const; + void zero() { x = y = z = w = 0; } + real_t distance_to(const Vector4 &p_to) const; real_t distance_squared_to(const Vector4 &p_to) const; Vector4 direction_to(const Vector4 &p_to) const;