Relintai/pandemonium_engine
1
0
Fork 0
mirror of https://github.com/Relintai/pandemonium_engine.git synced 2024-11-22 00:48:09 +01:00
Code Issues Packages Projects Releases Wiki Activity

Ported: Test, refactor and fix a bug in Basis.get_axis_angle

Browse Source 
- fabriceci
9f1a57d48b
This commit is contained in:
Relintai 2023-06-11 09:45:23 +02:00
parent c46c527f01
commit 6c9843e5cb
2 changed files with 93 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
core/math/basis.cpp
View File

@ -914,29 +914,30 @@ 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
*/
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
real_t x, y, z; // Variables for result.
if (Math::is_zero_approx(rows[0][1] - rows[1][0]) && Math::is_zero_approx(rows[0][2] - rows[2][0]) && Math::is_zero_approx(rows[1][2] - rows[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(rows[0][0] + rows[1][1] + rows[2][2] - 3) < 3 * CMP_EPSILON)) {
// This singularity is identity matrix so angle = 0.
if ((Math::abs(rows[1][0] - rows[0][1]) < CMP_EPSILON) && (Math::abs(rows[2][0] - rows[0][2]) < CMP_EPSILON) && (Math::abs(rows[2][1] - rows[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(rows[1][0] + rows[0][1]) < angle_epsilon) && (Math::abs(rows[2][0] + rows[0][2]) < angle_epsilon) && (Math::abs(rows[2][1] + rows[1][2]) < angle_epsilon) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < angle_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 = (rows[0][0] + 1) / 2;
real_t yy = (rows[1][1] + 1) / 2;
real_t zz = (rows[2][2] + 1) / 2;
real_t xy = (rows[1][0] + rows[0][1]) / 4;
real_t xz = (rows[2][0] + rows[0][2]) / 4;
real_t yz = (rows[2][1] + rows[1][2]) / 4;
if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term
real_t xy = (rows[0][1] + rows[1][0]) / 4;
real_t xz = (rows[0][2] + rows[2][0]) / 4;
real_t yz = (rows[1][2] + rows[2][1]) / 4;
if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term.
if (xx < CMP_EPSILON) {
x = 0;
y = Math_SQRT12;
@ -946,7 +947,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
y = xy / x;
z = xz / x;
}
} else if (yy > zz) { // rows[1][1] is the largest diagonal term
} else if (yy > zz) { // rows[1][1] is the largest diagonal term.
if (yy < CMP_EPSILON) {
x = Math_SQRT12;
y = 0;
@ -956,7 +957,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
x = xy / y;
z = yz / y;
}
} else { // rows[2][2] is the largest diagonal term so base result on this
} else { // rows[2][2] is the largest diagonal term so base result on this.
if (zz < CMP_EPSILON) {
x = Math_SQRT12;
y = Math_SQRT12;
@ -968,23 +969,25 @@ 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((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise
// acos does clamping.
angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2);
if (angle < 0) {
s = -s;
// As we have reached here there are no singularities so we can handle normally
real_t s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalise.
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 = (rows[2][1] - rows[1][2]) / s;
y = (rows[0][2] - rows[2][0]) / s;
z = (rows[1][0] - rows[0][1]) / s;
r_axis = Vector3(x, y, z);
r_angle = angle;
// acos does clamping.
r_angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2);
}
void Basis::set_quaternion(const Quaternion &p_quat) {

65
main/tests/test_basis.cpp
View File

@ -315,10 +315,75 @@ 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;
}