Add SLERP to Vector{2,3}, optimize Quat's Vector3 rotation.

Also even out Basis and Quat APIs a little.

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;

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));
+}

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 */

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);

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
 	{

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();

core/variant_call.cpp

@@ -340,6 +340,7 @@ struct _VariantCall {
 	VCALL_LOCALMEM1R(Vector2, angle_to);
 	VCALL_LOCALMEM1R(Vector2, angle_to_point);
 	VCALL_LOCALMEM2R(Vector2, linear_interpolate);
+	VCALL_LOCALMEM2R(Vector2, slerp);
 	VCALL_LOCALMEM4R(Vector2, cubic_interpolate);
 	VCALL_LOCALMEM1R(Vector2, rotated);
 	VCALL_LOCALMEM0R(Vector2, tangent);
@@ -380,6 +381,7 @@ struct _VariantCall {
 	VCALL_LOCALMEM1R(Vector3, snapped);
 	VCALL_LOCALMEM2R(Vector3, rotated);
 	VCALL_LOCALMEM2R(Vector3, linear_interpolate);
+	VCALL_LOCALMEM2R(Vector3, slerp);
 	VCALL_LOCALMEM4R(Vector3, cubic_interpolate);
 	VCALL_LOCALMEM1R(Vector3, dot);
 	VCALL_LOCALMEM1R(Vector3, cross);
@@ -439,6 +441,9 @@ struct _VariantCall {
 	VCALL_LOCALMEM2R(Quat, slerp);
 	VCALL_LOCALMEM2R(Quat, slerpni);
 	VCALL_LOCALMEM4R(Quat, cubic_slerp);
+	VCALL_LOCALMEM0R(Quat, get_euler);
+	VCALL_LOCALMEM1(Quat, set_euler);
+	VCALL_LOCALMEM2(Quat, set_axis_angle);
 
 	VCALL_LOCALMEM0R(Color, to_rgba32);
 	VCALL_LOCALMEM0R(Color, to_argb32);
@@ -876,6 +881,11 @@ struct _VariantCall {
 		r_ret = Quat(((Vector3)(*p_args[0])), ((float)(*p_args[1])));
 	}
 
+	static void Quat_init3(Variant &r_ret, const Variant **p_args) {
+
+		r_ret = Quat(((Vector3)(*p_args[0])));
+	}
+
 	static void Color_init1(Variant &r_ret, const Variant **p_args) {
 
 		r_ret = Color(*p_args[0], *p_args[1], *p_args[2], *p_args[3]);
@@ -1150,7 +1160,7 @@ Variant Variant::construct(const Variant::Type p_type, const Variant **p_args, i
 			case RECT2: return (Rect2(*p_args[0]));
 			case VECTOR3: return (Vector3(*p_args[0]));
 			case PLANE: return (Plane(*p_args[0]));
-			case QUAT: return (Quat(*p_args[0]));
+			case QUAT: return (p_args[0]->operator Quat());
 			case AABB:
 				return (::AABB(*p_args[0])); // 10
 			case BASIS: return (Basis(p_args[0]->operator Basis()));
@@ -1518,6 +1528,7 @@ void register_variant_methods() {
 	ADDFUNC1R(VECTOR2, REAL, Vector2, angle_to, VECTOR2, "to", varray());
 	ADDFUNC1R(VECTOR2, REAL, Vector2, angle_to_point, VECTOR2, "to", varray());
 	ADDFUNC2R(VECTOR2, VECTOR2, Vector2, linear_interpolate, VECTOR2, "b", REAL, "t", varray());
+	ADDFUNC2R(VECTOR2, VECTOR2, Vector2, slerp, VECTOR2, "b", REAL, "t", varray());
 	ADDFUNC4R(VECTOR2, VECTOR2, Vector2, cubic_interpolate, VECTOR2, "b", VECTOR2, "pre_a", VECTOR2, "post_b", REAL, "t", varray());
 	ADDFUNC1R(VECTOR2, VECTOR2, Vector2, rotated, REAL, "phi", varray());
 	ADDFUNC0R(VECTOR2, VECTOR2, Vector2, tangent, varray());
@@ -1557,6 +1568,7 @@ void register_variant_methods() {
 	ADDFUNC1R(VECTOR3, VECTOR3, Vector3, snapped, VECTOR3, "by", varray());
 	ADDFUNC2R(VECTOR3, VECTOR3, Vector3, rotated, VECTOR3, "axis", REAL, "phi", varray());
 	ADDFUNC2R(VECTOR3, VECTOR3, Vector3, linear_interpolate, VECTOR3, "b", REAL, "t", varray());
+	ADDFUNC2R(VECTOR3, VECTOR3, Vector3, slerp, VECTOR3, "b", REAL, "t", varray());
 	ADDFUNC4R(VECTOR3, VECTOR3, Vector3, cubic_interpolate, VECTOR3, "b", VECTOR3, "pre_a", VECTOR3, "post_b", REAL, "t", varray());
 	ADDFUNC1R(VECTOR3, REAL, Vector3, dot, VECTOR3, "b", varray());
 	ADDFUNC1R(VECTOR3, VECTOR3, Vector3, cross, VECTOR3, "b", varray());
@@ -1594,6 +1606,9 @@ void register_variant_methods() {
 	ADDFUNC2R(QUAT, QUAT, Quat, slerp, QUAT, "b", REAL, "t", varray());
 	ADDFUNC2R(QUAT, QUAT, Quat, slerpni, QUAT, "b", REAL, "t", varray());
 	ADDFUNC4R(QUAT, QUAT, Quat, cubic_slerp, QUAT, "b", QUAT, "pre_a", QUAT, "post_b", REAL, "t", varray());
+	ADDFUNC0R(QUAT, VECTOR3, Quat, get_euler, varray());
+	ADDFUNC1(QUAT, NIL, Quat, set_euler, VECTOR3, "euler", varray());
+	ADDFUNC2(QUAT, NIL, Quat, set_axis_angle, VECTOR3, "axis", REAL, "angle", varray());
 
 	ADDFUNC0R(COLOR, INT, Color, to_rgba32, varray());
 	ADDFUNC0R(COLOR, INT, Color, to_argb32, varray());
@@ -1816,6 +1831,7 @@ void register_variant_methods() {
 
 	_VariantCall::add_constructor(_VariantCall::Quat_init1, Variant::QUAT, "x", Variant::REAL, "y", Variant::REAL, "z", Variant::REAL, "w", Variant::REAL);
 	_VariantCall::add_constructor(_VariantCall::Quat_init2, Variant::QUAT, "axis", Variant::VECTOR3, "angle", Variant::REAL);
+	_VariantCall::add_constructor(_VariantCall::Quat_init3, Variant::QUAT, "euler", Variant::VECTOR3);
 
 	_VariantCall::add_constructor(_VariantCall::Color_init1, Variant::COLOR, "r", Variant::REAL, "g", Variant::REAL, "b", Variant::REAL, "a", Variant::REAL);
 	_VariantCall::add_constructor(_VariantCall::Color_init2, Variant::COLOR, "r", Variant::REAL, "g", Variant::REAL, "b", Variant::REAL);

doc/classes/Basis.xml

@@ -4,11 +4,12 @@
 		3x3 matrix datatype.
 	</brief_description>
 	<description>
-		3x3 matrix used for 3D rotation and scale. Contains 3 vector fields x,y and z as its columns, which can be interpreted as the local basis vectors of a transformation. Can also be accessed as array of 3D vectors. These vectors are orthogonal to each other, but are not necessarily normalized. Almost always used as orthogonal basis for a [Transform].
+		3x3 matrix used for 3D rotation and scale. Contains 3 vector fields x,y and z as its columns, which can be interpreted as the local basis vectors of a transformation. Can also be accessed as array of 3D vectors. These vectors are orthogonal to each other, but are not necessarily normalized (due to scaling). Almost always used as orthogonal basis for a [Transform].
 		For such use, it is composed of a scaling and a rotation matrix, in that order (M = R.S).
 	</description>
 	<tutorials>
 		http://docs.godotengine.org/en/latest/tutorials/3d/using_transforms.html
+		http://docs.godotengine.org/en/latest/tutorials/math/rotations.html
 	</tutorials>
 	<demos>
 	</demos>
@@ -105,7 +106,7 @@
 			<argument index="1" name="phi" type="float">
 			</argument>
 			<description>
-				Introduce an additional rotation around the given axis by phi (radians). Only relevant when the matrix is being used as a part of [Transform]. The axis must be a normalized vector.
+				Introduce an additional rotation around the given axis by phi (radians). The axis must be a normalized vector.
 			</description>
 		</method>
 		<method name="scaled">
@@ -114,7 +115,18 @@
 			<argument index="0" name="scale" type="Vector3">
 			</argument>
 			<description>
-				Introduce an additional scaling specified by the given 3D scaling factor. Only relevant when the matrix is being used as a part of [Transform].
+				Introduce an additional scaling specified by the given 3D scaling factor.
+			</description>
+		</method>
+		<method name="slerp">
+			<return type="Basis">
+			</return>
+			<argument index="0" name="b" type="Basis">
+			</argument>
+			<argument index="1" name="t" type="float">
+			</argument>
+			<description>
+				Assuming that the matrix is a proper rotation matrix, slerp performs a spherical-linear interpolation with another rotation matrix.
 			</description>
 		</method>
 		<method name="tdotx">

doc/classes/Quat.xml

@@ -4,13 +4,14 @@
 		Quaternion.
 	</brief_description>
 	<description>
-		A 4-dimensional vector representing a rotation.
-		The vector represents a 4 dimensional complex number where multiplication of the basis elements is not commutative (multiplying i with j gives a different result than multiplying j with i).
-		Multiplying quaternions reproduces rotation sequences. However quaternions need to be often renormalized, or else they suffer from precision issues.
-		It can be used to perform SLERP (spherical-linear interpolation) between two rotations.
+		A unit quaternion used for representing 3D rotations.
+		It is similar to [Basis], which implements matrix representation of rotations, and can be parametrized using both an axis-angle pair or Euler angles. But due to its compactness and the way it is stored in memory, certain operations (obtaining axis-angle and performing SLERP, in particular) are more efficient and robust against floating point errors.
+
+		Quaternions need to be (re)normalized.
 	</description>
 	<tutorials>
 		http://docs.godotengine.org/en/latest/tutorials/3d/using_transforms.html#interpolating-with-quaternions
+		http://docs.godotengine.org/en/latest/tutorials/math/rotations.html
 	</tutorials>
 	<demos>
 	</demos>
@@ -41,6 +42,15 @@
 				Returns a quaternion that will rotate around the given axis by the specified angle. The axis must be a normalized vector.
 			</description>
 		</method>
+		<method name="Quat">
+			<return type="Quat">
+			</return>
+			<argument index="0" name="euler" type="Vector3">
+			</argument>
+			<description>
+				Returns a quaternion that will perform a rotation specified by Euler angles (in the YXZ convention: first Z, then X, and Y last), given in the vector format as (X-angle, Y-angle, Z-angle).
+			</description>
+		</method>
 		<method name="Quat">
 			<return type="Quat">
 			</return>
@@ -74,6 +84,13 @@
 				Returns the dot product of two quaternions.
 			</description>
 		</method>
+		<method name="get_euler">
+			<return type="Vector3">
+			</return>
+			<description>
+				Return Euler angles (in the YXZ convention: first Z, then X, and Y last) corresponding to the rotation represented by the unit quaternion. Returned vector contains the rotation angles in the format (X-angle, Y-angle, Z-angle).
+			</description>
+		</method>
 		<method name="inverse">
 			<return type="Quat">
 			</return>
@@ -109,6 +126,22 @@
 				Returns a copy of the quaternion, normalized to unit length.
 			</description>
 		</method>
+		<method name="set_axis_angle">
+			<argument index="0" name="axis" type="Vector3">
+			</argument>
+			<argument index="1" name="phi" type="float">
+			</argument>
+			<description>
+				Set the quaternion to a rotation which rotates around axis by the specified angle, in radians. The axis must be a normalized vector.
+			</description>
+		</method>
+		<method name="set_euler">
+			<argument index="0" name="euler" type="Vector3">
+			</argument>
+			<description>
+				Set the quaternion to a rotation specified by Euler angles (in the YXZ convention: first Z, then X, and Y last), given in the vector format as (X-angle, Y-angle, Z-angle).
+			</description>
+		</method>
 		<method name="slerp">
 			<return type="Quat">
 			</return>

doc/classes/Vector2.xml

@@ -208,6 +208,18 @@
 			<description>
 			</description>
 		</method>
+		<method name="slerp">
+			<return type="Vector2">
+			</return>
+			<argument index="0" name="b" type="Vector2">
+			</argument>
+			<argument index="1" name="t" type="float">
+			</argument>
+			<description>
+				Returns the result of SLERP between this vector and "b", by amount "t". "t" should be a float of 0.0-1.0, a percentage of how far along the interpolation is.
+				Both vectors need to be normalized.
+			</description>
+		</method>
 		<method name="slide">
 			<return type="Vector2">
 			</return>

doc/classes/Vector3.xml

@@ -210,6 +210,18 @@
 			<description>
 			</description>
 		</method>
+		<method name="slerp">
+			<return type="Vector3">
+			</return>
+			<argument index="0" name="b" type="Vector3">
+			</argument>
+			<argument index="1" name="t" type="float">
+			</argument>
+			<description>
+				Returns the result of SLERP between this vector and "b", by amount "t". "t" should be a float of 0.0-1.0, a percentage of how far along the interpolation is.
+				Both vectors need to be normalized.
+			</description>
+		</method>
 		<method name="slide">
 			<return type="Vector3">
 			</return>