Update Basis/Transform3D/Quaternion to match the engine

include/godot_cpp/core/math.hpp

@@ -537,6 +537,27 @@ inline bool is_zero_approx(double s) {
 	return abs(s) < CMP_EPSILON;
 }
 
+inline float absf(float g) {
+	union {
+		float f;
+		uint32_t i;
+	} u;
+
+	u.f = g;
+	u.i &= 2147483647u;
+	return u.f;
+}
+
+inline double absd(double g) {
+	union {
+		double d;
+		uint64_t i;
+	} u;
+	u.d = g;
+	u.i &= (uint64_t)9223372036854775807ull;
+	return u.d;
+}
+
 inline double smoothstep(double p_from, double p_to, double p_weight) {
 	if (is_equal_approx(static_cast<real_t>(p_from), static_cast<real_t>(p_to))) {
 		return p_from;

include/godot_cpp/variant/basis.hpp

@@ -49,10 +49,10 @@ public:
 		Vector3(0, 0, 1)
 	};
 
-	inline const Vector3 &operator[](int axis) const {
+	_FORCE_INLINE_ const Vector3 &operator[](int axis) const {
 		return rows[axis];
 	}
-	inline Vector3 &operator[](int axis) {
+	_FORCE_INLINE_ Vector3 &operator[](int axis) {
 		return rows[axis];
 	}
 
@@ -62,67 +62,53 @@ public:
 	Basis inverse() const;
 	Basis transposed() const;
 
-	inline real_t determinant() const;
+	_FORCE_INLINE_ real_t determinant() const;
 
-	void from_z(const Vector3 &p_z);
+	enum EulerOrder {
+		EULER_ORDER_XYZ,
+		EULER_ORDER_XZY,
+		EULER_ORDER_YXZ,
+		EULER_ORDER_YZX,
+		EULER_ORDER_ZXY,
+		EULER_ORDER_ZYX
+	};
 
-	inline Vector3 get_axis(int p_axis) const {
-		// get actual basis axis (rows is transposed for performance)
-		return Vector3(rows[0][p_axis], rows[1][p_axis], rows[2][p_axis]);
-	}
-	inline void set_axis(int p_axis, const Vector3 &p_value) {
-		// get actual basis axis (rows is transposed for performance)
-		rows[0][p_axis] = p_value.x;
-		rows[1][p_axis] = p_value.y;
-		rows[2][p_axis] = p_value.z;
-	}
+	void from_z(const Vector3 &p_z);
 
-	void rotate(const Vector3 &p_axis, real_t p_phi);
-	Basis rotated(const Vector3 &p_axis, real_t p_phi) const;
+	void rotate(const Vector3 &p_axis, real_t p_angle);
+	Basis rotated(const Vector3 &p_axis, real_t p_angle) const;
 
-	void rotate_local(const Vector3 &p_axis, real_t p_phi);
-	Basis rotated_local(const Vector3 &p_axis, real_t p_phi) const;
+	void rotate_local(const Vector3 &p_axis, real_t p_angle);
+	Basis rotated_local(const Vector3 &p_axis, real_t p_angle) const;
 
-	void rotate(const Vector3 &p_euler);
-	Basis rotated(const Vector3 &p_euler) const;
+	void rotate(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ);
+	Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) const;
 
-	void rotate(const Quaternion &p_quat);
-	Basis rotated(const Quaternion &p_quat) const;
+	void rotate(const Quaternion &p_quaternion);
+	Basis rotated(const Quaternion &p_quaternion) const;
 
-	Vector3 get_rotation_euler() const;
+	Vector3 get_euler_normalized(EulerOrder p_order = EULER_ORDER_YXZ) const;
 	void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
 	void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const;
 	Quaternion get_rotation_quaternion() const;
-	Vector3 get_rotation() const { return get_rotation_euler(); };
 
-	Vector3 rotref_posscale_decomposition(Basis &rotref) const;
+	void rotate_to_align(Vector3 p_start_direction, Vector3 p_end_direction);
 
-	Vector3 get_euler_xyz() const;
-	void set_euler_xyz(const Vector3 &p_euler);
-
-	Vector3 get_euler_xzy() const;
-	void set_euler_xzy(const Vector3 &p_euler);
-
-	Vector3 get_euler_yzx() const;
-	void set_euler_yzx(const Vector3 &p_euler);
-
-	Vector3 get_euler_yxz() const;
-	void set_euler_yxz(const Vector3 &p_euler);
-
-	Vector3 get_euler_zxy() const;
-	void set_euler_zxy(const Vector3 &p_euler);
+	Vector3 rotref_posscale_decomposition(Basis &rotref) const;
 
-	Vector3 get_euler_zyx() const;
-	void set_euler_zyx(const Vector3 &p_euler);
+	Vector3 get_euler(EulerOrder p_order = EULER_ORDER_YXZ) const;
+	void set_euler(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ);
+	static Basis from_euler(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) {
+		Basis b;
+		b.set_euler(p_euler, p_order);
+		return b;
+	}
 
 	Quaternion get_quaternion() const;
-	void set_quaternion(const Quaternion &p_quat);
-
-	Vector3 get_euler() const { return get_euler_yxz(); }
-	void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }
+	void set_quaternion(const Quaternion &p_quaternion);
 
 	void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
-	void set_axis_angle(const Vector3 &p_axis, real_t p_phi);
+	void set_axis_angle(const Vector3 &p_axis, real_t p_angle);
 
 	void scale(const Vector3 &p_scale);
 	Basis scaled(const Vector3 &p_scale) const;
@@ -130,6 +116,9 @@ public:
 	void scale_local(const Vector3 &p_scale);
 	Basis scaled_local(const Vector3 &p_scale) const;
 
+	void scale_orthogonal(const Vector3 &p_scale);
+	Basis scaled_orthogonal(const Vector3 &p_scale) const;
+
 	void make_scale_uniform();
 	float get_uniform_scale() const;
 
@@ -137,18 +126,18 @@ public:
 	Vector3 get_scale_abs() const;
 	Vector3 get_scale_local() const;
 
-	void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale);
-	void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale);
-	void set_quaternion_scale(const Quaternion &p_quat, const Vector3 &p_scale);
+	void set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale);
+	void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EULER_ORDER_YXZ);
+	void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale);
 
 	// transposed dot products
