Add linear/cubic angle interpolation to Animation interpolation type

1 files changed, 60 insertions, 0 deletions

diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index fd20440663..cae76b182a 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -253,6 +253,35 @@ public:
 						(-p_pre + 3.0f * p_from - 3.0f * p_to + p_post) * (p_weight * p_weight * p_weight));
 	}
 
+	static _ALWAYS_INLINE_ double cubic_interpolate_angle(double p_from, double p_to, double p_pre, double p_post, double p_weight) {
+		double from_rot = fmod(p_from, Math_TAU);
+
+		double pre_diff = fmod(p_pre - from_rot, Math_TAU);
+		double pre_rot = from_rot + fmod(2.0 * pre_diff, Math_TAU) - pre_diff;
+
+		double to_diff = fmod(p_to - from_rot, Math_TAU);
+		double to_rot = from_rot + fmod(2.0 * to_diff, Math_TAU) - to_diff;
+
+		double post_diff = fmod(p_post - to_rot, Math_TAU);
+		double post_rot = to_rot + fmod(2.0 * post_diff, Math_TAU) - post_diff;
+
+		return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
+	}
+	static _ALWAYS_INLINE_ float cubic_interpolate_angle(float p_from, float p_to, float p_pre, float p_post, float p_weight) {
+		float from_rot = fmod(p_from, (float)Math_TAU);
+
+		float pre_diff = fmod(p_pre - from_rot, (float)Math_TAU);
+		float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)Math_TAU) - pre_diff;
+
+		float to_diff = fmod(p_to - from_rot, (float)Math_TAU);
+		float to_rot = from_rot + fmod(2.0f * to_diff, (float)Math_TAU) - to_diff;
+
+		float post_diff = fmod(p_post - to_rot, (float)Math_TAU);
+		float post_rot = to_rot + fmod(2.0f * post_diff, (float)Math_TAU) - post_diff;
+
+		return cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
+	}
+
 	static _ALWAYS_INLINE_ double cubic_interpolate_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight,
 			double p_to_t, double p_pre_t, double p_post_t) {
 		/* Barry-Goldman method */
@@ -276,6 +305,37 @@ public:
 		return Math::lerp(b1, b2, p_to_t == 0 ? 0.5f : t / p_to_t);
 	}
 
+	static _ALWAYS_INLINE_ double cubic_interpolate_angle_in_time(double p_from, double p_to, double p_pre, double p_post, double p_weight,
+			double p_to_t, double p_pre_t, double p_post_t) {
+		double from_rot = fmod(p_from, Math_TAU);
+
+		double pre_diff = fmod(p_pre - from_rot, Math_TAU);
+		double pre_rot = from_rot + fmod(2.0 * pre_diff, Math_TAU) - pre_diff;
+
+		double to_diff = fmod(p_to - from_rot, Math_TAU);
+		double to_rot = from_rot + fmod(2.0 * to_diff, Math_TAU) - to_diff;
+
+		double post_diff = fmod(p_post - to_rot, Math_TAU);
+		double post_rot = to_rot + fmod(2.0 * post_diff, Math_TAU) - post_diff;
+
+		return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
+	}
+	static _ALWAYS_INLINE_ float cubic_interpolate_angle_in_time(float p_from, float p_to, float p_pre, float p_post, float p_weight,
+			float p_to_t, float p_pre_t, float p_post_t) {
+		float from_rot = fmod(p_from, (float)Math_TAU);
+
+		float pre_diff = fmod(p_pre - from_rot, (float)Math_TAU);
+		float pre_rot = from_rot + fmod(2.0f * pre_diff, (float)Math_TAU) - pre_diff;
+
+		float to_diff = fmod(p_to - from_rot, (float)Math_TAU);
+		float to_rot = from_rot + fmod(2.0f * to_diff, (float)Math_TAU) - to_diff;
+
+		float post_diff = fmod(p_post - to_rot, (float)Math_TAU);
+		float post_rot = to_rot + fmod(2.0f * post_diff, (float)Math_TAU) - post_diff;
+
+		return cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
+	}
+
 	static _ALWAYS_INLINE_ double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) {
 		/* Formula from Wikipedia article on Bezier curves. */
 		double omt = (1.0 - p_t);