1 files changed, 38 insertions, 17 deletions

diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 8816e3639a..1540bc8fe1 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];
 
@@ -799,21 +813,28 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
 	ERR_FAIL_COND(!p_axis.is_normalized());
 #endif
 	Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z);
-
 	real_t cosine = Math::cos(p_phi);
-	real_t sine = Math::sin(p_phi);
-
 	elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x);
-	elements[0][1] = p_axis.x * p_axis.y * (1.0 - cosine) - p_axis.z * sine;
-	elements[0][2] = p_axis.z * p_axis.x * (1.0 - cosine) + p_axis.y * sine;
-
-	elements[1][0] = p_axis.x * p_axis.y * (1.0 - cosine) + p_axis.z * sine;
 	elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y);
-	elements[1][2] = p_axis.y * p_axis.z * (1.0 - cosine) - p_axis.x * sine;
-
-	elements[2][0] = p_axis.z * p_axis.x * (1.0 - cosine) - p_axis.y * sine;
-	elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine;
 	elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z);
+
+	real_t sine = Math::sin(p_phi);
+	real_t t = 1 - cosine;
+
+	real_t xyzt = p_axis.x * p_axis.y * t;
+	real_t zyxs = p_axis.z * sine;
+	elements[0][1] = xyzt - zyxs;
+	elements[1][0] = xyzt + zyxs;
+
+	xyzt = p_axis.x * p_axis.z * t;
+	zyxs = p_axis.y * sine;
+	elements[0][2] = xyzt + zyxs;
+	elements[2][0] = xyzt - zyxs;
+
+	xyzt = p_axis.y * p_axis.z * t;
+	zyxs = p_axis.x * sine;
+	elements[1][2] = xyzt - zyxs;
+	elements[2][1] = xyzt + zyxs;
 }
 
 void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) {