-	inline real_t tdotx(const Vector3 &v) const {
+	_FORCE_INLINE_ real_t tdotx(const Vector3 &v) const {
 		return rows[0][0] * v[0] + rows[1][0] * v[1] + rows[2][0] * v[2];
 	}
-	inline real_t tdoty(const Vector3 &v) const {
+	_FORCE_INLINE_ real_t tdoty(const Vector3 &v) const {
 		return rows[0][1] * v[0] + rows[1][1] * v[1] + rows[2][1] * v[2];
 	}
-	inline real_t tdotz(const Vector3 &v) const {
+	_FORCE_INLINE_ real_t tdotz(const Vector3 &v) const {
 		return rows[0][2] * v[0] + rows[1][2] * v[1] + rows[2][2] * v[2];
 	}
 
@@ -157,26 +146,22 @@ public:
 	bool operator==(const Basis &p_matrix) const;
 	bool operator!=(const Basis &p_matrix) const;
 
-	inline Vector3 xform(const Vector3 &p_vector) const;
-	inline Vector3 xform_inv(const Vector3 &p_vector) const;
-	inline void operator*=(const Basis &p_matrix);
-	inline Basis operator*(const Basis &p_matrix) const;
-	inline void operator+=(const Basis &p_matrix);
-	inline Basis operator+(const Basis &p_matrix) const;
-	inline void operator-=(const Basis &p_matrix);
-	inline Basis operator-(const Basis &p_matrix) const;
-	inline void operator*=(real_t p_val);
-	inline Basis operator*(real_t p_val) const;
-
-	int get_orthogonal_index() const;
-	void set_orthogonal_index(int p_index);
-
-	void set_diagonal(const Vector3 &p_diag);
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ void operator*=(const Basis &p_matrix);
+	_FORCE_INLINE_ Basis operator*(const Basis &p_matrix) const;
+	_FORCE_INLINE_ void operator+=(const Basis &p_matrix);
+	_FORCE_INLINE_ Basis operator+(const Basis &p_matrix) const;
+	_FORCE_INLINE_ void operator-=(const Basis &p_matrix);
+	_FORCE_INLINE_ Basis operator-(const Basis &p_matrix) const;
+	_FORCE_INLINE_ void operator*=(const real_t p_val);
+	_FORCE_INLINE_ Basis operator*(const real_t p_val) const;
 
 	bool is_orthogonal() const;
 	bool is_diagonal() const;
 	bool is_rotation() const;
 
+	Basis lerp(const Basis &p_to, const real_t &p_weight) const;
 	Basis slerp(const Basis &p_to, const real_t &p_weight) const;
 	void rotate_sh(real_t *p_values);
 
@@ -184,7 +169,7 @@ public:
 
 	/* create / set */
 
-	inline void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) {
+	_FORCE_INLINE_ void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) {
 		rows[0][0] = xx;
 		rows[0][1] = xy;
 		rows[0][2] = xz;
@@ -195,35 +180,35 @@ public:
 		rows[2][1] = zy;
 		rows[2][2] = zz;
 	}
-	inline void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
-		set_axis(0, p_x);
-		set_axis(1, p_y);
-		set_axis(2, p_z);
-	}
-	inline Vector3 get_column(int i) const {
-		return Vector3(rows[0][i], rows[1][i], rows[2][i]);
+	_FORCE_INLINE_ void set_columns(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
+		set_column(0, p_x);
+		set_column(1, p_y);
+		set_column(2, p_z);
 	}
 
-	inline Vector3 get_row(int i) const {
-		return Vector3(rows[i][0], rows[i][1], rows[i][2]);
+	_FORCE_INLINE_ Vector3 get_column(int p_index) const {
+		// Get actual basis axis column (we store transposed as rows for performance).
+		return Vector3(rows[0][p_index], rows[1][p_index], rows[2][p_index]);
 	}
-	inline Vector3 get_main_diagonal() const {
-		return Vector3(rows[0][0], rows[1][1], rows[2][2]);
+
+	_FORCE_INLINE_ void set_column(int p_index, const Vector3 &p_value) {
+		// Set actual basis axis column (we store transposed as rows for performance).
+		rows[0][p_index] = p_value.x;
+		rows[1][p_index] = p_value.y;
+		rows[2][p_index] = p_value.z;
 	}
 
-	inline void set_row(int i, const Vector3 &p_row) {
-		rows[i][0] = p_row.x;
-		rows[i][1] = p_row.y;
-		rows[i][2] = p_row.z;
+	_FORCE_INLINE_ Vector3 get_main_diagonal() const {
+		return Vector3(rows[0][0], rows[1][1], rows[2][2]);
 	}
 
-	inline void set_zero() {
+	_FORCE_INLINE_ void set_zero() {
 		rows[0].zero();
 		rows[1].zero();
 		rows[2].zero();
 	}
 
-	inline Basis transpose_xform(const Basis &m) const {
+	_FORCE_INLINE_ Basis transpose_xform(const Basis &m) const {
 		return Basis(
 				rows[0].x * m[0].x + rows[1].x * m[1].x + rows[2].x * m[2].x,
 				rows[0].x * m[0].y + rows[1].x * m[1].y + rows[2].x * m[2].y,
@@ -242,6 +227,9 @@ public:
 	void orthonormalize();
 	Basis orthonormalized() const;
 
+	void orthogonalize();
+	Basis orthogonalized() const;
+
 #ifdef MATH_CHECKS
 	bool is_symmetric() const;
 #endif
@@ -249,69 +237,71 @@ public:
 
 	operator Quaternion() const { return get_quaternion(); }
 
-	Basis(const Quaternion &p_quat) { set_quaternion(p_quat); };
-	Basis(const Quaternion &p_quat, const Vector3 &p_scale) { set_quaternion_scale(p_quat, p_scale); }
+	static Basis looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0));
 
-	Basis(const Vector3 &p_euler) { set_euler(p_euler); }
-	Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); }
+	Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); };
+	Basis(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_quaternion_scale(p_quaternion, p_scale); }
 
-	Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); }
-	Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); }
+	Basis(const Vector3 &p_axis, real_t p_angle) { set_axis_angle(p_axis, p_angle); }
+	Basis(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_angle, p_scale); }
+	static Basis from_scale(const Vector3 &p_scale);
 
