4 files changed, 71 insertions, 2 deletions

diff --git a/core/math/basis.cpp b/core/math/basis.cpp
index 6159e78bab..a9b4651664 100644
--- a/core/math/basis.cpp
+++ b/core/math/basis.cpp
@@ -94,6 +94,18 @@ Basis Basis::orthonormalized() const {
 	return c;
 }
 
+void Basis::orthogonalize() {
+	Vector3 scl = get_scale();
+	orthonormalize();
+	scale_local(scl);
+}
+
+Basis Basis::orthogonalized() const {
+	Basis c = *this;
+	c.orthogonalize();
+	return c;
+}
+
 bool Basis::is_orthogonal() const {
 	Basis identity;
 	Basis m = (*this) * transposed();
@@ -237,6 +249,24 @@ void Basis::scale_local(const Vector3 &p_scale) {
 	*this = scaled_local(p_scale);
 }
 
+void Basis::scale_orthogonal(const Vector3 &p_scale) {
+	*this = scaled_orthogonal(p_scale);
+}
+
+Basis Basis::scaled_orthogonal(const Vector3 &p_scale) const {
+	Basis m = *this;
+	Vector3 s = Vector3(-1, -1, -1) + p_scale;
+	Vector3 dots;
+	Basis b;
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			dots[j] += s[i] * abs(m.get_axis(i).normalized().dot(b.get_axis(j)));
+		}
+	}
+	m.scale_local(Vector3(1, 1, 1) + dots);
+	return m;
+}
+
 float Basis::get_uniform_scale() const {
 	return (elements[0].length() + elements[1].length() + elements[2].length()) / 3.0;
 }
@@ -931,6 +961,15 @@ void Basis::_set_diagonal(const Vector3 &p_diag) {
 	elements[2][2] = p_diag.z;
 }
 
+Basis Basis::lerp(const Basis &p_to, const real_t &p_weight) const {
+	Basis b;
+	b.elements[0] = elements[0].lerp(p_to.elements[0], p_weight);
+	b.elements[1] = elements[1].lerp(p_to.elements[1], p_weight);
+	b.elements[2] = elements[2].lerp(p_to.elements[2], p_weight);
+
+	return b;
+}
+
 Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const {
 	//consider scale
 	Quaternion from(*this);
diff --git a/core/math/basis.h b/core/math/basis.h
index 48367631d5..709f2cb3cf 100644
--- a/core/math/basis.h
+++ b/core/math/basis.h
@@ -123,6 +123,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;
 
@@ -168,6 +171,7 @@ public:
 	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);
 
@@ -233,6 +237,9 @@ public:
 	void orthonormalize();
 	Basis orthonormalized() const;
 
+	void orthogonalize();
+	Basis orthogonalized() const;
+
 #ifdef MATH_CHECKS
 	bool is_symmetric() const;
 #endif
diff --git a/core/math/transform_3d.cpp b/core/math/transform_3d.cpp
index 1e0ee13504..e5374315e2 100644
--- a/core/math/transform_3d.cpp
+++ b/core/math/transform_3d.cpp
@@ -80,9 +80,11 @@ void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, con
 	origin = p_eye;
 }
 
-Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const {
+Transform3D Transform3D::sphere_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();
 	Vector3 src_loc = origin;
@@ -91,13 +93,21 @@ 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);
 
 	return interp;
 }
 
+Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const {
+	Transform3D interp;
+
+	interp.basis = basis.lerp(p_transform.basis, p_c);
+	interp.origin = origin.lerp(p_transform.origin, p_c);
+
+	return interp;
+}
+
 void Transform3D::scale(const Vector3 &p_scale) {
 	basis.scale(p_scale);
 	origin *= p_scale;
@@ -139,6 +149,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);
 }
diff --git a/core/math/transform_3d.h b/core/math/transform_3d.h
index 4356aa4e79..c0ef2ecfc1 100644
--- a/core/math/transform_3d.h
+++ b/core/math/transform_3d.h
@@ -69,6 +69,8 @@ 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;
@@ -99,6 +101,7 @@ public:
 	void operator*=(const real_t p_val);
 	Transform3D operator*(const real_t p_val) const;
 
+	Transform3D sphere_interpolate_with(const Transform3D &p_transform, real_t p_c) const;
 	Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const;
 
 	_FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const {