-Changed KinematicBody API yet again to make it friendlier

-Fixed get_scale functions (and added set_scale) to make it more coherent when decomposing and composing (fixes bugs in transform interpolation)

3 files changed, 23 insertions, 6 deletions

diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index 9732a1ff37..4051de7afb 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -234,7 +234,22 @@ Basis Basis::scaled(const Vector3 &p_scale) const {
 	return m;
 }
 
+void Basis::set_scale(const Vector3 &p_scale) {
+
+	set_axis(0, get_axis(0).normalized() * p_scale.x);
+	set_axis(1, get_axis(1).normalized() * p_scale.y);
+	set_axis(2, get_axis(2).normalized() * p_scale.z);
+}
+
 Vector3 Basis::get_scale() const {
+
+	return Vector3(
+			Vector3(elements[0][0], elements[1][0], elements[2][0]).length(),
+			Vector3(elements[0][1], elements[1][1], elements[2][1]).length(),
+			Vector3(elements[0][2], elements[1][2], elements[2][2]).length());
+}
+
+Vector3 Basis::get_signed_scale() const {
 	// FIXME: We are assuming M = R.S (R is rotation and S is scaling), and use polar decomposition to extract R and S.
 	// A polar decomposition is M = O.P, where O is an orthogonal matrix (meaning rotation and reflection) and
 	// P is a positive semi-definite matrix (meaning it contains absolute values of scaling along its diagonal).
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 9c9080ac46..23429888e0 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -97,7 +97,9 @@ public:
 	void scale(const Vector3 &p_scale);
 	Basis scaled(const Vector3 &p_scale) const;
 
+	void set_scale(const Vector3 &p_scale);
 	Vector3 get_scale() const;
+	Vector3 get_signed_scale() const;
 
 	// transposed dot products
 	_FORCE_INLINE_ real_t tdotx(const Vector3 &v) const {
diff --git a/core/math/transform.cpp b/core/math/transform.cpp
index 60df69a509..638a39ab73 100644
--- a/core/math/transform.cpp
+++ b/core/math/transform.cpp
@@ -118,17 +118,17 @@ Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c)
 
 	/* not sure if very "efficient" but good enough? */
 
-	Vector3 src_scale = basis.get_scale();
-	Quat src_rot = basis;
+	Vector3 src_scale = basis.get_signed_scale();
+	Quat src_rot = basis.orthonormalized();
 	Vector3 src_loc = origin;
 
-	Vector3 dst_scale = p_transform.basis.get_scale();
+	Vector3 dst_scale = p_transform.basis.get_signed_scale();
 	Quat dst_rot = p_transform.basis;
 	Vector3 dst_loc = p_transform.origin;
 
-	Transform dst;
-	dst.basis = src_rot.slerp(dst_rot, p_c);
-	dst.basis.scale(src_scale.linear_interpolate(dst_scale, p_c));
+	Transform dst; //this could be made faster by using a single function in Basis..
+	dst.basis = src_rot.slerp(dst_rot, p_c).normalized();
+	dst.basis.set_scale(src_scale.linear_interpolate(dst_scale, p_c));
 	dst.origin = src_loc.linear_interpolate(dst_loc, p_c);
 
 	return dst;