diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 605035c2143..58a4ec5b74d 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -141,16 +141,24 @@ struct [[nodiscard]] Quaternion { } Quaternion(const Vector3 &p_v0, const Vector3 &p_v1) { // Shortest arc. - Vector3 c = p_v0.cross(p_v1); - - if (c.is_zero_approx()) { - Vector3 axis = p_v0.get_any_perpendicular(); +#ifdef MATH_CHECKS + ERR_FAIL_COND_MSG(p_v0.is_zero_approx() || p_v1.is_zero_approx(), "The vectors must not be zero."); +#endif + constexpr real_t ALMOST_ONE = 1.0f - (real_t)CMP_EPSILON; + Vector3 n0 = p_v0.normalized(); + Vector3 n1 = p_v1.normalized(); + real_t d = n0.dot(n1); + if (abs(d) > ALMOST_ONE) { + if (d >= 0) { + return; // Vectors are same. + } + Vector3 axis = n0.get_any_perpendicular(); x = axis.x; y = axis.y; z = axis.z; w = 0; } else { - real_t d = p_v0.dot(p_v1); + Vector3 c = n0.cross(n1); real_t s = Math::sqrt((1.0f + d) * 2.0f); real_t rs = 1.0f / s; diff --git a/tests/core/math/test_quaternion.h b/tests/core/math/test_quaternion.h index 003cb17ea98..60194daa041 100644 --- a/tests/core/math/test_quaternion.h +++ b/tests/core/math/test_quaternion.h @@ -263,6 +263,9 @@ TEST_CASE("[Quaternion] Construct Shortest Arc For 180 Degree Arc") { // For the consistency of the rotation direction, they should be symmetrical to the plane. CHECK(left_to_right.is_equal_approx(right_to_left.inverse())); + + // If vectors are same, no rotation. + CHECK(Quaternion(diagonal_up, diagonal_up).is_equal_approx(Quaternion())); } TEST_CASE("[Quaternion] Get Euler Orders") {