diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 08eac14b76d663..9721b59a8c9678 100644 --- a/core/math/quaternion.cpp +++ b/core/math/quaternion.cpp @@ -313,6 +313,42 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { } } +static bool _try_set_perpendicular_to(real_t p_in_x, real_t p_in_y, real_t p_in_z, real_t &r_out_x, real_t &r_out_y, real_t &r_out_z) { + real_t threshold = 1.0f / Math::sqrt(3.0f); + real_t abs_x = Math::abs(p_in_x); + if (abs_x > threshold + (real_t)CMP_EPSILON) { + return false; + } + + real_t length = Math::sqrt(1.0f - abs_x * abs_x); + r_out_x = 0; + r_out_y = p_in_z / length; + r_out_z = -p_in_y / length; + return true; +} + +Quaternion::Quaternion(const Vector3 &p_v0, const Vector3 &p_v1) { + Vector3 c = p_v0.cross(p_v1); + real_t d = p_v0.dot(p_v1); + + if (d < -1.0f + (real_t)CMP_EPSILON) { + // When given two vectors in opposite directions, produce a 180 degree + // arc around some arbitrary axis that is orthogonal to the given vectors. + // For backwards compatibility we prefer arcs that rotate around the y-axis + // when the parameter vectors lay on the XZ plane. + _try_set_perpendicular_to(p_v0.x, p_v0.y, p_v0.z, x, y, z) || _try_set_perpendicular_to(p_v0.z, p_v0.x, p_v0.y, z, x, y) || _try_set_perpendicular_to(p_v0.y, p_v0.z, p_v0.x, y, z, x); + w = 0; + } else { + real_t s = Math::sqrt((1.0f + d) * 2.0f); + real_t rs = 1.0f / s; + + x = c.x * rs; + y = c.y * rs; + z = c.z * rs; + w = s * 0.5f; + } +} + // Euler constructor expects a vector containing the Euler angles in the format // (ax, ay, az), where ax is the angle of rotation around x axis, // and similar for other axes. diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 868a2916f5b0d9..319627a4007003 100644 --- a/core/math/quaternion.h +++ b/core/math/quaternion.h @@ -140,25 +140,7 @@ struct _NO_DISCARD_ Quaternion { w = p_q.w; } - Quaternion(const Vector3 &p_v0, const Vector3 &p_v1) { // Shortest arc. - Vector3 c = p_v0.cross(p_v1); - real_t d = p_v0.dot(p_v1); - - if (d < -1.0f + (real_t)CMP_EPSILON) { - x = 0; - y = 1; - z = 0; - w = 0; - } else { - real_t s = Math::sqrt((1.0f + d) * 2.0f); - real_t rs = 1.0f / s; - - x = c.x * rs; - y = c.y * rs; - z = c.z * rs; - w = s * 0.5f; - } - } + Quaternion(const Vector3 &p_v0, const Vector3 &p_v1); // Shortest arc. }; real_t Quaternion::dot(const Quaternion &p_q) const { diff --git a/tests/core/math/test_quaternion.h b/tests/core/math/test_quaternion.h index 40db43b88b4027..71c8b49408a5f9 100644 --- a/tests/core/math/test_quaternion.h +++ b/tests/core/math/test_quaternion.h @@ -235,6 +235,36 @@ TEST_CASE("[Quaternion] Construct Basis Axes") { CHECK(q[3] == doctest::Approx(0.8582598)); } +TEST_CASE("[Quaternion] Construct Shortest Arc For 180 Degree Arc") { + Vector3 up(0, 1, 0); + Vector3 down(0, -1, 0); + Vector3 left(-1, 0, 0); + Vector3 right(1, 0, 0); + Vector3 forward(0, 0, -1); + Vector3 back(0, 0, 1); + + // When we have a 180 degree rotation quaternion which was defined as + // A to B, logically when we transform A we expect to get B. + Quaternion left_to_right(left, right); + CHECK(left_to_right.xform(left).is_equal_approx(right)); + CHECK(Quaternion(right, left).xform(right).is_equal_approx(left)); + CHECK(Quaternion(up, down).xform(up).is_equal_approx(down)); + CHECK(Quaternion(down, up).xform(down).is_equal_approx(up)); + CHECK(Quaternion(forward, back).xform(forward).is_equal_approx(back)); + CHECK(Quaternion(back, forward).xform(back).is_equal_approx(forward)); + + // With (arbitrary) opposite vectors that are not axis-aligned as parameters + Vector3 diagonal_up = Vector3(1.2, 2.3, 4.5).normalized(); + Vector3 diagonal_down = -diagonal_up; + Quaternion q1(diagonal_up, diagonal_down); + CHECK(q1.xform(diagonal_down).is_equal_approx(diagonal_up)); + CHECK(q1.xform(diagonal_up).is_equal_approx(diagonal_down)); + + // When the two vectors lie on the XZ plane, the rotation axis should be the + // Y-axis for backwards compatibility. + CHECK(left_to_right.get_axis().is_equal_approx(up)); +} + TEST_CASE("[Quaternion] Get Euler Orders") { double x = Math::deg_to_rad(30.0); double y = Math::deg_to_rad(45.0);