Merge pull request #859 from aaronfranke/basis-transform-quat

Update Basis/Transform3D/Quaternion to match the engine

4 files changed, 252 insertions, 201 deletions

diff --git a/include/godot_cpp/core/math.hpp b/include/godot_cpp/core/math.hpp
index 6bc2344..78f90ca 100644
--- a/include/godot_cpp/core/math.hpp
+++ b/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;
diff --git a/include/godot_cpp/variant/basis.hpp b/include/godot_cpp/variant/basis.hpp
index 7e11f39..05881f8 100644
--- a/include/godot_cpp/variant/basis.hpp
+++ b/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
diff --git a/include/godot_cpp/variant/quaternion.hpp b/include/godot_cpp/variant/quaternion.hpp
index b17afc5..815b116 100644
--- a/include/godot_cpp/variant/quaternion.hpp
+++ b/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
diff --git a/include/godot_cpp/variant/transform3d.hpp b/include/godot_cpp/variant/transform3d.hpp
index 01d7887..c6220f3 100644
--- a/include/godot_cpp/variant/transform3d.hpp
+++ b/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