1 files changed, 32 insertions, 14 deletions

diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index b0b05d1ec8..7db41756ed 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -242,18 +242,11 @@ void Basis::scale_local(const Vector3 &p_scale) {
 
 Basis Basis::scaled_local(const Vector3 &p_scale) const {
 	Basis b;
-	b.set_scale(p_scale);
+	b.set_diagonal(p_scale);
 
 	return (*this) * b;
 }
 
-void Basis::set_scale(const Vector3 &p_scale) {
-
-	set_axis(0, get_axis(0).normalized() * p_scale.x);
-	set_axis(1, get_axis(1).normalized() * p_scale.y);
-	set_axis(2, get_axis(2).normalized() * p_scale.z);
-}
-
 Vector3 Basis::get_scale_abs() const {
 
 	return Vector3(
@@ -356,8 +349,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 +363,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 +597,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];
 
@@ -826,3 +831,16 @@ void Basis::set_diagonal(const Vector3 p_diag) {
 	elements[2][1] = 0;
 	elements[2][2] = p_diag.z;
 }
+
+Basis Basis::slerp(const Basis &target, const real_t &t) const {
+// TODO: implement this directly without using quaternions to make it more efficient
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_rotation() == false, Basis());
+	ERR_FAIL_COND_V(target.is_rotation() == false, Basis());
+#endif
+
+	Quat from(*this);
+	Quat to(target);
+
+	return Basis(from.slerp(to, t));
+}