From db7c91e0ada0c91ba48269cf3cc217631ca33bba Mon Sep 17 00:00:00 2001
From: fabriceci <fabricecipolla@gmail.com>
Date: Tue, 19 Jul 2022 09:50:54 +0200
Subject: [PATCH] Fix the calculation of the angular velocity when the rotation
 speed is not high.

---
 core/math/basis.cpp | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index ce5e9aa9b3e9..f8e7c4710756 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -817,14 +817,13 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 #endif
 */
 	real_t angle, x, y, z; // variables for result
-	real_t epsilon = 0.01; // margin to allow for rounding errors
-	real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
+	real_t angle_epsilon = 0.1; // margin to distinguish between 0 and 180 degrees
 
-	if ((Math::abs(rows[1][0] - rows[0][1]) < epsilon) && (Math::abs(rows[2][0] - rows[0][2]) < epsilon) && (Math::abs(rows[2][1] - rows[1][2]) < epsilon)) {
+	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 diagonal and zero in other terms
-		if ((Math::abs(rows[1][0] + rows[0][1]) < epsilon2) && (Math::abs(rows[2][0] + rows[0][2]) < epsilon2) && (Math::abs(rows[2][1] + rows[1][2]) < epsilon2) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < epsilon2)) {
+		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;
@@ -839,7 +838,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 		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
-			if (xx < epsilon) {
+			if (xx < CMP_EPSILON) {
 				x = 0;
 				y = Math_SQRT12;
 				z = Math_SQRT12;
@@ -849,7 +848,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 				z = xz / x;
 			}
 		} else if (yy > zz) { // rows[1][1] is the largest diagonal term
-			if (yy < epsilon) {
+			if (yy < CMP_EPSILON) {
 				x = Math_SQRT12;
 				y = 0;
 				z = Math_SQRT12;
@@ -859,7 +858,7 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 				z = yz / y;
 			}
 		} else { // rows[2][2] is the largest diagonal term so base result on this
-			if (zz < epsilon) {
+			if (zz < CMP_EPSILON) {
 				x = Math_SQRT12;
 				y = Math_SQRT12;
 				z = 0;