4 files changed, 58 insertions, 27 deletions
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index a985e29abb..a928b4d0e5 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -133,16 +133,18 @@ Matrix3 Matrix3::transposed() const {
 	return tr;
 }
 
+// Multiplies the matrix from left by the scaling matrix: M -> S.M
+// See the comment for Matrix3::rotated for further explanation.
 void Matrix3::scale(const Vector3& p_scale) {
 
 	elements[0][0]*=p_scale.x;
-	elements[1][0]*=p_scale.x;
-	elements[2][0]*=p_scale.x;
-	elements[0][1]*=p_scale.y;
+	elements[0][1]*=p_scale.x;
+	elements[0][2]*=p_scale.x;
+	elements[1][0]*=p_scale.y;
 	elements[1][1]*=p_scale.y;
-	elements[2][1]*=p_scale.y;
-	elements[0][2]*=p_scale.z;
-	elements[1][2]*=p_scale.z;
+	elements[1][2]*=p_scale.y;
+	elements[2][0]*=p_scale.z;
+	elements[2][1]*=p_scale.z;
 	elements[2][2]*=p_scale.z;
 }
 
@@ -154,8 +156,13 @@ Matrix3 Matrix3::scaled( const Vector3& p_scale ) const {
 }
 
 Vector3 Matrix3::get_scale() const {
-
-	return Vector3(
+	// We are assuming M = R.S, and performing a polar decomposition to extract R and S.
+	// FIXME: We eventually need a proper polar decomposition.
+	// As a cheap workaround until then, to ensure that R is a proper rotation matrix with determinant +1
+	// (such that it can be represented by a Quat or Euler angles), we absorb the sign flip into the scaling matrix.
+	// As such, it works in conjuction with get_rotation().
+	real_t det_sign = determinant() > 0 ? 1 : -1;
+	return det_sign*Vector3(
 		Vector3(elements[0][0],elements[1][0],elements[2][0]).length(),
 		Vector3(elements[0][1],elements[1][1],elements[2][1]).length(),
 		Vector3(elements[0][2],elements[1][2],elements[2][2]).length()
@@ -163,18 +170,40 @@ Vector3 Matrix3::get_scale() const {
 
 }
 
-// Matrix3::rotate and Matrix3::rotated return M * R(axis,phi), and is a convenience function. They do *not* perform proper matrix rotation.
+// Multiplies the matrix from left by the rotation matrix: M -> R.M
+// Note that this does *not* rotate the matrix itself.
+//
+// The main use of Matrix3 is as Transform.basis, which is used a the transformation matrix
+// of 3D object. Rotate here refers to rotation of the object (which is R * (*this)),
+// not the matrix itself (which is R * (*this) * R.transposed()).
+Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const {
+	return Matrix3(p_axis, p_phi) * (*this);
+}
+
 void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) {
-	// TODO: This function should also be renamed as the current name is misleading: rotate does *not* perform matrix rotation.
-	// Same problem affects Matrix3::rotated.
-	// A similar problem exists in 2D math, which will be handled separately.
-	// After Matrix3 is renamed to Basis, this comments needs to be revised.
-	*this = *this * Matrix3(p_axis, p_phi);
+	*this = rotated(p_axis, p_phi);
 }
 
-Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const {
-	return *this * Matrix3(p_axis, p_phi);
+Matrix3 Matrix3::rotated(const Vector3& p_euler) const {
+	return Matrix3(p_euler) * (*this);
+}
+
+void Matrix3::rotate(const Vector3& p_euler) {
+	*this = rotated(p_euler);
+}
+
+Vector3 Matrix3::get_rotation() 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.
+	Matrix3 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_euler();
 }
 
 // get_euler returns a vector containing the Euler angles in the format
@@ -363,7 +392,7 @@ int Matrix3::get_orthogonal_index() const {
 	for(int i=0;i<3;i++) {
 		for(int j=0;j<3;j++) {
 
-			float v = orth[i][j];
+			real_t v = orth[i][j];
 			if (v>0.5)
 				v=1.0;
 			else if (v<-0.5)
@@ -398,9 +427,6 @@ void Matrix3::set_orthogonal_index(int p_index){
 
 
 void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const {
-	// TODO: We can handle improper matrices here too, in which case axis will also correspond to the axis of reflection.
-	// See Eq. (52) in http://scipp.ucsc.edu/~haber/ph251/rotreflect_13.pdf for example
-	// After that change, we should fail on is_orthogonal() == false.
 	ERR_FAIL_COND(is_rotation() == false);
 
 
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 1d967c03b8..33a5ce8687 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -55,7 +55,7 @@ public:
 	Matrix3 inverse() const;
 	Matrix3 transposed() const;
 
-	_FORCE_INLINE_ float determinant() const;
+	_FORCE_INLINE_ real_t determinant() const;
 
 	void from_z(const Vector3& p_z);
 
@@ -73,6 +73,10 @@ public:
 	void rotate(const Vector3& p_axis, real_t p_phi);
 	Matrix3 rotated(const Vector3& p_axis, real_t p_phi) const;
 
+	void rotate(const Vector3& p_euler);
+	Matrix3 rotated(const Vector3& p_euler) const;
+	Vector3 get_rotation() const;
+
 	void scale( const Vector3& p_scale );
 	Matrix3 scaled( const Vector3& p_scale ) const;
 	Vector3 get_scale() const;
@@ -226,7 +230,7 @@ Vector3 Matrix3::xform_inv(const Vector3& p_vector) const {
 	);
 }
 
-float Matrix3::determinant() const {
+real_t Matrix3::determinant() const {
 
 	return elements[0][0]*(elements[1][1]*elements[2][2] - elements[2][1]*elements[1][2]) -
 	       elements[1][0]*(elements[0][1]*elements[2][2] - elements[2][1]*elements[0][2]) +
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
index 8516e4afcf..0dba121013 100644
--- a/core/math/transform.cpp
+++ b/core/math/transform.cpp
@@ -54,7 +54,8 @@ void Transform::invert() {
 }
 
 Transform Transform::inverse() const {
-
+	// FIXME: this function assumes the basis is a rotation matrix, with no scaling.
+	// Transform::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
 	Transform ret=*this;
 	ret.invert();
 	return ret;
@@ -63,12 +64,12 @@ Transform Transform::inverse() const {
 
 void Transform::rotate(const Vector3& p_axis,real_t p_phi) {
 
-	*this = *this * Transform( Matrix3( p_axis, p_phi ), Vector3()  );
+	*this = rotated(p_axis, p_phi);
 }
 
 Transform Transform::rotated(const Vector3& p_axis,real_t p_phi) const{
 
-	return *this * Transform( Matrix3( p_axis, p_phi ), Vector3()  );
+	return Transform(Matrix3( p_axis, p_phi ), Vector3()) * (*this);
 }
 
 void Transform::rotate_basis(const Vector3& p_axis,real_t p_phi) {
@@ -113,7 +114,7 @@ void Transform::set_look_at( const Vector3& p_eye, const Vector3& p_target, cons
 
 }
 
-Transform Transform::interpolate_with(const Transform& p_transform, float p_c) const {
+Transform Transform::interpolate_with(const Transform& p_transform, real_t p_c) const {
 
 	/* not sure if very "efficient" but good enough? */
 
diff --git a/core/math/transform.h b/core/math/transform.h
index 7999f0b347..5f069ab586 100644
--- a/core/math/transform.h
+++ b/core/math/transform.h
@@ -86,7 +86,7 @@ public:
 	void operator*=(const Transform& p_transform);
 	Transform operator*(const Transform& p_transform) const;
 
-	Transform interpolate_with(const Transform& p_transform, float p_c) const;
+	Transform interpolate_with(const Transform& p_transform, real_t p_c) const;
 
 	_FORCE_INLINE_ Transform inverse_xform(const Transform& t) const {