Test, refactor and fix a bug in Basis.get_axis_angle

Backport of godotengine#63428.

Co-authored-by: juanFdS <[email protected]>

core/math/basis.cpp

@@ -865,29 +865,28 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 #ifdef MATH_CHECKS
 	ERR_FAIL_COND(!is_rotation());
 #endif
-*/
-	real_t angle, x, y, z; // variables for result
-	real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees
-
-	if ((Math::abs(elements[1][0] - elements[0][1]) < CMP_EPSILON) && (Math::abs(elements[2][0] - elements[0][2]) < CMP_EPSILON) && (Math::abs(elements[2][1] - elements[1][2]) < CMP_EPSILON)) {
-		// singularity found
-		// first check for identity matrix which must have +1 for all terms
-		//  in leading diagonaland zero in other terms
-		if ((Math::abs(elements[1][0] + elements[0][1]) < angle_epsilon) && (Math::abs(elements[2][0] + elements[0][2]) < angle_epsilon) && (Math::abs(elements[2][1] + elements[1][2]) < angle_epsilon) && (Math::abs(elements[0][0] + elements[1][1] + elements[2][2] - 3) < angle_epsilon)) {
-			// this singularity is identity matrix so angle = 0
+	*/
+
+	// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
+	real_t x, y, z; // Variables for result.
+	if (Math::is_zero_approx(elements[0][1] - elements[1][0]) && Math::is_zero_approx(elements[0][2] - elements[2][0]) && Math::is_zero_approx(elements[1][2] - elements[2][1])) {
+		// Singularity found.
+		// First check for identity matrix which must have +1 for all terms in leading diagonal and zero in other terms.
+		if (is_diagonal() && (Math::abs(elements[0][0] + elements[1][1] + elements[2][2] - 3) < 3 * CMP_EPSILON)) {
+			// This singularity is identity matrix so angle = 0.
 			r_axis = Vector3(0, 1, 0);
 			r_angle = 0;
 			return;
 		}
-		// otherwise this singularity is angle = 180
-		angle = Math_PI;
+		// Otherwise this singularity is angle = 180.
 		real_t xx = (elements[0][0] + 1) / 2;
 		real_t yy = (elements[1][1] + 1) / 2;
 		real_t zz = (elements[2][2] + 1) / 2;
-		real_t xy = (elements[1][0] + elements[0][1]) / 4;
-		real_t xz = (elements[2][0] + elements[0][2]) / 4;
-		real_t yz = (elements[2][1] + elements[1][2]) / 4;
-		if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term
+		real_t xy = (elements[0][1] + elements[1][0]) / 4;
+		real_t xz = (elements[0][2] + elements[2][0]) / 4;
+		real_t yz = (elements[1][2] + elements[2][1]) / 4;
+
+		if ((xx > yy) && (xx > zz)) { // elements[0][0] is the largest diagonal term.
 			if (xx < CMP_EPSILON) {
 				x = 0;
 				y = Math_SQRT12;
@@ -897,7 +896,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 				y = xy / x;
 				z = xz / x;
 			}
-		} else if (yy > zz) { // elements[1][1] is the largest diagonal term
+		} else if (yy > zz) { // elements[1][1] is the largest diagonal term.
 			if (yy < CMP_EPSILON) {
 				x = Math_SQRT12;
 				y = 0;
@@ -907,7 +906,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 				x = xy / y;
 				z = yz / y;
 			}
-		} else { // elements[2][2] is the largest diagonal term so base result on this
+		} else { // elements[2][2] is the largest diagonal term so base result on this.
 			if (zz < CMP_EPSILON) {
 				x = Math_SQRT12;
 				y = Math_SQRT12;
@@ -919,23 +918,24 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 			}
 		}
 		r_axis = Vector3(x, y, z);
-		r_angle = angle;
+		r_angle = Math_PI;
 		return;
 	}
-	// as we have reached here there are no singularities so we can handle normally
-	real_t s = Math::sqrt((elements[1][2] - elements[2][1]) * (elements[1][2] - elements[2][1]) + (elements[2][0] - elements[0][2]) * (elements[2][0] - elements[0][2]) + (elements[0][1] - elements[1][0]) * (elements[0][1] - elements[1][0])); // s=|axis||sin(angle)|, used to normalise
+	// As we have reached here there are no singularities so we can handle normally.
+	double s = Math::sqrt((elements[2][1] - elements[1][2]) * (elements[2][1] - elements[1][2]) + (elements[0][2] - elements[2][0]) * (elements[0][2] - elements[2][0]) + (elements[1][0] - elements[0][1]) * (elements[1][0] - elements[0][1])); // Used to normalise.
 
-	// acos does clamping.
-	angle = Math::acos((elements[0][0] + elements[1][1] + elements[2][2] - 1) / 2);
-	if (angle < 0) {
-		s = -s;
+	if (Math::abs(s) < CMP_EPSILON) {
+		// Prevent divide by zero, should not happen if matrix is orthogonal and should be caught by singularity test above.
+		s = 1;
 	}
+
 	x = (elements[2][1] - elements[1][2]) / s;
 	y = (elements[0][2] - elements[2][0]) / s;
 	z = (elements[1][0] - elements[0][1]) / s;
 
 	r_axis = Vector3(x, y, z);
-	r_angle = angle;
+	// acos does clamping.
+	r_angle = Math::acos((elements[0][0] + elements[1][1] + elements[2][2] - 1) / 2);
 }
 
 void Basis::set_quat(const Quat &p_quat) {

main/tests/test_basis.cpp

@@ -315,9 +315,73 @@ void test_euler_conversion() {
 	}
 }
 
+void check_test(std::string test_case_name, bool condition) {
+	if (!condition) {
+		OS::get_singleton()->print("FAILED - %s\n", test_case_name.c_str());
+	} else {
+		OS::get_singleton()->print("PASSED - %s\n", test_case_name.c_str());
+	}
+}
+
+void test_set_axis_angle() {
+	Vector3 axis;
+	real_t angle;
+	real_t pi = (real_t)Math_PI;
+
+	// Testing the singularity when the angle is 0°.
+	Basis identity(1, 0, 0, 0, 1, 0, 0, 0, 1);
+	identity.get_axis_angle(axis, angle);
+	check_test("Testing the singularity when the angle is 0.", angle == 0);
+
+	// Testing the singularity when the angle is 180°.
+	Basis singularityPi(-1, 0, 0, 0, 1, 0, 0, 0, -1);
+	singularityPi.get_axis_angle(axis, angle);
+	check_test("Testing the singularity when the angle is 180.", Math::is_equal_approx(angle, pi));
+
+	// Testing reversing the an axis (of an 30° angle).
+	float cos30deg = Math::cos(Math::deg2rad((real_t)30.0));
+	Basis z_positive(cos30deg, -0.5, 0, 0.5, cos30deg, 0, 0, 0, 1);
+	Basis z_negative(cos30deg, 0.5, 0, -0.5, cos30deg, 0, 0, 0, 1);
+
+	z_positive.get_axis_angle(axis, angle);
+	check_test("Testing reversing the an axis (of an 30 angle).", Math::is_equal_approx(angle, Math::deg2rad((real_t)30.0)));
+	check_test("Testing reversing the an axis (of an 30 angle).", axis == Vector3(0, 0, 1));
+
+	z_negative.get_axis_angle(axis, angle);
+	check_test("Testing reversing the an axis (of an 30 angle).", Math::is_equal_approx(angle, Math::deg2rad((real_t)30.0)));
+	check_test("Testing reversing the an axis (of an 30 angle).", axis == Vector3(0, 0, -1));
+
+	// Testing a rotation of 90° on x-y-z.
+	Basis x90deg(1, 0, 0, 0, 0, -1, 0, 1, 0);
+	x90deg.get_axis_angle(axis, angle);
+	check_test("Testing a rotation of 90 on x-y-z.", Math::is_equal_approx(angle, pi / (real_t)2));
+	check_test("Testing a rotation of 90 on x-y-z.", axis == Vector3(1, 0, 0));
+
+	Basis y90deg(0, 0, 1, 0, 1, 0, -1, 0, 0);
+	y90deg.get_axis_angle(axis, angle);
+	check_test("Testing a rotation of 90 on x-y-z.", axis == Vector3(0, 1, 0));
+
+	Basis z90deg(0, -1, 0, 1, 0, 0, 0, 0, 1);
+	z90deg.get_axis_angle(axis, angle);
+	check_test("Testing a rotation of 90 on x-y-z.", axis == Vector3(0, 0, 1));
+
+	// Regression test: checks that the method returns a small angle (not 0).
+	Basis tiny(1, 0, 0, 0, 0.9999995, -0.001, 0, 001, 0.9999995); // The min angle possible with float is 0.001rad.
+	tiny.get_axis_angle(axis, angle);
+	check_test("Regression test: checks that the method returns a small angle (not 0).", Math::is_equal_approx(angle, (real_t)0.001, (real_t)0.0001));
+
+	// Regression test: checks that the method returns an angle which is a number (not NaN)
+	Basis bugNan(1.00000024, 0, 0.000100001693, 0, 1, 0, -0.000100009143, 0, 1.00000024);
+	bugNan.get_axis_angle(axis, angle);
+	check_test("Regression test: checks that the method returns an angle which is a number (not NaN)", !Math::is_nan(angle));
+}
+
 MainLoop *test() {
 	OS::get_singleton()->print("Start euler conversion checks.\n");
 	test_euler_conversion();
+	OS::get_singleton()->print("\n---------------\n");
+	OS::get_singleton()->print("Start set axis angle checks.\n");
+	test_set_axis_angle();
 
 	return nullptr;
 }