Merge pull request #62458 from Geometror/interpolation-function-cleanup

Refactor Bezier interpolation functions

3 files changed, 20 insertions, 47 deletions

diff --git a/scene/resources/animation.cpp b/scene/resources/animation.cpp
index 8ae4872d14..7183accc66 100644
--- a/scene/resources/animation.cpp
+++ b/scene/resources/animation.cpp
@@ -3379,17 +3379,6 @@ Vector2 Animation::bezier_track_get_key_out_handle(int p_track, int p_index) con
 	return bt->values[p_index].value.out_handle;
 }
 
-static _FORCE_INLINE_ Vector2 _bezier_interp(real_t t, const Vector2 &start, const Vector2 &control_1, const Vector2 &control_2, const Vector2 &end) {
-	/* Formula from Wikipedia article on Bezier curves. */
-	real_t omt = (1.0 - t);
-	real_t omt2 = omt * omt;
-	real_t omt3 = omt2 * omt;
-	real_t t2 = t * t;
-	real_t t3 = t2 * t;
-
-	return start * omt3 + control_1 * omt2 * t * 3.0 + control_2 * omt * t2 * 3.0 + end * t3;
-}
-
 real_t Animation::bezier_track_interpolate(int p_track, double p_time) const {
 	//this uses a different interpolation scheme
 	ERR_FAIL_INDEX_V(p_track, tracks.size(), 0);
@@ -3438,7 +3427,7 @@ real_t Animation::bezier_track_interpolate(int p_track, double p_time) const {
 	for (int i = 0; i < iterations; i++) {
 		real_t middle = (low + high) / 2;
 
-		Vector2 interp = _bezier_interp(middle, start, start_out, end_in, end);
+		Vector2 interp = start.bezier_interpolate(start_out, end_in, end, middle);
 
 		if (interp.x < t) {
 			low = middle;
@@ -3448,8 +3437,8 @@ real_t Animation::bezier_track_interpolate(int p_track, double p_time) const {
 	}
 
 	//interpolate the result:
-	Vector2 low_pos = _bezier_interp(low, start, start_out, end_in, end);
-	Vector2 high_pos = _bezier_interp(high, start, start_out, end_in, end);
+	Vector2 low_pos = start.bezier_interpolate(start_out, end_in, end, low);
+	Vector2 high_pos = start.bezier_interpolate(start_out, end_in, end, high);
 	real_t c = (t - low_pos.x) / (high_pos.x - low_pos.x);
 
 	return low_pos.lerp(high_pos, c).y;
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index 96cf7bb708..da26a0261f 100644
--- a/scene/resources/curve.cpp
+++ b/scene/resources/curve.cpp
@@ -32,18 +32,6 @@
 
 #include "core/core_string_names.h"
 
-template <class T>
-static _FORCE_INLINE_ T _bezier_interp(real_t p_t, T p_start, T p_control_1, T p_control_2, T p_end) {
-	/* Formula from Wikipedia article on Bezier curves. */
-	real_t omt = (1.0 - p_t);
-	real_t omt2 = omt * omt;
-	real_t omt3 = omt2 * omt;
-	real_t t2 = p_t * p_t;
-	real_t t3 = t2 * p_t;
-
-	return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3;
-}
-
 const char *Curve::SIGNAL_RANGE_CHANGED = "range_changed";
 
 Curve::Curve() {
@@ -376,7 +364,7 @@ real_t Curve::interpolate_local_nocheck(int p_index, real_t p_local_offset) cons
 	real_t yac = a.position.y + d * a.right_tangent;
 	real_t ybc = b.position.y - d * b.left_tangent;
 
-	real_t y = _bezier_interp(p_local_offset, a.position.y, yac, ybc, b.position.y);
+	real_t y = Math::bezier_interpolate(a.position.y, yac, ybc, b.position.y, p_local_offset);
 
 	return y;
 }
@@ -747,7 +735,7 @@ Vector2 Curve2D::interpolate(int p_index, const real_t p_offset) const {
 	Vector2 p3 = points[p_index + 1].position;
 	Vector2 p2 = p3 + points[p_index + 1].in;
 
-	return _bezier_interp(p_offset, p0, p1, p2, p3);
+	return p0.bezier_interpolate(p1, p2, p3, p_offset);
 }
 
 Vector2 Curve2D::interpolatef(real_t p_findex) const {
@@ -767,9 +755,9 @@ void Curve2D::mark_dirty() {
 
 void Curve2D::_bake_segment2d(RBMap<real_t, Vector2> &r_bake, real_t p_begin, real_t p_end, const Vector2 &p_a, const Vector2 &p_out, const Vector2 &p_b, const Vector2 &p_in, int p_depth, int p_max_depth, real_t p_tol) const {
 	real_t mp = p_begin + (p_end - p_begin) * 0.5;
-	Vector2 beg = _bezier_interp(p_begin, p_a, p_a + p_out, p_b + p_in, p_b);
-	Vector2 mid = _bezier_interp(mp, p_a, p_a + p_out, p_b + p_in, p_b);
-	Vector2 end = _bezier_interp(p_end, p_a, p_a + p_out, p_b + p_in, p_b);
+	Vector2 beg = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_begin);
+	Vector2 mid = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, mp);
+	Vector2 end = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_end);
 
 	Vector2 na = (mid - beg).normalized();
 	Vector2 nb = (end - mid).normalized();
@@ -828,7 +816,7 @@ void Curve2D::_bake() const {
 				np = 1.0;
 			}
 
-			Vector2 npp = _bezier_interp(np, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position);
+			Vector2 npp = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, np);
 			real_t d = position.distance_to(npp);
 
 			if (d > bake_interval) {
@@ -841,7 +829,7 @@ void Curve2D::_bake() const {
 				real_t mid = low + (hi - low) * 0.5;
 
 				for (int j = 0; j < iterations; j++) {
-					npp = _bezier_interp(mid, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position);
+					npp = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, mid);
 					d = position.distance_to(npp);
 
 					if (bake_interval < d) {
@@ -1336,7 +1324,7 @@ Vector3 Curve3D::interpolate(int p_index, real_t p_offset) const {
 	Vector3 p3 = points[p_index + 1].position;
 	Vector3 p2 = p3 + points[p_index + 1].in;
 
-	return _bezier_interp(p_offset, p0, p1, p2, p3);
+	return p0.bezier_interpolate(p1, p2, p3, p_offset);
 }
 
 Vector3 Curve3D::interpolatef(real_t p_findex) const {
@@ -1356,9 +1344,9 @@ void Curve3D::mark_dirty() {
 
 void Curve3D::_bake_segment3d(RBMap<real_t, Vector3> &r_bake, real_t p_begin, real_t p_end, const Vector3 &p_a, const Vector3 &p_out, const Vector3 &p_b, const Vector3 &p_in, int p_depth, int p_max_depth, real_t p_tol) const {
 	real_t mp = p_begin + (p_end - p_begin) * 0.5;
-	Vector3 beg = _bezier_interp(p_begin, p_a, p_a + p_out, p_b + p_in, p_b);
-	Vector3 mid = _bezier_interp(mp, p_a, p_a + p_out, p_b + p_in, p_b);
-	Vector3 end = _bezier_interp(p_end, p_a, p_a + p_out, p_b + p_in, p_b);
+	Vector3 beg = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_begin);
+	Vector3 mid = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, mp);
+	Vector3 end = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_end);
 
 	Vector3 na = (mid - beg).normalized();
 	Vector3 nb = (end - mid).normalized();
@@ -1426,7 +1414,7 @@ void Curve3D::_bake() const {
 				np = 1.0;
 			}
 
-			Vector3 npp = _bezier_interp(np, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position);
+			Vector3 npp = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, np);
 			real_t d = position.distance_to(npp);
 
 			if (d > bake_interval) {
@@ -1439,7 +1427,7 @@ void Curve3D::_bake() const {
 				real_t mid = low + (hi - low) * 0.5;
 
 				for (int j = 0; j < iterations; j++) {
-					npp = _bezier_interp(mid, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position);
+					npp = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, mid);
 					d = position.distance_to(npp);
 
 					if (bake_interval < d) {
diff --git a/scene/resources/gradient.h b/scene/resources/gradient.h
index a3d3449099..2b04ead0af 100644
--- a/scene/resources/gradient.h
+++ b/scene/resources/gradient.h
@@ -92,10 +92,6 @@ public:
 	void set_interpolation_mode(InterpolationMode p_interp_mode);
 	InterpolationMode get_interpolation_mode();
 
-	_FORCE_INLINE_ float cubic_interpolate(float p0, float p1, float p2, float p3, float x) {
-		return p1 + 0.5 * x * (p2 - p0 + x * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3 + x * (3.0 * (p1 - p2) + p3 - p0)));
-	}
-
 	_FORCE_INLINE_ Color get_color_at_offset(float p_offset) {
 		if (points.is_empty()) {
 			return Color(0, 0, 0, 1);
@@ -161,10 +157,10 @@ public:
 				const Point &pointP3 = points[p3];
 
 				float x = (p_offset - pointFirst.offset) / (pointSecond.offset - pointFirst.offset);
-				float r = cubic_interpolate(pointP0.color.r, pointFirst.color.r, pointSecond.color.r, pointP3.color.r, x);
-				float g = cubic_interpolate(pointP0.color.g, pointFirst.color.g, pointSecond.color.g, pointP3.color.g, x);
-				float b = cubic_interpolate(pointP0.color.b, pointFirst.color.b, pointSecond.color.b, pointP3.color.b, x);
-				float a = cubic_interpolate(pointP0.color.a, pointFirst.color.a, pointSecond.color.a, pointP3.color.a, x);
+				float r = Math::cubic_interpolate(pointP0.color.r, pointFirst.color.r, pointSecond.color.r, pointP3.color.r, x);
+				float g = Math::cubic_interpolate(pointP0.color.g, pointFirst.color.g, pointSecond.color.g, pointP3.color.g, x);
+				float b = Math::cubic_interpolate(pointP0.color.b, pointFirst.color.b, pointSecond.color.b, pointP3.color.b, x);
+				float a = Math::cubic_interpolate(pointP0.color.a, pointFirst.color.a, pointSecond.color.a, pointP3.color.a, x);
 
 				return Color(r, g, b, a);
 			} break;