6 files changed, 65 insertions, 9 deletions
diff --git a/core/math/math_2d.h b/core/math/math_2d.h
index 611d47e3ff..25c39e5d7a 100644
--- a/core/math/math_2d.h
+++ b/core/math/math_2d.h
@@ -112,6 +112,7 @@ struct Vector2 {
 
 	_FORCE_INLINE_ static Vector2 linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t);
 	_FORCE_INLINE_ Vector2 linear_interpolate(const Vector2 &p_b, real_t p_t) const;
+	_FORCE_INLINE_ Vector2 slerp(const Vector2 &p_b, real_t p_t) const;
 	Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_t) const;
 
 	Vector2 slide(const Vector2 &p_normal) const;
@@ -263,6 +264,14 @@ Vector2 Vector2::linear_interpolate(const Vector2 &p_b, real_t p_t) const {
 	return res;
 }
 
+Vector2 Vector2::slerp(const Vector2 &p_b, real_t p_t) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_normalized() == false, Vector2());
+#endif
+	real_t theta = angle_to(p_b);
+	return rotated(theta * p_t);
+}
+
 Vector2 Vector2::linear_interpolate(const Vector2 &p_a, const Vector2 &p_b, real_t p_t) {
 
 	Vector2 res = p_a;
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index b0b05d1ec8..8ee8ccb457 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -826,3 +826,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));
+}
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index fd383fc673..63d4f5d79d 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -155,6 +155,8 @@ public:
 	bool is_diagonal() const;
 	bool is_rotation() const;
 
+	Basis slerp(const Basis &target, const real_t &t) const;
+
 	operator String() const;
 
 	/* create / set */
diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index 4f61401ac7..b938fc3cfd 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -98,6 +98,9 @@ void Quat::set_euler_yxz(const Vector3 &p_euler) {
 // and similar for other axes.
 // This implementation uses YXZ convention (Z is the first rotation).
 Vector3 Quat::get_euler_yxz() const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_normalized() == false, Vector3(0, 0, 0));
+#endif
 	Basis m(*this);
 	return m.get_euler_yxz();
 }
@@ -135,11 +138,17 @@ bool Quat::is_normalized() const {
 }
 
 Quat Quat::inverse() const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_normalized() == false, Quat(0, 0, 0, 0));
+#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));
+#endif
 	Quat to1;
 	real_t omega, cosom, sinom, scale0, scale1;
 
@@ -215,7 +224,10 @@ Quat::operator String() const {
 	return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w);
 }
 
-Quat::Quat(const Vector3 &axis, const real_t &angle) {
+void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND(axis.is_normalized() == false);
+#endif
 	real_t d = axis.length();
 	if (d == 0)
 		set(0, 0, 0, 0);
diff --git a/core/math/quat.h b/core/math/quat.h
index ebc924504b..3e1344a913 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -64,11 +64,13 @@ public:
 	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;
 
+	void set_axis_angle(const Vector3 &axis, const real_t &angle);
 	_FORCE_INLINE_ void get_axis_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);
+		real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
+		r_axis.x = x * r;
+		r_axis.y = y * r;
+		r_axis.z = z * r;
 	}
 
 	void operator*=(const Quat &q);
@@ -83,9 +85,9 @@ public:
 
 	_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
 
-		Quat q = *this * v;
-		q *= this->inverse();
-		return Vector3(q.x, q.y, q.z);
+		Vector3 u(x, y, z);
+		Vector3 uv = u.cross(v);
+		return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
 	}
 
 	_FORCE_INLINE_ void operator+=(const Quat &q);
@@ -115,7 +117,15 @@ public:
 		z = p_z;
 		w = p_w;
 	}
-	Quat(const Vector3 &axis, const real_t &angle);
+	Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); }
+
+	Quat(const Vector3 &euler) { set_euler(euler); }
+	Quat(const Quat &q) {
+		x = q.x;
+		y = q.y;
+		z = q.z;
+		w = q.w;
+	}
 
 	Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc
 	{
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 3bbfd7627c..433adf09ee 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -91,6 +91,7 @@ struct Vector3 {
 	/* Static Methods between 2 vector3s */
 
 	_FORCE_INLINE_ Vector3 linear_interpolate(const Vector3 &p_b, real_t p_t) const;
+	_FORCE_INLINE_ Vector3 slerp(const Vector3 &p_b, real_t p_t) const;
 	Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const;
 	Vector3 cubic_interpolaten(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_t) const;
 
@@ -218,6 +219,15 @@ Vector3 Vector3::linear_interpolate(const Vector3 &p_b, real_t p_t) const {
 			z + (p_t * (p_b.z - z)));
 }
 
+Vector3 Vector3::slerp(const Vector3 &p_b, real_t p_t) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V(is_normalized() == false, Vector3());
+#endif
+
+	real_t theta = angle_to(p_b);
+	return rotated(cross(p_b), theta * p_t);
+}
+
 real_t Vector3::distance_to(const Vector3 &p_b) const {
 
 	return (p_b - *this).length();