Curve2D/Curve3D: exact linear interpolation

While calculating interpolated points, intervals between two baked
points has been assummed to be `baked_interval`. The assumption could
cause significant error in some extreme cases (for example #7088).

To improve accuracy, `baked_dist_cache` is introduced, which stores
distance from starting point for each baked points. `interpolate_baked`
now returns exact linear-interpolated position along baked points.

2 files changed, 73 insertions, 26 deletions

diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index 3b666640f8..a364a27e80 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -662,19 +662,27 @@ void Curve2D::_bake() const {
 
 	if (points.size() == 0) {
 		baked_point_cache.resize(0);
+		baked_dist_cache.resize(0);
 		return;
 	}
 
 	if (points.size() == 1) {
 		baked_point_cache.resize(1);
 		baked_point_cache.set(0, points[0].pos);
+
+		baked_dist_cache.resize(1);
+		baked_dist_cache.set(0, 0.0);
 		return;
 	}
 
 	Vector2 pos = points[0].pos;
+	float dist = 0.0;
+
 	List<Vector2> pointlist;
+	List<float> distlist;
 
 	pointlist.push_back(pos); //start always from origin
+	distlist.push_back(0.0);
 
 	for (int i = 0; i < points.size() - 1; i++) {
 		float step = 0.1; // at least 10 substeps ought to be enough?
@@ -712,7 +720,10 @@ void Curve2D::_bake() const {
 
 				pos = npp;
 				p = mid;
+				dist += d;
+
 				pointlist.push_back(pos);
+				distlist.push_back(dist);
 			} else {
 				p = np;
 			}
@@ -722,16 +733,20 @@ void Curve2D::_bake() const {
 	Vector2 lastpos = points[points.size() - 1].pos;
 
 	float rem = pos.distance_to(lastpos);
-	baked_max_ofs = (pointlist.size() - 1) * bake_interval + rem;
+	dist += rem;
+	baked_max_ofs = dist;
 	pointlist.push_back(lastpos);
+	distlist.push_back(dist);
 
 	baked_point_cache.resize(pointlist.size());
+	baked_dist_cache.resize(distlist.size());
+
 	Vector2 *w = baked_point_cache.ptrw();
-	int idx = 0;
+	float *wd = baked_dist_cache.ptrw();
 
-	for (const Vector2 &E : pointlist) {
-		w[idx] = E;
-		idx++;
+	for (int i = 0; i < pointlist.size(); i++) {
+		w[i] = pointlist[i];
+		wd[i] = distlist[i];
 	}
 }
 
@@ -766,19 +781,26 @@ Vector2 Curve2D::interpolate_baked(float p_offset, bool p_cubic) const {
 		return r[bpc - 1];
 	}
 
-	int idx = Math::floor((double)p_offset / (double)bake_interval);
-	float frac = Math::fmod(p_offset, (float)bake_interval);
-
-	if (idx >= bpc - 1) {
-		return r[bpc - 1];
-	} else if (idx == bpc - 2) {
-		if (frac > 0) {
-			frac /= Math::fmod(baked_max_ofs, bake_interval);
+	int start = 0, end = bpc, idx = (end + start) / 2;
+	// binary search to find baked points
+	while (start < idx) {
+		float offset = baked_dist_cache[idx];
+		if (p_offset <= offset) {
+			end = idx;
+		} else {
+			start = idx;
 		}
-	} else {
-		frac /= bake_interval;
+		idx = (end + start) / 2;
 	}
 
+	float offset_begin = baked_dist_cache[idx];
+	float offset_end = baked_dist_cache[idx + 1];
+
+	float idx_interval = offset_end - offset_begin;
+	ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector2(), "failed to find baked segment");
+
+	float frac = (p_offset - offset_begin) / idx_interval;
+
 	if (p_cubic) {
 		Vector2 pre = idx > 0 ? r[idx - 1] : r[idx];
 		Vector2 post = (idx < (bpc - 2)) ? r[idx + 2] : r[idx + 1];
@@ -1145,6 +1167,7 @@ void Curve3D::_bake() const {
 		baked_point_cache.resize(0);
 		baked_tilt_cache.resize(0);
 		baked_up_vector_cache.resize(0);
+		baked_dist_cache.resize(0);
 		return;
 	}
 
@@ -1153,6 +1176,8 @@ void Curve3D::_bake() const {
 		baked_point_cache.set(0, points[0].pos);
 		baked_tilt_cache.resize(1);
 		baked_tilt_cache.set(0, points[0].tilt);
+		baked_dist_cache.resize(1);
+		baked_dist_cache.set(0, 0.0);
 
 		if (up_vector_enabled) {
 			baked_up_vector_cache.resize(1);
@@ -1165,8 +1190,12 @@ void Curve3D::_bake() const {
 	}
 
 	Vector3 pos = points[0].pos;
+	float dist = 0.0;
 	List<Plane> pointlist;
+	List<float> distlist;
+
 	pointlist.push_back(Plane(pos, points[0].tilt));
+	distlist.push_back(0.0);
 
 	for (int i = 0; i < points.size() - 1; i++) {
 		float step = 0.1; // at least 10 substeps ought to be enough?
@@ -1207,7 +1236,10 @@ void Curve3D::_bake() const {
 				Plane post;
 				post.normal = pos;
 				post.d = Math::lerp(points[i].tilt, points[i + 1].tilt, mid);
+				dist += d;
+
 				pointlist.push_back(post);
+				distlist.push_back(dist);
 			} else {
 				p = np;
 			}
@@ -1218,8 +1250,10 @@ void Curve3D::_bake() const {
 	float lastilt = points[points.size() - 1].tilt;
 
 	float rem = pos.distance_to(lastpos);
-	baked_max_ofs = (pointlist.size() - 1) * bake_interval + rem;
+	dist += rem;
+	baked_max_ofs = dist;
 	pointlist.push_back(Plane(lastpos, lastilt));
+	distlist.push_back(dist);
 
 	baked_point_cache.resize(pointlist.size());
 	Vector3 *w = baked_point_cache.ptrw();
@@ -1231,6 +1265,9 @@ void Curve3D::_bake() const {
 	baked_up_vector_cache.resize(up_vector_enabled ? pointlist.size() : 0);
 	Vector3 *up_write = baked_up_vector_cache.ptrw();
 
+	baked_dist_cache.resize(pointlist.size());
+	float *wd = baked_dist_cache.ptrw();
+
 	Vector3 sideways;
 	Vector3 up;
 	Vector3 forward;
@@ -1242,6 +1279,7 @@ void Curve3D::_bake() const {
 	for (const Plane &E : pointlist) {
 		w[idx] = E.normal;
 		wt[idx] = E.d;
+		wd[idx] = distlist[idx];
 
 		if (!up_vector_enabled) {
 			idx++;
@@ -1308,19 +1346,26 @@ Vector3 Curve3D::interpolate_baked(float p_offset, bool p_cubic) const {
 		return r[bpc - 1];
 	}
 
-	int idx = Math::floor((double)p_offset / (double)bake_interval);
-	float frac = Math::fmod(p_offset, bake_interval);
-
-	if (idx >= bpc - 1) {
-		return r[bpc - 1];
-	} else if (idx == bpc - 2) {
-		if (frac > 0) {
-			frac /= Math::fmod(baked_max_ofs, bake_interval);
+	int start = 0, end = bpc, idx = (end + start) / 2;
+	// binary search to find baked points
+	while (start < idx) {
+		float offset = baked_dist_cache[idx];
+		if (p_offset <= offset) {
+			end = idx;
+		} else {
+			start = idx;
 		}
-	} else {
-		frac /= bake_interval;
+		idx = (end + start) / 2;
 	}
 
+	float offset_begin = baked_dist_cache[idx];
+	float offset_end = baked_dist_cache[idx + 1];
+
+	float idx_interval = offset_end - offset_begin;
+	ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(), "failed to find baked segment");
+
+	float frac = (p_offset - offset_begin) / idx_interval;
+
 	if (p_cubic) {
 		Vector3 pre = idx > 0 ? r[idx - 1] : r[idx];
 		Vector3 post = (idx < (bpc - 2)) ? r[idx + 2] : r[idx + 1];
diff --git a/scene/resources/curve.h b/scene/resources/curve.h
index c25d307608..5808fd6508 100644
--- a/scene/resources/curve.h
+++ b/scene/resources/curve.h
@@ -161,6 +161,7 @@ class Curve2D : public Resource {
 
 	mutable bool baked_cache_dirty = false;
 	mutable PackedVector2Array baked_point_cache;
+	mutable PackedFloat32Array baked_dist_cache;
 	mutable float baked_max_ofs = 0.0;
 
 	void _bake() const;
@@ -224,6 +225,7 @@ class Curve3D : public Resource {
 	mutable PackedVector3Array baked_point_cache;
 	mutable Vector<real_t> baked_tilt_cache;
 	mutable PackedVector3Array baked_up_vector_cache;
+	mutable PackedFloat32Array baked_dist_cache;
 	mutable float baked_max_ofs = 0.0;
 
 	void _bake() const;