Implement cubic_interpolate() as MathFunc for refactoring

3 files changed, 24 insertions, 32 deletions

diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 6b5eb655d3..47e5ab2709 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -235,6 +235,21 @@ public:
 	static _ALWAYS_INLINE_ double lerp(double p_from, double p_to, double p_weight) { return p_from + (p_to - p_from) * p_weight; }
 	static _ALWAYS_INLINE_ float lerp(float p_from, float p_to, float p_weight) { return p_from + (p_to - p_from) * p_weight; }
 
+	static _ALWAYS_INLINE_ double cubic_interpolate(double p_from, double p_to, double p_pre, double p_post, double p_weight) {
+		return 0.5 *
+				((p_from * 2.0) +
+						(-p_pre + p_to) * p_weight +
+						(2.0 * p_pre - 5.0 * p_from + 4.0 * p_to - p_post) * (p_weight * p_weight) +
+						(-p_pre + 3.0 * p_from - 3.0 * p_to + p_post) * (p_weight * p_weight * p_weight));
+	}
+	static _ALWAYS_INLINE_ float cubic_interpolate(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
+		return 0.5f *
+				((p_from * 2.0f) +
+						(-p_pre + p_to) * p_weight +
+						(2.0f * p_pre - 5.0f * p_from + 4.0f * p_to - p_post) * (p_weight * p_weight) +
+						(-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight));
+	}
+
 	static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) {
 		double difference = fmod(p_to - p_from, Math_TAU);
 		double distance = fmod(2.0 * difference, Math_TAU) - difference;
diff --git a/core/math/vector2.cpp b/core/math/vector2.cpp
index 120b66e432..ed4266b115 100644
--- a/core/math/vector2.cpp
+++ b/core/math/vector2.cpp
@@ -153,22 +153,10 @@ Vector2 Vector2::limit_length(const real_t p_len) const {
 }
 
 Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const {
-	Vector2 p0 = p_pre_a;
-	Vector2 p1 = *this;
-	Vector2 p2 = p_b;
-	Vector2 p3 = p_post_b;
-
-	real_t t = p_weight;
-	real_t t2 = t * t;
-	real_t t3 = t2 * t;
-
-	Vector2 out;
-	out = 0.5f *
-			((p1 * 2.0f) +
-					(-p0 + p2) * t +
-					(2.0f * p0 - 5.0f * p1 + 4 * p2 - p3) * t2 +
-					(-p0 + 3.0f * p1 - 3.0f * p2 + p3) * t3);
-	return out;
+	Vector2 res = *this;
+	res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+	res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+	return res;
 }
 
 Vector2 Vector2::move_toward(const Vector2 &p_to, const real_t p_delta) const {
diff --git a/core/math/vector3.cpp b/core/math/vector3.cpp
index bafb01da59..998c437a22 100644
--- a/core/math/vector3.cpp
+++ b/core/math/vector3.cpp
@@ -83,22 +83,11 @@ Vector3 Vector3::limit_length(const real_t p_len) const {
 }
 
 Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const {
-	Vector3 p0 = p_pre_a;
-	Vector3 p1 = *this;
-	Vector3 p2 = p_b;
-	Vector3 p3 = p_post_b;
-
-	real_t t = p_weight;
-	real_t t2 = t * t;
-	real_t t3 = t2 * t;
-
-	Vector3 out;
-	out = 0.5f *
-			((p1 * 2.0f) +
-					(-p0 + p2) * t +
-					(2.0f * p0 - 5.0f * p1 + 4.0f * p2 - p3) * t2 +
-					(-p0 + 3.0f * p1 - 3.0f * p2 + p3) * t3);
-	return out;
+	Vector3 res = *this;
+	res.x = Math::cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+	res.y = Math::cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+	res.z = Math::cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight);
+	return res;
 }
 
 Vector3 Vector3::move_toward(const Vector3 &p_to, const real_t p_delta) const {