Merge pull request #19193 from tagcup/quat_norm

Fixed Basis -> Quat conversions, added a few safety checks.

5 files changed, 37 insertions, 15 deletions

diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index 202115e2ca..2371f49561 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -356,8 +356,7 @@ void Basis::rotate(const Quat &p_quat) {
 	*this = rotated(p_quat);
 }
 
-// TODO: rename this to get_rotation_euler
-Vector3 Basis::get_rotation() const {
+Vector3 Basis::get_rotation_euler() const {
 	// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
 	// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
 	// See the comment in get_scale() for further information.
@@ -371,6 +370,20 @@ Vector3 Basis::get_rotation() const {
 	return m.get_euler();
 }
 
+Quat Basis::get_rotation_quat() const {
+	// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
+	// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
+	// See the comment in get_scale() for further information.
+	Basis m = orthonormalized();
+	real_t det = m.determinant();
+	if (det < 0) {
+		// Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
+		m.scale(Vector3(-1, -1, -1));
+	}
+
+	return m.get_quat();
+}
+
 void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const {
 	// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
 	// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
@@ -591,10 +604,9 @@ Basis::operator String() const {
 }
 
 Quat Basis::get_quat() const {
-	//commenting this check because precision issues cause it to fail when it shouldn't
-	//#ifdef MATH_CHECKS
-	//ERR_FAIL_COND_V(is_rotation() == false, Quat());
-	//#endif
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_rotation() == false, Quat());
+#endif
 	real_t trace = elements[0][0] + elements[1][1] + elements[2][2];
 	real_t temp[4];
 
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 63d4f5d79d..cd1b51baa6 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -84,9 +84,11 @@ public:
 	void rotate(const Quat &p_quat);
 	Basis rotated(const Quat &p_quat) const;
 
-	Vector3 get_rotation() const;
+	Vector3 get_rotation_euler() const;
 	void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
 	void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const;
+	Quat get_rotation_quat() const;
+	Vector3 get_rotation() const { return get_rotation_euler(); };
 
 	Vector3 rotref_posscale_decomposition(Basis &rotref) const;
 
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index b938fc3cfd..67c9048a41 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -139,15 +139,15 @@ bool Quat::is_normalized() const {
 
 Quat Quat::inverse() const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
+	ERR_FAIL_COND_V(is_normalized() == false, Quat());
 #endif
 	return Quat(-x, -y, -z, w);
 }
 
 Quat Quat::slerp(const Quat &q, const real_t &t) const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
-	ERR_FAIL_COND_V(q.is_normalized() == false, Quat(0, 0, 0, 0));
+	ERR_FAIL_COND_V(is_normalized() == false, Quat());
+	ERR_FAIL_COND_V(q.is_normalized() == false, Quat());
 #endif
 	Quat to1;
 	real_t omega, cosom, sinom, scale0, scale1;
@@ -192,7 +192,10 @@ Quat Quat::slerp(const Quat &q, const real_t &t) const {
 }
 
 Quat Quat::slerpni(const Quat &q, const real_t &t) const {
-
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_normalized() == false, Quat());
+	ERR_FAIL_COND_V(q.is_normalized() == false, Quat());
+#endif
 	const Quat &from = *this;
 
 	real_t dot = from.dot(q);
@@ -211,7 +214,10 @@ Quat Quat::slerpni(const Quat &q, const real_t &t) const {
 }
 
 Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const {
-
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_normalized() == false, Quat());
+	ERR_FAIL_COND_V(q.is_normalized() == false, Quat());
+#endif
 	//the only way to do slerp :|
 	real_t t2 = (1.0 - t) * t * 2;
 	Quat sp = this->slerp(q, t);
diff --git a/core/math/quat.h b/core/math/quat.h
index 3e1344a913..6dc8d66f60 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -84,7 +84,9 @@ public:
 	}
 
 	_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
-
+#ifdef MATH_CHECKS
+		ERR_FAIL_COND_V(is_normalized() == false, v);
+#endif
 		Vector3 u(x, y, z);
 		Vector3 uv = u.cross(v);
 		return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
index 7cd186ca60..d1e190f4b9 100644
--- a/core/math/transform.cpp
+++ b/core/math/transform.cpp
@@ -120,11 +120,11 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c)
 	/* not sure if very "efficient" but good enough? */
 
 	Vector3 src_scale = basis.get_scale();
-	Quat src_rot = basis.orthonormalized();
+	Quat src_rot = basis.get_rotation_quat();
 	Vector3 src_loc = origin;
 
 	Vector3 dst_scale = p_transform.basis.get_scale();
-	Quat dst_rot = p_transform.basis;
+	Quat dst_rot = p_transform.basis.get_rotation_quat();
 	Vector3 dst_loc = p_transform.origin;
 
 	Transform dst; //this could be made faster by using a single function in Basis..