From dee98d3b6d58cbe42fc403d999b278aec9447105 Mon Sep 17 00:00:00 2001
From: Juan Linietsky <reduzio@gmail.com>
Date: Mon, 1 Apr 2019 11:11:02 -0300
Subject: [PATCH] Some improvements to is_equal_approx, restored Quat operator.

---
 core/math/basis.cpp    | 26 ++++++++++++++++++++------
 core/math/basis.h      |  3 ++-
 core/math/math_funcs.h |  4 ++--
 core/variant_call.cpp  |  4 ++++
 4 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 8816e3639a3..82b2f7006da 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -557,11 +557,23 @@ void Basis::set_euler_yxz(const Vector3 &p_euler) {
 	*this = ymat * xmat * zmat;
 }
 
-bool Basis::is_equal_approx(const Basis &a, const Basis &b) const {
+bool Basis::is_equal_approx(const Basis &a, const Basis &b,real_t p_epsilon) const {
 
 	for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
-			if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], UNIT_EPSILON))
+			if (!Math::is_equal_approx(a.elements[i][j], b.elements[i][j], p_epsilon))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+bool Basis::is_equal_approx_ratio(const Basis &a, const Basis &b,real_t p_epsilon) const {
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			if (!Math::is_equal_approx_ratio(a.elements[i][j], b.elements[i][j], p_epsilon))
 				return false;
 		}
 	}
@@ -605,12 +617,14 @@ Basis::operator String() const {
 
 Quat Basis::get_quat() const {
 
+#ifdef MATH_CHECKS
+	if (!is_rotation()) {
+		ERR_EXPLAIN("Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() instead.");
+		ERR_FAIL_V(Quat());
+	}
+#endif
 	/* Allow getting a quaternion from an unnormalized transform */
 	Basis m = *this;
-	m.elements[0].normalize();
-	m.elements[1].normalize();
-	m.elements[2].normalize();
-
 	real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2];
 	real_t temp[4];
 
diff --git a/core/math/basis.h b/core/math/basis.h
index 128e56b494f..aa0ddb280fa 100644
--- a/core/math/basis.h
+++ b/core/math/basis.h
@@ -133,7 +133,8 @@ public:
 		return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
 	}
 
-	bool is_equal_approx(const Basis &a, const Basis &b) const;
+	bool is_equal_approx(const Basis &a, const Basis &b, real_t p_epsilon=CMP_EPSILON) const;
+	bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon=UNIT_EPSILON) const;
 
 	bool operator==(const Basis &p_matrix) const;
 	bool operator!=(const Basis &p_matrix) const;
diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 17112d89404..6ac68398274 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -249,11 +249,11 @@ public:
 	static float random(float from, float to);
 	static real_t random(int from, int to) { return (real_t)random((real_t)from, (real_t)to); }
 
-	static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON) {
+	static _ALWAYS_INLINE_ bool is_equal_approx_ratio(real_t a, real_t b, real_t epsilon = CMP_EPSILON, real_t min_epsilon = CMP_EPSILON) {
 		// this is an approximate way to check that numbers are close, as a ratio of their average size
 		// helps compare approximate numbers that may be very big or very small
 		real_t diff = abs(a - b);
-		if (diff == 0.0) {
+		if (diff == 0.0 || diff < min_epsilon) {
 			return true;
 		}
 		real_t avg_size = (abs(a) + abs(b)) / 2.0;
diff --git a/core/variant_call.cpp b/core/variant_call.cpp
index 25a0f3957c0..3812592639d 100644
--- a/core/variant_call.cpp
+++ b/core/variant_call.cpp
@@ -773,6 +773,8 @@ struct _VariantCall {
 	VCALL_PTR0R(Basis, get_orthogonal_index);
 	VCALL_PTR0R(Basis, orthonormalized);
 	VCALL_PTR2R(Basis, slerp);
+	VCALL_PTR2R(Basis, is_equal_approx);
+	VCALL_PTR0R(Basis, get_rotation_quat);
 
 	VCALL_PTR0R(Transform, inverse);
 	VCALL_PTR0R(Transform, affine_inverse);
@@ -1842,6 +1844,8 @@ void register_variant_methods() {
 	ADDFUNC1R(BASIS, VECTOR3, Basis, xform_inv, VECTOR3, "v", varray());
 	ADDFUNC0R(BASIS, INT, Basis, get_orthogonal_index, varray());
 	ADDFUNC2R(BASIS, BASIS, Basis, slerp, BASIS, "b", REAL, "t", varray());
+	ADDFUNC2R(BASIS, BOOL, Basis, is_equal_approx, BASIS, "b", REAL, "epsilon", varray(CMP_EPSILON));
+	ADDFUNC0R(BASIS, QUAT, Basis, get_rotation_quat, varray());
 
 	ADDFUNC0R(TRANSFORM, TRANSFORM, Transform, inverse, varray());
 	ADDFUNC0R(TRANSFORM, TRANSFORM, Transform, affine_inverse, varray());