Merge pull request #64924 from TokageItLab/2drot

Add linear/cubic angle interpolation to `Animation::InterpolationType` for shortest 2D rotation

2 files changed, 71 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);
diff --git a/core/variant/variant_utility.cpp b/core/variant/variant_utility.cpp
index 964150aa2d..1be17405c7 100644
--- a/core/variant/variant_utility.cpp
+++ b/core/variant/variant_utility.cpp
@@ -367,11 +367,20 @@ struct VariantUtilityFunctions {
 		return Math::cubic_interpolate(from, to, pre, post, weight);
 	}
 
+	static inline double cubic_interpolate_angle(double from, double to, double pre, double post, double weight) {
+		return Math::cubic_interpolate_angle(from, to, pre, post, weight);
+	}
+
 	static inline double cubic_interpolate_in_time(double from, double to, double pre, double post, double weight,
 			double to_t, double pre_t, double post_t) {
 		return Math::cubic_interpolate_in_time(from, to, pre, post, weight, to_t, pre_t, post_t);
 	}
 
+	static inline double cubic_interpolate_angle_in_time(double from, double to, double pre, double post, double weight,
+			double to_t, double pre_t, double post_t) {
+		return Math::cubic_interpolate_angle_in_time(from, to, pre, post, weight, to_t, pre_t, post_t);
+	}
+
 	static inline double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t) {
 		return Math::bezier_interpolate(p_start, p_control_1, p_control_2, p_end, p_t);
 	}
@@ -1419,7 +1428,9 @@ void Variant::_register_variant_utility_functions() {
 	FUNCBINDVR3(lerp, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(lerpf, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(cubic_interpolate, sarray("from", "to", "pre", "post", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
+	FUNCBINDR(cubic_interpolate_angle, sarray("from", "to", "pre", "post", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(cubic_interpolate_in_time, sarray("from", "to", "pre", "post", "weight", "to_t", "pre_t", "post_t"), Variant::UTILITY_FUNC_TYPE_MATH);
+	FUNCBINDR(cubic_interpolate_angle_in_time, sarray("from", "to", "pre", "post", "weight", "to_t", "pre_t", "post_t"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(bezier_interpolate, sarray("start", "control_1", "control_2", "end", "t"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(lerp_angle, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
 	FUNCBINDR(inverse_lerp, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);