-	inline Basis(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
-		set_axis(0, p_x);
-		set_axis(1, p_y);
-		set_axis(2, p_z);
+	_FORCE_INLINE_ Basis(const Vector3 &p_x_axis, const Vector3 &p_y_axis, const Vector3 &p_z_axis) {
+		set_columns(p_x_axis, p_y_axis, p_z_axis);
 	}
 
-	inline Basis() {}
+	_FORCE_INLINE_ Basis() {}
+
+private:
+	// Helper method.
+	void _set_diagonal(const Vector3 &p_diag);
 };
 
-inline void Basis::operator*=(const Basis &p_matrix) {
+_FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) {
 	set(
 			p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]),
 			p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]),
 			p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2]));
 }
 
-inline Basis Basis::operator*(const Basis &p_matrix) const {
+_FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const {
 	return Basis(
 			p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]),
 			p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]),
 			p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2]));
 }
 
-inline void Basis::operator+=(const Basis &p_matrix) {
+_FORCE_INLINE_ void Basis::operator+=(const Basis &p_matrix) {
 	rows[0] += p_matrix.rows[0];
 	rows[1] += p_matrix.rows[1];
 	rows[2] += p_matrix.rows[2];
 }
 
-inline Basis Basis::operator+(const Basis &p_matrix) const {
+_FORCE_INLINE_ Basis Basis::operator+(const Basis &p_matrix) const {
 	Basis ret(*this);
 	ret += p_matrix;
 	return ret;
 }
 
-inline void Basis::operator-=(const Basis &p_matrix) {
+_FORCE_INLINE_ void Basis::operator-=(const Basis &p_matrix) {
 	rows[0] -= p_matrix.rows[0];
 	rows[1] -= p_matrix.rows[1];
 	rows[2] -= p_matrix.rows[2];
 }
 
-inline Basis Basis::operator-(const Basis &p_matrix) const {
+_FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const {
 	Basis ret(*this);
 	ret -= p_matrix;
 	return ret;
 }
 
-inline void Basis::operator*=(real_t p_val) {
+_FORCE_INLINE_ void Basis::operator*=(const real_t p_val) {
 	rows[0] *= p_val;
 	rows[1] *= p_val;
 	rows[2] *= p_val;
 }
 
-inline Basis Basis::operator*(real_t p_val) const {
+_FORCE_INLINE_ Basis Basis::operator*(const real_t p_val) const {
 	Basis ret(*this);
 	ret *= p_val;
 	return ret;
@@ -333,8 +323,8 @@ Vector3 Basis::xform_inv(const Vector3 &p_vector) const {
 
 real_t Basis::determinant() const {
 	return rows[0][0] * (rows[1][1] * rows[2][2] - rows[2][1] * rows[1][2]) -
-		   rows[1][0] * (rows[0][1] * rows[2][2] - rows[2][1] * rows[0][2]) +
-		   rows[2][0] * (rows[0][1] * rows[1][2] - rows[1][1] * rows[0][2]);
+			rows[1][0] * (rows[0][1] * rows[2][2] - rows[2][1] * rows[0][2]) +
+			rows[2][0] * (rows[0][1] * rows[1][2] - rows[1][1] * rows[0][2]);
 }
 
 } // namespace godot

include/godot_cpp/variant/quaternion.hpp

@@ -28,8 +28,8 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
 
-#ifndef GODOT_QUAT_HPP
-#define GODOT_QUAT_HPP
+#ifndef GODOT_QUATERNION_HPP
+#define GODOT_QUATERNION_HPP
 
 #include <godot_cpp/core/math.hpp>
 #include <godot_cpp/variant/vector3.hpp>
@@ -52,20 +52,23 @@ public:
 		real_t components[4] = { 0, 0, 0, 1.0 };
 	};
 
-	inline real_t &operator[](int idx) {
+	_FORCE_INLINE_ real_t &operator[](int idx) {
 		return components[idx];
 	}
-	inline const real_t &operator[](int idx) const {
+	_FORCE_INLINE_ const real_t &operator[](int idx) const {
 		return components[idx];
 	}
-	inline real_t length_squared() const;
-	bool is_equal_approx(const Quaternion &p_quat) const;
+	_FORCE_INLINE_ real_t length_squared() const;
+	bool is_equal_approx(const Quaternion &p_quaternion) const;
 	real_t length() const;
 	void normalize();
 	Quaternion normalized() const;
 	bool is_normalized() const;
 	Quaternion inverse() const;
-	inline real_t dot(const Quaternion &p_q) const;
+	Quaternion log() const;
+	Quaternion exp() const;
+	_FORCE_INLINE_ real_t dot(const Quaternion &p_q) const;
+	real_t angle_to(const Quaternion &p_to) const;
 
 	Vector3 get_euler_xyz() const;
 	Vector3 get_euler_yxz() const;
@@ -73,9 +76,13 @@ public:
 
 	Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const;
 	Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const;
-	Quaternion cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const;
+	Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const;
+	Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
 
-	inline void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
+	Vector3 get_axis() const;
+	real_t get_angle() const;
+
+	_FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
 		r_angle = 2 * Math::acos(w);
 		real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
 		r_axis.x = x * r;
@@ -86,44 +93,37 @@ public:
 	void operator*=(const Quaternion &p_q);
 	Quaternion operator*(const Quaternion &p_q) const;
 
-	Quaternion operator*(const Vector3 &v) const {
-		return Quaternion(w * v.x + y * v.z - z * v.y,
-				w * v.y + z * v.x - x * v.z,
-				w * v.z + x * v.y - y * v.x,
-				-x * v.x - y * v.y - z * v.z);
-	}
-
-	inline Vector3 xform(const Vector3 &v) const {
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
 #ifdef MATH_CHECKS
-		ERR_FAIL_COND_V(!is_normalized(), v);
+		ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized.");
 #endif
 		Vector3 u(x, y, z);
 		Vector3 uv = u.cross(v);
 		return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
 	}
 
-	inline Vector3 xform_inv(const Vector3 &v) const {
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &v) const {
 		return inverse().xform(v);
 	}
 
-	inline void operator+=(const Quaternion &p_q);
-	inline void operator-=(const Quaternion &p_q);
-	inline void operator*=(const real_t &s);
-	inline void operator/=(const real_t &s);
-	inline Quaternion operator+(const Quaternion &q2) const;
-	inline Quaternion operator-(const Quaternion &q2) const;
-	inline Quaternion operator-() const;
-	inline Quaternion operator*(const real_t &s) const;
-	inline Quaternion operator/(const real_t &s) const;
+	_FORCE_INLINE_ void operator+=(const Quaternion &p_q);
+	_FORCE_INLINE_ void operator-=(const Quaternion &p_q);
+	_FORCE_INLINE_ void operator*=(const real_t &s);
+	_FORCE_INLINE_ void operator/=(const real_t &s);
+	_FORCE_INLINE_ Quaternion operator+(const Quaternion &q2) const;
+	_FORCE_INLINE_ Quaternion operator-(const Quaternion &q2) const;
+	_FORCE_INLINE_ Quaternion operator-() const;
+	_FORCE_INLINE_ Quaternion operator*(const real_t &s) const;
+	_FORCE_INLINE_ Quaternion operator/(const real_t &s) const;
 
-	inline bool operator==(const Quaternion &p_quat) const;
-	inline bool operator!=(const Quaternion &p_quat) const;
+	_FORCE_INLINE_ bool operator==(const Quaternion &p_quaternion) const;
+	_FORCE_INLINE_ bool operator!=(const Quaternion &p_quaternion) const;
 
 	operator String() const;
 
-	inline Quaternion() {}
+	_FORCE_INLINE_ Quaternion() {}
 
-	inline Quaternion(real_t p_x, real_t p_y, real_t p_z, real_t p_w) :
+	_FORCE_INLINE_ Quaternion(real_t p_x, real_t p_y, real_t p_z, real_t p_w) :
 			x(p_x),
 			y(p_y),
 			z(p_z),
@@ -141,12 +141,11 @@ public:
 			w(p_q.w) {
 	}
 
-	Quaternion &operator=(const Quaternion &p_q) {
+	void operator=(const Quaternion &p_q) {
 		x = p_q.x;
 		y = p_q.y;
 		z = p_q.z;
 		w = p_q.w;
-		return *this;
 	}
 
 	Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc
@@ -154,19 +153,19 @@ public:
 		Vector3 c = v0.cross(v1);
 		real_t d = v0.dot(v1);
 
-		if (d < (real_t)-1.0 + CMP_EPSILON) {
-			x = (real_t)0.0;
-			y = (real_t)1.0;
-			z = (real_t)0.0;
-			w = (real_t)0.0;
+		if (d < -1.0f + (real_t)CMP_EPSILON) {
+			x = 0;
+			y = 1;
+			z = 0;
+			w = 0;
 		} else {
-			real_t s = Math::sqrt(((real_t)1.0 + d) * (real_t)2.0);
-			real_t rs = (real_t)1.0 / s;
+			real_t s = Math::sqrt((1.0f + d) * 2.0f);
+			real_t rs = 1.0f / s;
 
 			x = c.x * rs;
 			y = c.y * rs;
 			z = c.z * rs;
-			w = s * (real_t)0.5;
+			w = s * 0.5f;
 		}
 	}
 };
@@ -201,7 +200,7 @@ void Quaternion::operator*=(const real_t &s) {
 }
 
 void Quaternion::operator/=(const real_t &s) {
-	*this *= (real_t)1.0 / s;
+	*this *= 1.0f / s;
 }
 
 Quaternion Quaternion::operator+(const Quaternion &q2) const {
@@ -224,21 +223,21 @@ Quaternion Quaternion::operator*(const real_t &s) const {
 }
 
 Quaternion Quaternion::operator/(const real_t &s) const {
-	return *this * ((real_t)1.0 / s);
+	return *this * (1.0f / s);
 }
 
-bool Quaternion::operator==(const Quaternion &p_quat) const {
-	return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w;
+bool Quaternion::operator==(const Quaternion &p_quaternion) const {
+	return x == p_quaternion.x && y == p_quaternion.y && z == p_quaternion.z && w == p_quaternion.w;
 }
 
-bool Quaternion::operator!=(const Quaternion &p_quat) const {
-	return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w;
+bool Quaternion::operator!=(const Quaternion &p_quaternion) const {
+	return x != p_quaternion.x || y != p_quaternion.y || z != p_quaternion.z || w != p_quaternion.w;
 }
 
-inline Quaternion operator*(const real_t &p_real, const Quaternion &p_quat) {
-	return p_quat * p_real;
+_FORCE_INLINE_ Quaternion operator*(const real_t &p_real, const Quaternion &p_quaternion) {
+	return p_quaternion * p_real;
 }
 
 } // namespace godot
 
-#endif // GODOT_QUAT_HPP
+#endif // GODOT_QUATERNION_HPP

include/godot_cpp/variant/transform3d.hpp

@@ -54,11 +54,11 @@ public:
 	void affine_invert();
 	Transform3D affine_inverse() const;
 
-	Transform3D rotated(const Vector3 &p_axis, real_t p_phi) const;
+	Transform3D rotated(const Vector3 &p_axis, real_t p_angle) const;
 	Transform3D rotated_local(const Vector3 &p_axis, real_t p_angle) const;
 
-	void rotate(const Vector3 &p_axis, real_t p_phi);
-	void rotate_basis(const Vector3 &p_axis, real_t p_phi);
+	void rotate(const Vector3 &p_axis, real_t p_angle);
+	void rotate_basis(const Vector3 &p_axis, real_t p_angle);
 
 	void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0));
 	Transform3D looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0)) const;
@@ -67,8 +67,8 @@ public:
 	Transform3D scaled(const Vector3 &p_scale) const;
 	Transform3D scaled_local(const Vector3 &p_scale) const;
 	void scale_basis(const Vector3 &p_scale);
-	void translate(real_t p_tx, real_t p_ty, real_t p_tz);
-	void translate(const Vector3 &p_translation);
+	void translate_local(real_t p_tx, real_t p_ty, real_t p_tz);
+	void translate_local(const Vector3 &p_translation);
 	Transform3D translated(const Vector3 &p_translation) const;
 	Transform3D translated_local(const Vector3 &p_translation) const;
 
@@ -80,29 +80,41 @@ public:
 
 	void orthonormalize();
 	Transform3D orthonormalized() const;
+	void orthogonalize();
+	Transform3D orthogonalized() const;
 	bool is_equal_approx(const Transform3D &p_transform) const;
 
 	bool operator==(const Transform3D &p_transform) const;
 	bool operator!=(const Transform3D &p_transform) const;
 
-	inline Vector3 xform(const Vector3 &p_vector) const;
-	inline Vector3 xform_inv(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ AABB xform(const AABB &p_aabb) const;
+	_FORCE_INLINE_ PackedVector3Array xform(const PackedVector3Array &p_array) const;
 
-	inline Plane xform(const Plane &p_plane) const;
-	inline Plane xform_inv(const Plane &p_plane) const;
+	// NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results.
+	// They use the transpose.
+	// For safe inverse transforms, xform by the affine_inverse.
+	_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
+	_FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const;
+	_FORCE_INLINE_ PackedVector3Array xform_inv(const PackedVector3Array &p_array) const;
 
-	inline AABB xform(const AABB &p_aabb) const;
-	inline AABB xform_inv(const AABB &p_aabb) const;
+	// Safe with non-uniform scaling (uses affine_inverse).
+	_FORCE_INLINE_ Plane xform(const Plane &p_plane) const;
+	_FORCE_INLINE_ Plane xform_inv(const Plane &p_plane) const;
 
-	inline PackedVector3Array xform(const PackedVector3Array &p_array) const;
-	inline PackedVector3Array xform_inv(const PackedVector3Array &p_array) const;
+	// These fast versions use precomputed affine inverse, and should be used in bottleneck areas where
+	// multiple planes are to be transformed.
+	_FORCE_INLINE_ Plane xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const;
+	static _FORCE_INLINE_ Plane xform_inv_fast(const Plane &p_plane, const Transform3D &p_inverse, const Basis &p_basis_transpose);
 
 	void operator*=(const Transform3D &p_transform);
 	Transform3D operator*(const Transform3D &p_transform) const;
+	void operator*=(const real_t p_val);
+	Transform3D operator*(const real_t p_val) const;
 
 	Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const;
 
-	inline Transform3D inverse_xform(const Transform3D &t) const {
+	_FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const {
 		Vector3 v = t.origin - origin;
 		return Transform3D(basis.transpose_xform(t.basis),
 				basis.xform(v));
@@ -123,14 +135,14 @@ public:
 	Transform3D(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz);
 };
 
-inline Vector3 Transform3D::xform(const Vector3 &p_vector) const {
+_FORCE_INLINE_ Vector3 Transform3D::xform(const Vector3 &p_vector) const {
 	return Vector3(
 			basis[0].dot(p_vector) + origin.x,
 			basis[1].dot(p_vector) + origin.y,
 			basis[2].dot(p_vector) + origin.z);
 }
 
-inline Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const {
+_FORCE_INLINE_ Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const {
 	Vector3 v = p_vector - origin;
 
 	return Vector3(
@@ -139,34 +151,24 @@ inline Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const {
 			(basis.rows[0][2] * v.x) + (basis.rows[1][2] * v.y) + (basis.rows[2][2] * v.z));
 }
 
-inline Plane Transform3D::xform(const Plane &p_plane) const {
-	Vector3 point = p_plane.normal * p_plane.d;
-	Vector3 point_dir = point + p_plane.normal;
-	point = xform(point);
-	point_dir = xform(point_dir);
-
-	Vector3 normal = point_dir - point;
-	normal.normalize();
-	real_t d = normal.dot(point);
-
-	return Plane(normal, d);
+// Neither the plane regular xform or xform_inv are particularly efficient,
+// as they do a basis inverse. For xforming a large number
+// of planes it is better to pre-calculate the inverse transpose basis once
+// and reuse it for each plane, by using the 'fast' version of the functions.
+_FORCE_INLINE_ Plane Transform3D::xform(const Plane &p_plane) const {
+	Basis b = basis.inverse();
+	b.transpose();
+	return xform_fast(p_plane, b);
 }
 
-inline Plane Transform3D::xform_inv(const Plane &p_plane) const {
-	Vector3 point = p_plane.normal * p_plane.d;
-	Vector3 point_dir = point + p_plane.normal;
-	point = xform_inv(point);
-	point_dir = xform_inv(point_dir);
-
-	Vector3 normal = point_dir - point;
-	normal.normalize();
-	real_t d = normal.dot(point);
-
-	return Plane(normal, d);
+_FORCE_INLINE_ Plane Transform3D::xform_inv(const Plane &p_plane) const {
+	Transform3D inv = affine_inverse();
+	Basis basis_transpose = basis.transposed();
+	return xform_inv_fast(p_plane, inv, basis_transpose);
 }
 
-inline AABB Transform3D::xform(const AABB &p_aabb) const {
-	/* http://dev.theomader.com/transform-bounding-boxes/ */
+_FORCE_INLINE_ AABB Transform3D::xform(const AABB &p_aabb) const {
+	/* https://dev.theomader.com/transform-bounding-boxes/ */
 	Vector3 min = p_aabb.position;
 	Vector3 max = p_aabb.position + p_aabb.size;
 	Vector3 tmin, tmax;
@@ -190,7 +192,7 @@ inline AABB Transform3D::xform(const AABB &p_aabb) const {
 	return r_aabb;
 }
 
-inline AABB Transform3D::xform_inv(const AABB &p_aabb) const {
+_FORCE_INLINE_ AABB Transform3D::xform_inv(const AABB &p_aabb) const {
 	/* define vertices */
 	Vector3 vertices[8] = {
 		Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z),
@@ -218,8 +220,11 @@ PackedVector3Array Transform3D::xform(const PackedVector3Array &p_array) const {
 	PackedVector3Array array;
 	array.resize(p_array.size());
 
+	const Vector3 *r = p_array.ptr();
+	Vector3 *w = array.ptrw();
+
 	for (int i = 0; i < p_array.size(); ++i) {
-		array[i] = xform(p_array[i]);
+		w[i] = xform(r[i]);
 	}
 	return array;
 }
@@ -228,12 +233,48 @@ PackedVector3Array Transform3D::xform_inv(const PackedVector3Array &p_array) con
 	PackedVector3Array array;
 	array.resize(p_array.size());
 
+	const Vector3 *r = p_array.ptr();
+	Vector3 *w = array.ptrw();
+
 	for (int i = 0; i < p_array.size(); ++i) {
-		array[i] = xform_inv(p_array[i]);
+		w[i] = xform_inv(r[i]);
 	}
 	return array;
 }
 
+_FORCE_INLINE_ Plane Transform3D::xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const {
+	// Transform a single point on the plane.
+	Vector3 point = p_plane.normal * p_plane.d;
+	point = xform(point);
+
+	// Use inverse transpose for correct normals with non-uniform scaling.
+	Vector3 normal = p_basis_inverse_transpose.xform(p_plane.normal);
+	normal.normalize();
+
+	real_t d = normal.dot(point);
+	return Plane(normal, d);
+}
+
+_FORCE_INLINE_ Plane Transform3D::xform_inv_fast(const Plane &p_plane, const Transform3D &p_inverse, const Basis &p_basis_transpose) {
+	// Transform a single point on the plane.
+	Vector3 point = p_plane.normal * p_plane.d;
+	point = p_inverse.xform(point);
+
+	// Note that instead of precalculating the transpose, an alternative
+	// would be to use the transpose for the basis transform.
+	// However that would be less SIMD friendly (requiring a swizzle).
+	// So the cost is one extra precalced value in the calling code.
+	// This is probably worth it, as this could be used in bottleneck areas. And
+	// where it is not a bottleneck, the non-fast method is fine.
+
+	// Use transpose for correct normals with non-uniform scaling.
+	Vector3 normal = p_basis_transpose.xform(p_plane.normal);
+	normal.normalize();
+
+	real_t d = normal.dot(point);
+	return Plane(normal, d);
+}
+
 } // namespace godot
 
-#endif // GODOT_TRANSFORM_HPP
+#endif // GODOT_TRANSFORM3D_HPP

src/variant/quaternion.cpp

@@ -35,13 +35,18 @@
 
 namespace godot {
 
+real_t Quaternion::angle_to(const Quaternion &p_to) const {
+	real_t d = dot(p_to);
+	return Math::acos(CLAMP(d * d * 2 - 1, -1, 1));
+}
+
 // get_euler_xyz returns a vector containing the Euler angles in the format
 // (ax,ay,az), where ax is the angle of rotation around x axis,
 // and similar for other axes.
 // This implementation uses XYZ convention (Z is the first rotation).
 Vector3 Quaternion::get_euler_xyz() const {
 	Basis m(*this);
-	return m.get_euler_xyz();
+	return m.get_euler(Basis::EULER_ORDER_XYZ);
 }
 
 // get_euler_yxz returns a vector containing the Euler angles in the format
@@ -50,17 +55,20 @@ Vector3 Quaternion::get_euler_xyz() const {
 // This implementation uses YXZ convention (Z is the first rotation).
 Vector3 Quaternion::get_euler_yxz() const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(!is_normalized(), Vector3(0, 0, 0));
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized.");
 #endif
 	Basis m(*this);
-	return m.get_euler_yxz();
+	return m.get_euler(Basis::EULER_ORDER_YXZ);
 }
 
 void Quaternion::operator*=(const Quaternion &p_q) {
-	x = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y;
-	y = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z;
-	z = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x;
+	real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y;
+	real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z;
+	real_t zz = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x;
 	w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z;
+	x = xx;
+	y = yy;
+	z = zz;
 }
 
 Quaternion Quaternion::operator*(const Quaternion &p_q) const {
@@ -69,8 +77,8 @@ Quaternion Quaternion::operator*(const Quaternion &p_q) const {
 	return r;
 }
 
-bool Quaternion::is_equal_approx(const Quaternion &p_quat) const {
-	return Math::is_equal_approx(x, p_quat.x) && Math::is_equal_approx(y, p_quat.y) && Math::is_equal_approx(z, p_quat.z) && Math::is_equal_approx(w, p_quat.w);
+bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const {
+	return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w);
 }
 
 real_t Quaternion::length() const {
@@ -91,15 +99,32 @@ bool Quaternion::is_normalized() const {
 
 Quaternion Quaternion::inverse() const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(!is_normalized(), Quaternion());
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion must be normalized.");
 #endif
 	return Quaternion(-x, -y, -z, w);
 }
 
+Quaternion Quaternion::log() const {
+	Quaternion src = *this;
+	Vector3 src_v = src.get_axis() * src.get_angle();
+	return Quaternion(src_v.x, src_v.y, src_v.z, 0);
+}
+
+Quaternion Quaternion::exp() const {
+	Quaternion src = *this;
+	Vector3 src_v = Vector3(src.x, src.y, src.z);
+	real_t theta = src_v.length();
+	src_v = src_v.normalized();
+	if (theta < CMP_EPSILON || !src_v.is_normalized()) {
+		return Quaternion(0, 0, 0, 1);
+	}
+	return Quaternion(src_v, theta);
+}
+
 Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(!is_normalized(), Quaternion());
-	ERR_FAIL_COND_V(!p_to.is_normalized(), Quaternion());
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
 #endif
 	Quaternion to1;
 	real_t omega, cosom, sinom, scale0, scale1;
@@ -108,22 +133,16 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
 	cosom = dot(p_to);
 
 	// adjust signs (if necessary)
-	if (cosom < 0.0) {
+	if (cosom < 0.0f) {
 		cosom = -cosom;
-		to1.x = -p_to.x;
-		to1.y = -p_to.y;
-		to1.z = -p_to.z;
-		to1.w = -p_to.w;
+		to1 = -p_to;
 	} else {
-		to1.x = p_to.x;
-		to1.y = p_to.y;
-		to1.z = p_to.z;
-		to1.w = p_to.w;
+		to1 = p_to;
 	}
 
 	// calculate coefficients
 
-	if ((1.0 - cosom) > CMP_EPSILON) {
+	if ((1.0f - cosom) > (real_t)CMP_EPSILON) {
 		// standard case (slerp)
 		omega = Math::acos(cosom);
 		sinom = Math::sin(omega);
@@ -132,7 +151,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
 	} else {
 		// "from" and "to" quaternions are very close
 		//  ... so we can do a linear interpolation
-		scale0 = 1.0 - p_weight;
+		scale0 = 1.0f - p_weight;
 		scale1 = p_weight;
 	}
 	// calculate final values
@@ -145,21 +164,21 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
 
 Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(!is_normalized(), Quaternion());
-	ERR_FAIL_COND_V(!p_to.is_normalized(), Quaternion());
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
 #endif
 	const Quaternion &from = *this;
 
 	real_t dot = from.dot(p_to);
 
-	if (Math::abs(dot) > 0.9999) {
+	if (Math::absf(dot) > 0.9999f) {
 		return from;
 	}
 
 	real_t theta = Math::acos(dot),
-		   sinT = 1.0 / Math::sin(theta),
+		   sinT = 1.0f / Math::sin(theta),
 		   newFactor = Math::sin(p_weight * theta) * sinT,
-		   invFactor = Math::sin((1.0 - p_weight) * theta) * sinT;
+		   invFactor = Math::sin((1.0f - p_weight) * theta) * sinT;
 
 	return Quaternion(invFactor * from.x + newFactor * p_to.x,
 			invFactor * from.y + newFactor * p_to.y,
@@ -167,25 +186,126 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c
 			invFactor * from.w + newFactor * p_to.w);
 }
 
-Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const {
+Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
+#endif
+	Quaternion from_q = *this;
+	Quaternion pre_q = p_pre_a;
+	Quaternion to_q = p_b;
+	Quaternion post_q = p_post_b;
+
+	// Align flip phases.
+	from_q = Basis(from_q).get_rotation_quaternion();
+	pre_q = Basis(pre_q).get_rotation_quaternion();
+	to_q = Basis(to_q).get_rotation_quaternion();
+	post_q = Basis(post_q).get_rotation_quaternion();
+
+	// Flip quaternions to shortest path if necessary.
+	bool flip1 = Math::sign(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = Math::sign(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : Math::sign(to_q.dot(post_q));
+	post_q = flip3 ? -post_q : post_q;
+
+	// Calc by Expmap in from_q space.
+	Quaternion ln_from = Quaternion(0, 0, 0, 0);
+	Quaternion ln_to = (from_q.inverse() * to_q).log();
+	Quaternion ln_pre = (from_q.inverse() * pre_q).log();
+	Quaternion ln_post = (from_q.inverse() * post_q).log();
+	Quaternion ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+	ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+	ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+	Quaternion q1 = from_q * ln.exp();
+
+	// Calc by Expmap in to_q space.
+	ln_from = (to_q.inverse() * from_q).log();
+	ln_to = Quaternion(0, 0, 0, 0);
+	ln_pre = (to_q.inverse() * pre_q).log();
+	ln_post = (to_q.inverse() * post_q).log();
+	ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+	ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+	ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+	Quaternion q2 = to_q * ln.exp();
+
+	// To cancel error made by Expmap ambiguity, do blends.
+	return q1.slerp(q2, p_weight);
+}
+
+Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight,
+		const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND_V(!is_normalized(), Quaternion());
-	ERR_FAIL_COND_V(!p_b.is_normalized(), Quaternion());
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
 #endif
-	//the only way to do slerp :|
-	real_t t2 = (1.0 - p_weight) * p_weight * 2;
-	Quaternion sp = this->slerp(p_b, p_weight);
-	Quaternion sq = p_pre_a.slerpni(p_post_b, p_weight);
-	return sp.slerpni(sq, t2);
+	Quaternion from_q = *this;
+	Quaternion pre_q = p_pre_a;
+	Quaternion to_q = p_b;
+	Quaternion post_q = p_post_b;
+
+	// Align flip phases.
+	from_q = Basis(from_q).get_rotation_quaternion();
+	pre_q = Basis(pre_q).get_rotation_quaternion();
+	to_q = Basis(to_q).get_rotation_quaternion();
+	post_q = Basis(post_q).get_rotation_quaternion();
+
+	// Flip quaternions to shortest path if necessary.
+	bool flip1 = Math::sign(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = Math::sign(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : Math::sign(to_q.dot(post_q));
+	post_q = flip3 ? -post_q : post_q;
+
+	// Calc by Expmap in from_q space.
+	Quaternion ln_from = Quaternion(0, 0, 0, 0);
+	Quaternion ln_to = (from_q.inverse() * to_q).log();
+	Quaternion ln_pre = (from_q.inverse() * pre_q).log();
+	Quaternion ln_post = (from_q.inverse() * post_q).log();
+	Quaternion ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	Quaternion q1 = from_q * ln.exp();
+
+	// Calc by Expmap in to_q space.
+	ln_from = (to_q.inverse() * from_q).log();
+	ln_to = Quaternion(0, 0, 0, 0);
+	ln_pre = (to_q.inverse() * pre_q).log();
+	ln_post = (to_q.inverse() * post_q).log();
+	ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	Quaternion q2 = to_q * ln.exp();
+
+	// To cancel error made by Expmap ambiguity, do blends.
+	return q1.slerp(q2, p_weight);
 }
 
 Quaternion::operator String() const {
-	return String::num(x, 5) + ", " + String::num(y, 5) + ", " + String::num(z, 5) + ", " + String::num(w, 5);
+	return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")";
+}
+
+Vector3 Quaternion::get_axis() const {
+	if (Math::abs(w) > 1 - CMP_EPSILON) {
+		return Vector3(x, y, z);
+	}
+	real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
+	return Vector3(x * r, y * r, z * r);
+}
+
+real_t Quaternion::get_angle() const {
+	return 2 * Math::acos(w);
 }
 
 Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND(!p_axis.is_normalized());
+	ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 must be normalized.");
 #endif
 	real_t d = p_axis.length();
 	if (d == 0) {
@@ -194,8 +314,8 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
 		z = 0;
 		w = 0;
 	} else {
-		real_t sin_angle = Math::sin(p_angle * 0.5);
-		real_t cos_angle = Math::cos(p_angle * 0.5);
+		real_t sin_angle = Math::sin(p_angle * 0.5f);
+		real_t cos_angle = Math::cos(p_angle * 0.5f);
 		real_t s = sin_angle / d;
 		x = p_axis.x * s;
 		y = p_axis.y * s;
@@ -209,9 +329,9 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
 // and similar for other axes.
 // This implementation uses YXZ convention (Z is the first rotation).
 Quaternion::Quaternion(const Vector3 &p_euler) {
-	real_t half_a1 = p_euler.y * 0.5;
-	real_t half_a2 = p_euler.x * 0.5;
-	real_t half_a3 = p_euler.z * 0.5;
+	real_t half_a1 = p_euler.y * 0.5f;
+	real_t half_a2 = p_euler.x * 0.5f;
+	real_t half_a3 = p_euler.z * 0.5f;
 
 	// R = Y(a1).X(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-6)

src/variant/transform3d.cpp

@@ -58,13 +58,13 @@ Transform3D Transform3D::inverse() const {
 	return ret;
 }
 
-void Transform3D::rotate(const Vector3 &p_axis, real_t p_phi) {
-	*this = rotated(p_axis, p_phi);
+void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) {
+	*this = rotated(p_axis, p_angle);
 }
 
-Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_phi) const {
+Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const {
 	// Equivalent to left multiplication
-	Basis p_basis(p_axis, p_phi);
+	Basis p_basis(p_axis, p_angle);
 	return Transform3D(p_basis * basis, p_basis.xform(origin));
 }
 
@@ -74,51 +74,29 @@ Transform3D Transform3D::rotated_local(const Vector3 &p_axis, real_t p_angle) co
 	return Transform3D(basis * p_basis, origin);
 }
 
-void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_phi) {
-	basis.rotate(p_axis, p_phi);
+void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) {
+	basis.rotate(p_axis, p_angle);
 }
 
 Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(origin.is_equal_approx(p_target), Transform3D(), "The transform's origin and target can't be equal.");
+#endif
 	Transform3D t = *this;
-	t.set_look_at(origin, p_target, p_up);
+	t.basis = Basis::looking_at(p_target - origin, p_up);
 	return t;
 }
 
 void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) {
 #ifdef MATH_CHECKS
-	ERR_FAIL_COND(p_eye == p_target);
-	ERR_FAIL_COND(p_up.length() == 0);
-#endif
-	// RefCounted: MESA source code
-	Vector3 v_x, v_y, v_z;
-
-	/* Make rotation matrix */
-
-	/* Z vector */
-	v_z = p_eye - p_target;
-
-	v_z.normalize();
-
-	v_y = p_up;
-
-	v_x = v_y.cross(v_z);
-#ifdef MATH_CHECKS
-	ERR_FAIL_COND(v_x.length() == 0);
+	ERR_FAIL_COND_MSG(p_eye.is_equal_approx(p_target), "The eye and target vectors can't be equal.");
 #endif
-
-	/* Recompute Y = Z cross X */
-	v_y = v_z.cross(v_x);
-
-	v_x.normalize();
-	v_y.normalize();
-
-	basis.set(v_x, v_y, v_z);
-
+	basis = Basis::looking_at(p_target - p_eye, p_up);
 	origin = p_eye;
 }
 
 Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const {
-	/* not sure if very "efficient" but good enough? */
+	Transform3D interp;
 
 	Vector3 src_scale = basis.get_scale();
 	Quaternion src_rot = basis.get_rotation_quaternion();
@@ -128,7 +106,6 @@ Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t
 	Quaternion dst_rot = p_transform.basis.get_rotation_quaternion();
 	Vector3 dst_loc = p_transform.origin;
 
-	Transform3D interp;
 	interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c));
 	interp.origin = src_loc.lerp(dst_loc, p_c);
 
@@ -154,11 +131,11 @@ void Transform3D::scale_basis(const Vector3 &p_scale) {
 	basis.scale(p_scale);
 }
 
-void Transform3D::translate(real_t p_tx, real_t p_ty, real_t p_tz) {
-	translate(Vector3(p_tx, p_ty, p_tz));
+void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t p_tz) {
+	translate_local(Vector3(p_tx, p_ty, p_tz));
 }
 
-void Transform3D::translate(const Vector3 &p_translation) {
+void Transform3D::translate_local(const Vector3 &p_translation) {
 	for (int i = 0; i < 3; i++) {
 		origin[i] += basis[i].dot(p_translation);
 	}
@@ -184,6 +161,16 @@ Transform3D Transform3D::orthonormalized() const {
 	return _copy;
 }
 
+void Transform3D::orthogonalize() {
+	basis.orthogonalize();
+}
+
+Transform3D Transform3D::orthogonalized() const {
+	Transform3D _copy = *this;
+	_copy.orthogonalize();
+	return _copy;
+}
+
 bool Transform3D::is_equal_approx(const Transform3D &p_transform) const {
 	return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin);
 }
@@ -207,8 +194,22 @@ Transform3D Transform3D::operator*(const Transform3D &p_transform) const {
 	return t;
 }
 
+void Transform3D::operator*=(const real_t p_val) {
+	origin *= p_val;
+	basis *= p_val;
+}
+
+Transform3D Transform3D::operator*(const real_t p_val) const {
+	Transform3D ret(*this);
+	ret *= p_val;
+	return ret;
+}
+
 Transform3D::operator String() const {
-	return basis.operator String() + " - " + origin.operator String();
+	return "[X: " + basis.get_column(0).operator String() +
+			", Y: " + basis.get_column(1).operator String() +
+			", Z: " + basis.get_column(2).operator String() +
+			", O: " + origin.operator String() + "]";
 }
 
 Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) :
@@ -218,9 +219,9 @@ Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) :
 
 Transform3D::Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin) :
 		origin(p_origin) {
-	basis.set_axis(0, p_x);
-	basis.set_axis(1, p_y);
-	basis.set_axis(2, p_z);
+	basis.set_column(0, p_x);
+	basis.set_column(1, p_y);
+	basis.set_column(2, p_z);
 }
 
 Transform3D::Transform3D(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz) {