Add support for interpolating skewed transforms

2 files changed, 19 insertions, 33 deletions

diff --git a/core/math/transform_2d.cpp b/core/math/transform_2d.cpp
index 910995d717..96010b4096 100644
--- a/core/math/transform_2d.cpp
+++ b/core/math/transform_2d.cpp
@@ -263,39 +263,12 @@ real_t Transform2D::basis_determinant() const {
 	return columns[0].x * columns[1].y - columns[0].y * columns[1].x;
 }
 
-Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const real_t p_c) const {
-	//extract parameters
-	Vector2 p1 = get_origin();
-	Vector2 p2 = p_transform.get_origin();
-
-	real_t r1 = get_rotation();
-	real_t r2 = p_transform.get_rotation();
-
-	Size2 s1 = get_scale();
-	Size2 s2 = p_transform.get_scale();
-
-	//slerp rotation
-	Vector2 v1(Math::cos(r1), Math::sin(r1));
-	Vector2 v2(Math::cos(r2), Math::sin(r2));
-
-	real_t dot = v1.dot(v2);
-
-	dot = CLAMP(dot, (real_t)-1.0, (real_t)1.0);
-
-	Vector2 v;
-
-	if (dot > 0.9995f) {
-		v = v1.lerp(v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues
-	} else {
-		real_t angle = p_c * Math::acos(dot);
-		Vector2 v3 = (v2 - v1 * dot).normalized();
-		v = v1 * Math::cos(angle) + v3 * Math::sin(angle);
-	}
-
-	//construct matrix
-	Transform2D res(v.angle(), p1.lerp(p2, p_c));
-	res.scale_basis(s1.lerp(s2, p_c));
-	return res;
+Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, const real_t p_weight) const {
+	return Transform2D(
+			Math::lerp_angle(get_rotation(), p_transform.get_rotation(), p_weight),
+			get_scale().lerp(p_transform.get_scale(), p_weight),
+			Math::lerp_angle(get_skew(), p_transform.get_skew(), p_weight),
+			get_origin().lerp(p_transform.get_origin(), p_weight));
 }
 
 void Transform2D::operator*=(const real_t p_val) {
diff --git a/tests/core/math/test_transform_2d.h b/tests/core/math/test_transform_2d.h
index dc2b6e2ba8..ca27776180 100644
--- a/tests/core/math/test_transform_2d.h
+++ b/tests/core/math/test_transform_2d.h
@@ -84,6 +84,19 @@ TEST_CASE("[Transform2D] rotation") {
 	CHECK(orig.rotated_local(phi) == orig * R);
 }
 
+TEST_CASE("[Transform2D] Interpolation") {
+	Transform2D rotate_scale_skew_pos = Transform2D(Math::deg_to_rad(170.0), Vector2(3.6, 8.0), Math::deg_to_rad(20.0), Vector2(2.4, 6.8));
+	Transform2D rotate_scale_skew_pos_halfway = Transform2D(Math::deg_to_rad(85.0), Vector2(2.3, 4.5), Math::deg_to_rad(10.0), Vector2(1.2, 3.4));
+	Transform2D interpolated = Transform2D().interpolate_with(rotate_scale_skew_pos, 0.5);
+	CHECK(interpolated.get_origin().is_equal_approx(rotate_scale_skew_pos_halfway.get_origin()));
+	CHECK(interpolated.get_rotation() == doctest::Approx(rotate_scale_skew_pos_halfway.get_rotation()));
+	CHECK(interpolated.get_scale().is_equal_approx(rotate_scale_skew_pos_halfway.get_scale()));
+	CHECK(interpolated.get_skew() == doctest::Approx(rotate_scale_skew_pos_halfway.get_skew()));
+	CHECK(interpolated.is_equal_approx(rotate_scale_skew_pos_halfway));
+	interpolated = rotate_scale_skew_pos.interpolate_with(Transform2D(), 0.5);
+	CHECK(interpolated.is_equal_approx(rotate_scale_skew_pos_halfway));
+}
+
 TEST_CASE("[Transform2D] Finite number checks") {
 	const Vector2 x(0, 1);
 	const Vector2 infinite(NAN, NAN);