Merge branch 'master' of https://github.com/godotengine/godot

6 files changed, 159 insertions, 82 deletions

diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index ec0ed39471..24081528f0 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -96,6 +96,15 @@ public:
 
 	static double random(double from, double to);
 
+	static _FORCE_INLINE_ bool isequal_approx(real_t a, real_t b) {
+		// TODO: Comparing floats for approximate-equality is non-trivial.
+		// Using epsilon should cover the typical cases in Godot (where a == b is used to compare two reals), such as matrix and vector comparison operators.
+		// A proper implementation in terms of ULPs should eventually replace the contents of this function.
+		// See https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ for details.
+
+		return abs(a-b) < CMP_EPSILON;
+	}
+
 
 	static _FORCE_INLINE_ real_t abs(real_t g) {
 
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index c30401cc24..a985e29abb 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -73,6 +73,7 @@ void Matrix3::invert() {
 }
 
 void Matrix3::orthonormalize() {
+	ERR_FAIL_COND(determinant() == 0);
 
 	// Gram-Schmidt Process
 
@@ -99,6 +100,17 @@ Matrix3 Matrix3::orthonormalized() const {
 	return c;
 }
 
+bool Matrix3::is_orthogonal() const {
+	Matrix3 id;
+	Matrix3 m = (*this)*transposed();
+
+	return isequal_approx(id,m);
+}
+
+bool Matrix3::is_rotation() const {
+	return Math::isequal_approx(determinant(), 1) && is_orthogonal();
+}
+
 
 Matrix3 Matrix3::inverse() const {
 
@@ -150,42 +162,58 @@ Vector3 Matrix3::get_scale() const {
 	);
 
 }
-void Matrix3::rotate(const Vector3& p_axis, real_t p_phi) {
 
+// Matrix3::rotate and Matrix3::rotated return M * R(axis,phi), and is a convenience function. They do *not* perform proper matrix rotation.
+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);
 }
 
 Matrix3 Matrix3::rotated(const Vector3& p_axis, real_t p_phi) const {
-
 	return *this * Matrix3(p_axis, p_phi);
 
 }
 
+// get_euler returns a vector containing the Euler angles in the format
+// (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last
+// (following the convention they are commonly defined in the literature).
+//
+// The current implementation uses XYZ convention (Z is the first rotation),
+// so euler.z is the angle of the (first) rotation around Z axis and so on,
+//
+// And thus, assuming the matrix is a rotation matrix, this function returns
+// the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates
+// around the z-axis by a and so on.
 Vector3 Matrix3::get_euler() const {
 
+	// Euler angles in XYZ convention.
+	// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
+	//
 	// rot =  cy*cz          -cy*sz           sy
-	    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
-	    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
-
-	Matrix3 m = *this;
-	m.orthonormalize();
+	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 
 	Vector3 euler;
 
-	euler.y = Math::asin(m[0][2]);
+	ERR_FAIL_COND_V(is_rotation() == false, euler);
+
+	euler.y = Math::asin(elements[0][2]);
 	if ( euler.y < Math_PI*0.5) {
 		if ( euler.y > -Math_PI*0.5) {
-			euler.x = Math::atan2(-m[1][2],m[2][2]);
-			euler.z = Math::atan2(-m[0][1],m[0][0]);
+			euler.x = Math::atan2(-elements[1][2],elements[2][2]);
+			euler.z = Math::atan2(-elements[0][1],elements[0][0]);
 
 		} else {
-			real_t r = Math::atan2(m[1][0],m[1][1]);
+			real_t r = Math::atan2(elements[1][0],elements[1][1]);
 			euler.z = 0.0;
 			euler.x = euler.z - r;
 
 		}
 	} else {
-		real_t r = Math::atan2(m[0][1],m[1][1]);
+		real_t r = Math::atan2(elements[0][1],elements[1][1]);
 		euler.z = 0;
 		euler.x = r - euler.z;
 	}
@@ -195,6 +223,9 @@ Vector3 Matrix3::get_euler() const {
 
 }
 
+// set_euler expects a vector containing the Euler angles in the format
+// (c,b,a), where a is the angle of the first rotation, and c is the last.
+// The current implementation uses XYZ convention (Z is the first rotation).
 void Matrix3::set_euler(const Vector3& p_euler) {
 
 	real_t c, s;
@@ -215,17 +246,30 @@ void Matrix3::set_euler(const Vector3& p_euler) {
 	*this = xmat*(ymat*zmat);
 }
 
+bool Matrix3::isequal_approx(const Matrix3& a, const Matrix3& b) const {
+
+        for (int i=0;i<3;i++) {
+                for (int j=0;j<3;j++) {
+                        if (Math::isequal_approx(a.elements[i][j],b.elements[i][j]) == false)
+                                return false;
+                }
+        }
+
+        return true;
+}
+
 bool Matrix3::operator==(const Matrix3& p_matrix) const {
 
 	for (int i=0;i<3;i++) {
 		for (int j=0;j<3;j++) {
-			if (elements[i][j]!=p_matrix.elements[i][j])
+			if (elements[i][j] != p_matrix.elements[i][j])
 				return false;
 		}
 	}
 
 	return true;
 }
+
 bool Matrix3::operator!=(const Matrix3& p_matrix) const {
 
 	return (!(*this==p_matrix));
@@ -249,11 +293,9 @@ Matrix3::operator String() const {
 }
 
 Matrix3::operator Quat() const {
+	ERR_FAIL_COND_V(is_rotation() == false, Quat());
 
-	Matrix3 m=*this;
-	m.orthonormalize();
-
-	real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2];
+	real_t trace = elements[0][0] + elements[1][1] + elements[2][2];
 	real_t temp[4];
 
 	if (trace > 0.0)
@@ -262,25 +304,25 @@ Matrix3::operator Quat() const {
 		temp[3]=(s * 0.5);
 		s = 0.5 / s;
 
-		temp[0]=((m.elements[2][1] - m.elements[1][2]) * s);
-		temp[1]=((m.elements[0][2] - m.elements[2][0]) * s);
-		temp[2]=((m.elements[1][0] - m.elements[0][1]) * s);
+		temp[0]=((elements[2][1] - elements[1][2]) * s);
+		temp[1]=((elements[0][2] - elements[2][0]) * s);
+		temp[2]=((elements[1][0] - elements[0][1]) * s);
 	}
 	else
 	{
-		int i = m.elements[0][0] < m.elements[1][1] ?
-			(m.elements[1][1] < m.elements[2][2] ? 2 : 1) :
-			(m.elements[0][0] < m.elements[2][2] ? 2 : 0);
+		int i = elements[0][0] < elements[1][1] ?
+			(elements[1][1] < elements[2][2] ? 2 : 1) :
+			(elements[0][0] < elements[2][2] ? 2 : 0);
 		int j = (i + 1) % 3;
 		int k = (i + 2) % 3;
 
-		real_t s = Math::sqrt(m.elements[i][i] - m.elements[j][j] - m.elements[k][k] + 1.0);
+		real_t s = Math::sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0);
 		temp[i] = s * 0.5;
 		s = 0.5 / s;
 
-		temp[3] = (m.elements[k][j] - m.elements[j][k]) * s;
-		temp[j] = (m.elements[j][i] + m.elements[i][j]) * s;
-		temp[k] = (m.elements[k][i] + m.elements[i][k]) * s;
+		temp[3] = (elements[k][j] - elements[j][k]) * s;
+		temp[j] = (elements[j][i] + elements[i][j]) * s;
+		temp[k] = (elements[k][i] + elements[i][k]) * s;
 	}
 
 	return Quat(temp[0],temp[1],temp[2],temp[3]);
@@ -356,6 +398,10 @@ 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);
 
 
 	double angle,x,y,z; // variables for result
@@ -423,14 +469,13 @@ void Matrix3::get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const {
 	// as we have reached here there are no singularities so we can handle normally
 	double s = Math::sqrt((elements[1][2] - elements[2][1])*(elements[1][2] - elements[2][1])
 		+(elements[2][0] - elements[0][2])*(elements[2][0] - elements[0][2])
-		+(elements[0][1] - elements[1][0])*(elements[0][1] - elements[1][0])); // used to normalise
-	if (Math::abs(s) < 0.001) s=1;
-		// prevent divide by zero, should not happen if matrix is orthogonal and should be
-		// caught by singularity test above, but I've left it in just in case
+		+(elements[0][1] - elements[1][0])*(elements[0][1] - elements[1][0])); // s=|axis||sin(angle)|, used to normalise
+
 	angle = Math::acos(( elements[0][0] + elements[1][1] + elements[2][2] - 1)/2);
-	x = (elements[1][2] - elements[2][1])/s;
-	y = (elements[2][0] - elements[0][2])/s;
-	z = (elements[0][1] - elements[1][0])/s;
+	if (angle < 0) s = -s;
+	x = (elements[2][1] - elements[1][2])/s;
+	y = (elements[0][2] - elements[2][0])/s;
+	z = (elements[1][0] - elements[0][1])/s;
 
 	r_axis=Vector3(x,y,z);
 	r_angle=angle;
@@ -457,6 +502,7 @@ Matrix3::Matrix3(const Quat& p_quat) {
 }
 
 Matrix3::Matrix3(const Vector3& p_axis, real_t p_phi) {
+	// Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
 
 	Vector3 axis_sq(p_axis.x*p_axis.x,p_axis.y*p_axis.y,p_axis.z*p_axis.z);
 
@@ -464,15 +510,15 @@ Matrix3::Matrix3(const Vector3& p_axis, real_t 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[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][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[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][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 );
 
 }
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 2792200b7d..1d967c03b8 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -91,6 +91,8 @@ public:
 		return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
 	}
 
+	bool isequal_approx(const Matrix3& a, const Matrix3& b) const;
+
 	bool operator==(const Matrix3& p_matrix) const;
 	bool operator!=(const Matrix3& p_matrix) const;
 
@@ -102,6 +104,9 @@ public:
 	int get_orthogonal_index() const;
 	void set_orthogonal_index(int p_index);
 
+	bool is_orthogonal() const;
+	bool is_rotation() const;
+
 	operator String() const;
 
 	void get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const;
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index 8aa06a2046..afe71100e1 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -27,22 +27,40 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 #include "quat.h"
+#include "matrix3.h"
 #include "print_string.h"
 
+// set_euler expects a vector containing the Euler angles in the format
+// (c,b,a), where a is the angle of the first rotation, and c is the last.
+// The current implementation uses XYZ convention (Z is the first rotation).
 void Quat::set_euler(const Vector3& p_euler) {
-	real_t half_yaw = p_euler.x * 0.5;
-	real_t half_pitch = p_euler.y * 0.5;
-	real_t half_roll = p_euler.z * 0.5;
-	real_t cos_yaw = Math::cos(half_yaw);
-	real_t sin_yaw = Math::sin(half_yaw);
-	real_t cos_pitch = Math::cos(half_pitch);
-	real_t sin_pitch = Math::sin(half_pitch);
-	real_t cos_roll = Math::cos(half_roll);
-	real_t sin_roll = Math::sin(half_roll);
-	set(cos_roll * sin_pitch * cos_yaw+sin_roll * cos_pitch * sin_yaw,
-		cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw,
-		sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw,
-		cos_roll * cos_pitch * cos_yaw+sin_roll * sin_pitch * sin_yaw);
+	real_t half_a1 = p_euler.x * 0.5;
+	real_t half_a2 = p_euler.y * 0.5;
+	real_t half_a3 = p_euler.z * 0.5;
+
+	// R = X(a1).Y(a2).Z(a3) convention for Euler angles.
+	// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-2)
+	// a3 is the angle of the first rotation, following the notation in this reference.
+
+	real_t cos_a1 = Math::cos(half_a1);
+	real_t sin_a1 = Math::sin(half_a1);
+	real_t cos_a2 = Math::cos(half_a2);
+	real_t sin_a2 = Math::sin(half_a2);
+	real_t cos_a3 = Math::cos(half_a3);
+	real_t sin_a3 = Math::sin(half_a3);
+
+	set(sin_a1*cos_a2*cos_a3 + sin_a2*sin_a3*cos_a1,
+		-sin_a1*sin_a3*cos_a2 + sin_a2*cos_a1*cos_a3,
+		sin_a1*sin_a2*cos_a3 + sin_a3*cos_a1*cos_a2,
+		-sin_a1*sin_a2*sin_a3 + cos_a1*cos_a2*cos_a3);
+}
+
+// get_euler returns a vector containing the Euler angles in the format
+// (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last.
+// The current implementation uses XYZ convention (Z is the first rotation).
+Vector3 Quat::get_euler() const {
+	Matrix3 m(*this);
+	return m.get_euler();
 }
 
 void Quat::operator*=(const Quat& q) {
@@ -126,26 +144,25 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const {
 	}
 #else
 
-	real_t         to1[4];
+	Quat          to1;
 	real_t        omega, cosom, sinom, scale0, scale1;
 
 
 	// calc cosine
-	cosom = x * q.x + y * q.y + z * q.z
-			+ w * q.w;
-
+	cosom = dot(q);
 
 	// adjust signs (if necessary)
 	if ( cosom <0.0 ) {
-		cosom = -cosom; to1[0] = - q.x;
-		to1[1] = - q.y;
-		to1[2] = - q.z;
-		to1[3] = - q.w;
+		cosom = -cosom;
+		to1.x = - q.x;
+		to1.y = - q.y;
+		to1.z = - q.z;
+		to1.w = - q.w;
 	} else  {
-		to1[0] = q.x;
-		to1[1] = q.y;
-		to1[2] = q.z;
-		to1[3] = q.w;
+		to1.x = q.x;
+		to1.y = q.y;
+		to1.z = q.z;
+		to1.w = q.w;
 	}
 
 
@@ -165,10 +182,10 @@ Quat Quat::slerp(const Quat& q, const real_t& t) const {
 	}
 	// calculate final values
 	return Quat(
-		scale0 * x + scale1 * to1[0],
-		scale0 * y + scale1 * to1[1],
-		scale0 * z + scale1 * to1[2],
-		scale0 * w + scale1 * to1[3]
+		scale0 * x + scale1 * to1.x,
+		scale0 * y + scale1 * to1.y,
+		scale0 * z + scale1 * to1.z,
+		scale0 * w + scale1 * to1.w
 	);
 #endif
 }
@@ -186,10 +203,10 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const {
 		newFactor	= Math::sin(t * theta) * sinT,
 		invFactor	= Math::sin((1.0f - t) * theta) * sinT;
 
-	return Quat( invFactor * from.x + newFactor * q.x,
-			   invFactor * from.y + newFactor * q.y,
-			   invFactor * from.z + newFactor * q.z,
-			   invFactor * from.w + newFactor * q.w );
+	return Quat(invFactor * from.x + newFactor * q.x,
+				invFactor * from.y + newFactor * q.y,
+				invFactor * from.z + newFactor * q.z,
+				invFactor * from.w + newFactor * q.w);
 
 #if 0
 	real_t         to1[4];
@@ -203,7 +220,7 @@ Quat Quat::slerpni(const Quat& q, const real_t& t) const {
 
 	// adjust signs (if necessary)
 	if ( cosom <0.0 && false) {
-		cosom = -cosom; to1[0] = - q.x;
+		cosom = -cosom;to1[0] = - q.x;
 		to1[1] = - q.y;
 		to1[2] = - q.z;
 		to1[3] = - q.w;
@@ -260,8 +277,10 @@ Quat::Quat(const Vector3& axis, const real_t& angle) {
 	if (d==0)
 		set(0,0,0,0);
 	else {
-		real_t s = Math::sin(-angle * 0.5) / d;
+		real_t sin_angle = Math::sin(angle * 0.5);
+		real_t cos_angle = Math::cos(angle * 0.5);
+		real_t s = sin_angle / d;
 		set(axis.x * s, axis.y * s, axis.z * s,
-			Math::cos(-angle * 0.5));
+			cos_angle);
 	}
 }
diff --git a/core/math/quat.h b/core/math/quat.h
index 9f4145cddb..40c048006f 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -49,15 +49,16 @@ public:
 	Quat inverse() const;
 	_FORCE_INLINE_ real_t dot(const Quat& q) const;
 	void set_euler(const Vector3& p_euler);
+	Vector3 get_euler() const;
 	Quat slerp(const Quat& q, const real_t& t) const;
 	Quat slerpni(const Quat& q, const real_t& t) const;
 	Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const;
 
 	_FORCE_INLINE_ void get_axis_and_angle(Vector3& r_axis, real_t &r_angle) const {
 		r_angle = 2 * Math::acos(w);
-		r_axis.x = -x / Math::sqrt(1-w*w);
-		r_axis.y = -y / Math::sqrt(1-w*w);
-		r_axis.z = -z / Math::sqrt(1-w*w);
+		r_axis.x = x / Math::sqrt(1-w*w);
+		r_axis.y = y / Math::sqrt(1-w*w);
+		r_axis.z = z / Math::sqrt(1-w*w);
 	}
 
 	void operator*=(const Quat& q);
@@ -183,12 +184,10 @@ Quat Quat::operator/(const real_t& s) const {
 
 
 bool Quat::operator==(const Quat& p_quat) const {
-
 	return x==p_quat.x && y==p_quat.y && z==p_quat.z && w==p_quat.w;
 }
 
 bool Quat::operator!=(const Quat& p_quat) const {
-
 	return x!=p_quat.x || y!=p_quat.y || z!=p_quat.z || w!=p_quat.w;
 }
 
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 3f451b0ab7..14cf1bc6ca 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -293,7 +293,6 @@ bool Vector3::operator==(const Vector3& p_v) const {
 }
 
 bool Vector3::operator!=(const Vector3& p_v) const {
-
 	return (x!=p_v.x || y!=p_v.y || z!=p_v.z);
 }