Quaternions: Handle 180 degree arcs in the shortest arc constructor better

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 (unlikely(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.

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 {

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);