Merge pull request #63956 from xiongyaohua/interpolate_on_curve2d

Move rotation interpolation logic from PathFollower2D to Curve2D

3 files changed, 48 insertions, 39 deletions

diff --git a/scene/2d/path_2d.cpp b/scene/2d/path_2d.cpp
index c1044fdf5b..b5945a4562 100644
--- a/scene/2d/path_2d.cpp
+++ b/scene/2d/path_2d.cpp
@@ -175,51 +175,18 @@ void PathFollow2D::_update_transform() {
 	if (path_length == 0) {
 		return;
 	}
-	Vector2 pos = c->sample_baked(progress, cubic);
 
 	if (rotates) {
-		real_t ahead = progress + lookahead;
-
-		if (loop && ahead >= path_length) {
-			// If our lookahead will loop, we need to check if the path is closed.
-			int point_count = c->get_point_count();
-			if (point_count > 0) {
-				Vector2 start_point = c->get_point_position(0);
-				Vector2 end_point = c->get_point_position(point_count - 1);
-				if (start_point == end_point) {
-					// Since the path is closed we want to 'smooth off'
-					// the corner at the start/end.
-					// So we wrap the lookahead back round.
-					ahead = Math::fmod(ahead, path_length);
-				}
-			}
-		}
-
-		Vector2 ahead_pos = c->sample_baked(ahead, cubic);
-
-		Vector2 tangent_to_curve;
-		if (ahead_pos == pos) {
-			// This will happen at the end of non-looping or non-closed paths.
-			// We'll try a look behind instead, in order to get a meaningful angle.
-			tangent_to_curve =
-					(pos - c->sample_baked(progress - lookahead, cubic)).normalized();
-		} else {
-			tangent_to_curve = (ahead_pos - pos).normalized();
-		}
-
-		Vector2 normal_of_curve = -tangent_to_curve.orthogonal();
-
-		pos += tangent_to_curve * h_offset;
-		pos += normal_of_curve * v_offset;
-
-		set_rotation(tangent_to_curve.angle());
-
+		Transform2D xform = c->sample_baked_with_rotation(progress, cubic, loop, lookahead);
+		xform.translate_local(v_offset, h_offset);
+		set_rotation(xform[1].angle());
+		set_position(xform[2]);
 	} else {
+		Vector2 pos = c->sample_baked(progress, cubic);
 		pos.x += h_offset;
 		pos.y += v_offset;
+		set_position(pos);
 	}
-
-	set_position(pos);
 }
 
 void PathFollow2D::_notification(int p_what) {
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index 49b78a091d..eda9af9dde 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -936,6 +936,46 @@ Vector2 Curve2D::sample_baked(real_t p_offset, bool p_cubic) const {
 	}
 }
 
+Transform2D Curve2D::sample_baked_with_rotation(real_t p_offset, bool p_cubic, bool p_loop, real_t p_lookahead) const {
+	real_t path_length = get_baked_length(); // Ensure baked.
+	ERR_FAIL_COND_V_MSG(path_length == 0, Transform2D(), "Length of Curve2D is 0.");
+
+	Vector2 pos = sample_baked(p_offset, p_cubic);
+
+	real_t ahead = p_offset + p_lookahead;
+
+	if (p_loop && ahead >= path_length) {
+		// If our lookahead will loop, we need to check if the path is closed.
+		int point_count = get_point_count();
+		if (point_count > 0) {
+			Vector2 start_point = get_point_position(0);
+			Vector2 end_point = get_point_position(point_count - 1);
+			if (start_point == end_point) {
+				// Since the path is closed we want to 'smooth off'
+				// the corner at the start/end.
+				// So we wrap the lookahead back round.
+				ahead = Math::fmod(ahead, path_length);
+			}
+		}
+	}
+
+	Vector2 ahead_pos = sample_baked(ahead, p_cubic);
+
+	Vector2 tangent_to_curve;
+	if (ahead_pos == pos) {
+		// This will happen at the end of non-looping or non-closed paths.
+		// We'll try a look behind instead, in order to get a meaningful angle.
+		tangent_to_curve =
+				(pos - sample_baked(p_offset - p_lookahead, p_cubic)).normalized();
+	} else {
+		tangent_to_curve = (ahead_pos - pos).normalized();
+	}
+
+	Vector2 normal_of_curve = -tangent_to_curve.orthogonal();
+
+	return Transform2D(normal_of_curve, tangent_to_curve, pos);
+}
+
 PackedVector2Array Curve2D::get_baked_points() const {
 	if (baked_cache_dirty) {
 		_bake();
@@ -1184,6 +1224,7 @@ void Curve2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("get_baked_length"), &Curve2D::get_baked_length);
 	ClassDB::bind_method(D_METHOD("sample_baked", "offset", "cubic"), &Curve2D::sample_baked, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("sample_baked_with_rotation", "offset", "cubic", "loop", "lookahead"), &Curve2D::sample_baked_with_rotation, DEFVAL(false), DEFVAL(true), DEFVAL(4.0));
 	ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve2D::get_baked_points);
 	ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Curve2D::get_closest_point);
 	ClassDB::bind_method(D_METHOD("get_closest_offset", "to_point"), &Curve2D::get_closest_offset);
diff --git a/scene/resources/curve.h b/scene/resources/curve.h
index 88b6dda096..fa1d35aab1 100644
--- a/scene/resources/curve.h
+++ b/scene/resources/curve.h
@@ -216,6 +216,7 @@ public:
 
 	real_t get_baked_length() const;
 	Vector2 sample_baked(real_t p_offset, bool p_cubic = false) const;
+	Transform2D sample_baked_with_rotation(real_t p_offset, bool p_cubic = false, bool p_loop = true, real_t p_lookahead = 4.0) const;
 	PackedVector2Array get_baked_points() const; //useful for going through
 	Vector2 get_closest_point(const Vector2 &p_to_point) const;
 	real_t get_closest_offset(const Vector2 &p_to_point) const;