Merge pull request #53684 from TokageItLab/orthogonal-mode

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