Refactor Curv3D and PathFollow3D

This commit makes the following major changes

1. Add "sample_baked_with_rotation()" to Curve3D, making it usable independently. A similar change was made to Curve2D previously.
2. Refactor the _bake() method on Curve3D, using Parallel Transport Frame instead of Frenet Frame.
3. Refactor the sample_* methods, including:
  i. Factor out common binary search code, following the DRY principe
  ii. sample_up_vector() interpolated up vector as part of rotation frame(posture) for consistancy and accuracy.

2 files changed, 261 insertions, 151 deletions

diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index eda9af9dde..0c36abc148 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -31,6 +31,7 @@
 #include "curve.h"
 
 #include "core/core_string_names.h"
+#include "core/math/math_funcs.h"
 
 const char *Curve::SIGNAL_RANGE_CHANGED = "range_changed";
 
@@ -1413,8 +1414,9 @@ void Curve3D::_bake() const {
 	if (points.size() == 0) {
 		baked_point_cache.clear();
 		baked_tilt_cache.clear();
-		baked_up_vector_cache.clear();
 		baked_dist_cache.clear();
+
+		baked_up_vector_cache.clear();
 		return;
 	}
 
@@ -1438,15 +1440,16 @@ void Curve3D::_bake() const {
 
 	Vector3 position = points[0].position;
 	real_t dist = 0.0;
-	List<Plane> pointlist;
+	List<Plane> pointlist; // Abuse Plane for (position, dist)
 	List<real_t> distlist;
 
 	// Start always from origin.
 	pointlist.push_back(Plane(position, points[0].tilt));
 	distlist.push_back(0.0);
 
+	// Step 1: Sample points
+	const real_t step = 0.1; // At least 10 substeps ought to be enough?
 	for (int i = 0; i < points.size() - 1; i++) {
-		real_t step = 0.1; // at least 10 substeps ought to be enough?
 		real_t p = 0.0;
 
 		while (p < 1.0) {
@@ -1461,7 +1464,7 @@ void Curve3D::_bake() const {
 			if (d > bake_interval) {
 				// OK! between P and NP there _has_ to be Something, let's go searching!
 
-				int iterations = 10; //lots of detail!
+				const int iterations = 10; // Lots of detail!
 
 				real_t low = p;
 				real_t hi = np;
@@ -1496,76 +1499,135 @@ void Curve3D::_bake() const {
 		Vector3 npp = points[i + 1].position;
 		real_t d = position.distance_to(npp);
 
-		position = npp;
-		Plane post;
-		post.normal = position;
-		post.d = points[i + 1].tilt;
+		if (d > CMP_EPSILON) { // Avoid the degenerate case of two very close points.
+			position = npp;
+			Plane post;
+			post.normal = position;
+			post.d = points[i + 1].tilt;
 
-		dist += d;
+			dist += d;
 
-		pointlist.push_back(post);
-		distlist.push_back(dist);
+			pointlist.push_back(post);
+			distlist.push_back(dist);
+		}
 	}
 
 	baked_max_ofs = dist;
 
-	baked_point_cache.resize(pointlist.size());
-	Vector3 *w = baked_point_cache.ptrw();
-	int idx = 0;
+	const int point_count = pointlist.size();
+	{
+		baked_point_cache.resize(point_count);
+		Vector3 *w = baked_point_cache.ptrw();
 
-	baked_tilt_cache.resize(pointlist.size());
-	real_t *wt = baked_tilt_cache.ptrw();
+		baked_tilt_cache.resize(point_count);
+		real_t *wt = baked_tilt_cache.ptrw();
 
-	baked_up_vector_cache.resize(up_vector_enabled ? pointlist.size() : 0);
-	Vector3 *up_write = baked_up_vector_cache.ptrw();
+		baked_dist_cache.resize(point_count);
+		real_t *wd = baked_dist_cache.ptrw();
 
-	baked_dist_cache.resize(pointlist.size());
-	real_t *wd = baked_dist_cache.ptrw();
+		int idx = 0;
+		for (const Plane &E : pointlist) {
+			w[idx] = E.normal;
+			wt[idx] = E.d;
+			wd[idx] = distlist[idx];
 
-	Vector3 sideways;
-	Vector3 up;
-	Vector3 forward;
+			idx++;
+		}
+	}
+
+	if (!up_vector_enabled) {
+		baked_up_vector_cache.resize(0);
+		return;
+	}
 
-	Vector3 prev_sideways = Vector3(1, 0, 0);
-	Vector3 prev_up = Vector3(0, 1, 0);
-	Vector3 prev_forward = Vector3(0, 0, 1);
+	// Step 2: Calculate the up vectors and the whole local reference frame
+	//
+	// See Dougan, Carl. "The parallel transport frame." Game Programming Gems 2 (2001): 215-219.
+	// for an example discussing about why not the Frenet frame.
+	{
+		PackedVector3Array forward_vectors;
 
-	for (const Plane &E : pointlist) {
-		w[idx] = E.normal;
-		wt[idx] = E.d;
-		wd[idx] = distlist[idx];
+		baked_up_vector_cache.resize(point_count);
+		forward_vectors.resize(point_count);
 
-		if (!up_vector_enabled) {
-			idx++;
-			continue;
+		Vector3 *up_write = baked_up_vector_cache.ptrw();
+		Vector3 *forward_write = forward_vectors.ptrw();
+
+		const Vector3 *points_ptr = baked_point_cache.ptr();
+
+		Basis frame; // X-right, Y-up, Z-forward.
+		Basis frame_prev;
+
+		// Set the initial frame based on Y-up rule.
+		{
+			Vector3 up(0, 1, 0);
+			Vector3 forward = (points_ptr[1] - points_ptr[0]).normalized();
+			if (forward.is_equal_approx(Vector3())) {
+				forward = Vector3(1, 0, 0);
+			}
+
+			if (abs(forward.dot(up)) > 1.0 - UNIT_EPSILON) {
+				frame_prev = Basis::looking_at(-forward, up);
+			} else {
+				frame_prev = Basis::looking_at(-forward, Vector3(1, 0, 0));
+			}
+
+			up_write[0] = frame_prev.get_column(1);
+			forward_write[0] = frame_prev.get_column(2);
 		}
 
-		forward = idx > 0 ? (w[idx] - w[idx - 1]).normalized() : prev_forward;
+		// Calculate the Parallel Transport Frame.
+		for (int idx = 1; idx < point_count; idx++) {
+			Vector3 forward = (points_ptr[idx] - points_ptr[idx - 1]).normalized();
+			if (forward.is_equal_approx(Vector3())) {
+				forward = frame_prev.get_column(2);
+			}
 
-		real_t y_dot = prev_up.dot(forward);
+			Basis rotate;
+			rotate.rotate_to_align(frame_prev.get_column(2), forward);
+			frame = rotate * frame_prev;
+			frame.orthonormalize(); // guard against float error accumulation
 
-		if (y_dot > (1.0f - CMP_EPSILON)) {
-			sideways = prev_sideways;
-			up = -prev_forward;
-		} else if (y_dot < -(1.0f - CMP_EPSILON)) {
-			sideways = prev_sideways;
-			up = prev_forward;
-		} else {
-			sideways = prev_up.cross(forward).normalized();
-			up = forward.cross(sideways).normalized();
+			up_write[idx] = frame.get_column(1);
+			forward_write[idx] = frame.get_column(2);
+
+			frame_prev = frame;
 		}
 
-		if (idx == 1) {
-			up_write[0] = up;
+		bool is_loop = true;
+		// Loop smoothing only applies when the curve is a loop, which means two ends meet, and share forward directions.
+		{
+			if (!points_ptr[0].is_equal_approx(points_ptr[point_count - 1])) {
+				is_loop = false;
+			}
+
+			real_t dot = forward_write[0].dot(forward_write[point_count - 1]);
+			if (dot < 1.0 - 0.01) { // Alignment should not be too tight, or it dosen't work for coarse bake interval
+				is_loop = false;
+			}
 		}
 
-		up_write[idx] = up;
+		// Twist up vectors, so that they align at two ends of the curve.
+		if (is_loop) {
+			const Vector3 up_start = up_write[0];
+			const Vector3 up_end = up_write[point_count - 1];
+
+			real_t sign = SIGN(up_end.cross(up_start).dot(forward_write[0]));
+			real_t full_angle = Quaternion(up_end, up_start).get_angle();
 
-		prev_sideways = sideways;
-		prev_up = up;
-		prev_forward = forward;
+			if (abs(full_angle) < UNIT_EPSILON) {
+				return;
+			} else {
+				const real_t *dists = baked_dist_cache.ptr();
+				for (int idx = 1; idx < point_count; idx++) {
+					const real_t frac = dists[idx] / baked_max_ofs;
+					const real_t angle = Math::lerp((real_t)0.0, full_angle, frac);
+					Basis twist(forward_write[idx] * sign, angle);
 
-		idx++;
+					up_write[idx] = twist.xform(up_write[idx]);
+				}
+			}
+		}
 	}
 }
 
@@ -1577,27 +1639,15 @@ real_t Curve3D::get_baked_length() const {
 	return baked_max_ofs;
 }
 
-Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
-	if (baked_cache_dirty) {
-		_bake();
-	}
+Curve3D::Interval Curve3D::_find_interval(real_t p_offset) const {
+	Interval interval = {
+		-1,
+		0.0
+	};
+	ERR_FAIL_COND_V_MSG(baked_cache_dirty, interval, "Backed cache is dirty");
 
-	// Validate: Curve may not have baked points.
 	int pc = baked_point_cache.size();
-	ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D.");
-
-	if (pc == 1) {
-		return baked_point_cache.get(0);
-	}
-
-	const Vector3 *r = baked_point_cache.ptr();
-
-	if (p_offset < 0) {
-		return r[0];
-	}
-	if (p_offset >= baked_max_ofs) {
-		return r[pc - 1];
-	}
+	ERR_FAIL_COND_V_MSG(pc < 2, interval, "Less than two points in cache");
 
 	int start = 0;
 	int end = pc;
@@ -1617,9 +1667,27 @@ Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
 	real_t offset_end = baked_dist_cache[idx + 1];
 
 	real_t idx_interval = offset_end - offset_begin;
-	ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(), "Couldn't find baked segment.");
+	ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, interval, "Offset out of range.");
 
-	real_t frac = (p_offset - offset_begin) / idx_interval;
+	interval.idx = idx;
+	if (idx_interval < FLT_EPSILON) {
+		interval.frac = 0.5; // For a very short interval, 0.5 is a reasonable choice.
+		ERR_FAIL_V_MSG(interval, "Zero length interval.");
+	}
+
+	interval.frac = (p_offset - offset_begin) / idx_interval;
+	return interval;
+}
+
+Vector3 Curve3D::_sample_baked(Interval p_interval, bool p_cubic) const {
+	// Assuming p_interval is valid.
+	ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Vector3(), "Invalid interval");
+
+	int idx = p_interval.idx;
+	real_t frac = p_interval.frac;
+
+	const Vector3 *r = baked_point_cache.ptr();
+	int pc = baked_point_cache.size();
 
 	if (p_cubic) {
 		Vector3 pre = idx > 0 ? r[idx - 1] : r[idx];
@@ -1630,114 +1698,150 @@ Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
 	}
 }
 
-real_t Curve3D::sample_baked_tilt(real_t p_offset) const {
-	if (baked_cache_dirty) {
-		_bake();
-	}
-
-	// Validate: Curve may not have baked tilts.
-	int pc = baked_tilt_cache.size();
-	ERR_FAIL_COND_V_MSG(pc == 0, 0, "No tilts in Curve3D.");
+real_t Curve3D::_sample_baked_tilt(Interval p_interval) const {
+	// Assuming that p_interval is valid.
+	ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_tilt_cache.size(), 0.0, "Invalid interval");
 
-	if (pc == 1) {
-		return baked_tilt_cache.get(0);
-	}
+	int idx = p_interval.idx;
+	real_t frac = p_interval.frac;
 
 	const real_t *r = baked_tilt_cache.ptr();
 
-	if (p_offset < 0) {
-		return r[0];
+	return Math::lerp(r[idx], r[idx + 1], frac);
+}
+
+Basis Curve3D::_sample_posture(Interval p_interval, bool p_apply_tilt) const {
+	// Assuming that p_interval is valid.
+	ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Basis(), "Invalid interval");
+	if (up_vector_enabled) {
+		ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_up_vector_cache.size(), Basis(), "Invalid interval");
 	}
-	if (p_offset >= baked_max_ofs) {
-		return r[pc - 1];
+
+	int idx = p_interval.idx;
+	real_t frac = p_interval.frac;
+
+	Vector3 forward_begin;
+	Vector3 forward_end;
+	if (idx == 0) {
+		forward_begin = (baked_point_cache[1] - baked_point_cache[0]).normalized();
+		forward_end = (baked_point_cache[1] - baked_point_cache[0]).normalized();
+	} else {
+		forward_begin = (baked_point_cache[idx] - baked_point_cache[idx - 1]).normalized();
+		forward_end = (baked_point_cache[idx + 1] - baked_point_cache[idx]).normalized();
 	}
 
-	int start = 0;
-	int end = pc;
-	int idx = (end + start) / 2;
-	// Binary search to find baked points.
-	while (start < idx) {
-		real_t offset = baked_dist_cache[idx];
-		if (p_offset <= offset) {
-			end = idx;
-		} else {
-			start = idx;
-		}
-		idx = (end + start) / 2;
+	Vector3 up_begin;
+	Vector3 up_end;
+	if (up_vector_enabled) {
+		const Vector3 *up_ptr = baked_up_vector_cache.ptr();
+		up_begin = up_ptr[idx];
+		up_end = up_ptr[idx + 1];
+	} else {
+		up_begin = Vector3(0.0, 1.0, 0.0);
+		up_end = Vector3(0.0, 1.0, 0.0);
 	}
 
-	real_t offset_begin = baked_dist_cache[idx];
-	real_t offset_end = baked_dist_cache[idx + 1];
+	// Build frames at both ends of the interval, then interpolate.
+	const Basis frame_begin = Basis::looking_at(-forward_begin, up_begin);
+	const Basis frame_end = Basis::looking_at(-forward_end, up_end);
+	const Basis frame = frame_begin.slerp(frame_end, frac).orthonormalized();
 
-	real_t idx_interval = offset_end - offset_begin;
-	ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, 0, "Couldn't find baked segment.");
+	if (!p_apply_tilt) {
+		return frame;
+	}
 
-	real_t frac = (p_offset - offset_begin) / idx_interval;
+	// Applying tilt.
+	const real_t tilt = _sample_baked_tilt(p_interval);
+	Vector3 forward = frame.get_column(2);
 
-	return Math::lerp(r[idx], r[idx + 1], (real_t)frac);
+	const Basis twist(forward, tilt);
+	return twist * frame;
 }
 
-Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) const {
+Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const {
 	if (baked_cache_dirty) {
 		_bake();
 	}
 
-	// Validate: Curve may not have baked up vectors.
-	int count = baked_up_vector_cache.size();
-	ERR_FAIL_COND_V_MSG(count == 0, Vector3(0, 1, 0), "No up vectors in Curve3D.");
+	// Validate: Curve may not have baked points.
+	int pc = baked_point_cache.size();
+	ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D.");
 
-	if (count == 1) {
-		return baked_up_vector_cache.get(0);
+	if (pc == 1) {
+		return baked_point_cache[0];
 	}
 
-	const Vector3 *r = baked_up_vector_cache.ptr();
-	const Vector3 *rp = baked_point_cache.ptr();
-	const real_t *rt = baked_tilt_cache.ptr();
+	p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic.
 
-	int start = 0;
-	int end = count;
-	int idx = (end + start) / 2;
-	// Binary search to find baked points.
-	while (start < idx) {
-		real_t offset = baked_dist_cache[idx];
-		if (p_offset <= offset) {
-			end = idx;
-		} else {
-			start = idx;
-		}
-		idx = (end + start) / 2;
+	Curve3D::Interval interval = _find_interval(p_offset);
+	return _sample_baked(interval, p_cubic);
+}
+
+Transform3D Curve3D::sample_baked_with_rotation(real_t p_offset, bool p_cubic, bool p_apply_tilt) const {
+	if (baked_cache_dirty) {
+		_bake();
 	}
 
-	if (idx == count - 1) {
-		return p_apply_tilt ? r[idx].rotated((rp[idx] - rp[idx - 1]).normalized(), rt[idx]) : r[idx];
+	// Validate: Curve may not have baked points.
+	const int point_count = baked_point_cache.size();
+	ERR_FAIL_COND_V_MSG(point_count == 0, Transform3D(), "No points in Curve3D.");
+
+	if (point_count == 1) {
+		Transform3D t;
+		t.origin = baked_point_cache.get(0);
+		ERR_FAIL_V_MSG(t, "Only 1 point in Curve3D.");
 	}
 
-	real_t offset_begin = baked_dist_cache[idx];
-	real_t offset_end = baked_dist_cache[idx + 1];
+	p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic.
 
-	real_t idx_interval = offset_end - offset_begin;
-	ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(0, 1, 0), "Couldn't find baked segment.");
+	// 0. Find interval for all sampling steps.
+	Curve3D::Interval interval = _find_interval(p_offset);
 
-	real_t frac = (p_offset - offset_begin) / idx_interval;
+	// 1. Sample position.
+	Vector3 pos = _sample_baked(interval, p_cubic);
+
+	// 2. Sample rotation frame.
+	Basis frame = _sample_posture(interval, p_apply_tilt);
+
+	return Transform3D(frame, pos);
+}
+
+real_t Curve3D::sample_baked_tilt(real_t p_offset) const {
+	if (baked_cache_dirty) {
+		_bake();
+	}
 
-	Vector3 forward = (rp[idx + 1] - rp[idx]).normalized();
-	Vector3 up = r[idx];
-	Vector3 up1 = r[idx + 1];
+	// Validate: Curve may not have baked tilts.
+	int pc = baked_tilt_cache.size();
+	ERR_FAIL_COND_V_MSG(pc == 0, 0, "No tilts in Curve3D.");
 
-	if (p_apply_tilt) {
-		up.rotate(forward, rt[idx]);
-		up1.rotate(idx + 2 >= count ? forward : (rp[idx + 2] - rp[idx + 1]).normalized(), rt[idx + 1]);
+	if (pc == 1) {
+		return baked_tilt_cache.get(0);
 	}
 
-	Vector3 axis = up.cross(up1);
+	p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic
 
-	if (axis.length_squared() < CMP_EPSILON2) {
-		axis = forward;
-	} else {
-		axis.normalize();
+	Curve3D::Interval interval = _find_interval(p_offset);
+	return _sample_baked_tilt(interval);
+}
+
+Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) const {
+	if (baked_cache_dirty) {
+		_bake();
+	}
+
+	// Validate: Curve may not have baked up vectors.
+	ERR_FAIL_COND_V_MSG(!up_vector_enabled, Vector3(0, 1, 0), "No up vectors in Curve3D.");
+
+	int count = baked_up_vector_cache.size();
+	if (count == 1) {
+		return baked_up_vector_cache.get(0);
 	}
 
-	return up.rotated(axis, up.angle_to(up1) * frac);
+	p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic.
+
+	Curve3D::Interval interval = _find_interval(p_offset);
+	return _sample_posture(interval, p_apply_tilt).get_column(1);
 }
 
 PackedVector3Array Curve3D::get_baked_points() const {
@@ -2034,6 +2138,7 @@ void Curve3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("get_baked_length"), &Curve3D::get_baked_length);
 	ClassDB::bind_method(D_METHOD("sample_baked", "offset", "cubic"), &Curve3D::sample_baked, DEFVAL(false));
+	ClassDB::bind_method(D_METHOD("sample_baked_with_rotation", "offset", "cubic", "apply_tilt"), &Curve3D::sample_baked_with_rotation, DEFVAL(false), DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("sample_baked_up_vector", "offset", "apply_tilt"), &Curve3D::sample_baked_up_vector, DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve3D::get_baked_points);
 	ClassDB::bind_method(D_METHOD("get_baked_tilts"), &Curve3D::get_baked_tilts);
diff --git a/scene/resources/curve.h b/scene/resources/curve.h
index fa1d35aab1..d0c813f262 100644
--- a/scene/resources/curve.h
+++ b/scene/resources/curve.h
@@ -238,11 +238,6 @@ class Curve3D : public Resource {
 
 	Vector<Point> points;
 
-	struct BakedPoint {
-		real_t ofs = 0.0;
-		Vector3 point;
-	};
-
 	mutable bool baked_cache_dirty = false;
 	mutable PackedVector3Array baked_point_cache;
 	mutable Vector<real_t> baked_tilt_cache;
@@ -254,6 +249,15 @@ class Curve3D : public Resource {
 
 	void _bake() const;
 
+	struct Interval {
+		int idx;
+		real_t frac;
+	};
+	Interval _find_interval(real_t p_offset) const;
+	Vector3 _sample_baked(Interval p_interval, bool p_cubic) const;
+	real_t _sample_baked_tilt(Interval p_interval) const;
+	Basis _sample_posture(Interval p_interval, bool p_apply_tilt = false) const;
+
 	real_t bake_interval = 0.2;
 	bool up_vector_enabled = true;
 
@@ -296,9 +300,10 @@ public:
 
 	real_t get_baked_length() const;
 	Vector3 sample_baked(real_t p_offset, bool p_cubic = false) const;
+	Transform3D sample_baked_with_rotation(real_t p_offset, bool p_cubic = false, bool p_apply_tilt = false) const;
 	real_t sample_baked_tilt(real_t p_offset) const;
 	Vector3 sample_baked_up_vector(real_t p_offset, bool p_apply_tilt = false) const;
-	PackedVector3Array get_baked_points() const; //useful for going through
+	PackedVector3Array get_baked_points() const; // Useful for going through.
 	Vector<real_t> get_baked_tilts() const; //useful for going through
 	PackedVector3Array get_baked_up_vectors() const;
 	Vector3 get_closest_point(const Vector3 &p_to_point) const;