Make `cubic_interpolate()` consider key time in animation

7 files changed, 102 insertions, 0 deletions

diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h
index 53deb9bd42..463e119add 100644
--- a/core/math/math_funcs.h
+++ b/core/math/math_funcs.h
@@ -253,6 +253,29 @@ 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_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 */
+		double t = Math::lerp(0.0, p_to_t, p_weight);
+		double a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0 : (t - p_pre_t) / -p_pre_t);
+		double a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5 : t / p_to_t);
+		double a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0 : (t - p_to_t) / (p_post_t - p_to_t));
+		double b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0 : (t - p_pre_t) / (p_to_t - p_pre_t));
+		double b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0 : t / p_post_t);
+		return Math::lerp(b1, b2, p_to_t == 0 ? 0.5 : t / p_to_t);
+	}
+	static _ALWAYS_INLINE_ float cubic_interpolate_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) {
+		/* Barry-Goldman method */
+		float t = Math::lerp(0.0f, p_to_t, p_weight);
+		float a1 = Math::lerp(p_pre, p_from, p_pre_t == 0 ? 0.0f : (t - p_pre_t) / -p_pre_t);
+		float a2 = Math::lerp(p_from, p_to, p_to_t == 0 ? 0.5f : t / p_to_t);
+		float a3 = Math::lerp(p_to, p_post, p_post_t - p_to_t == 0 ? 1.0f : (t - p_to_t) / (p_post_t - p_to_t));
+		float b1 = Math::lerp(a1, a2, p_to_t - p_pre_t == 0 ? 0.0f : (t - p_pre_t) / (p_to_t - p_pre_t));
+		float b2 = Math::lerp(a2, a3, p_post_t == 0 ? 1.0f : t / p_post_t);
+		return Math::lerp(b1, b2, p_to_t == 0 ? 0.5f : t / p_to_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/math/quaternion.cpp b/core/math/quaternion.cpp
index c681c60694..36358f6feb 100644
--- a/core/math/quaternion.cpp
+++ b/core/math/quaternion.cpp
@@ -233,6 +233,57 @@ Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const
 	return q1.slerp(q2, p_weight);
 }
 
+Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight,
+		const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
+#endif
+	Quaternion from_q = *this;
+	Quaternion pre_q = p_pre_a;
+	Quaternion to_q = p_b;
+	Quaternion post_q = p_post_b;
+
+	// Align flip phases.
+	from_q = Basis(from_q).get_rotation_quaternion();
+	pre_q = Basis(pre_q).get_rotation_quaternion();
+	to_q = Basis(to_q).get_rotation_quaternion();
+	post_q = Basis(post_q).get_rotation_quaternion();
+
+	// Flip quaternions to shortest path if necessary.
+	bool flip1 = signbit(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = signbit(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : signbit(to_q.dot(post_q));
+	post_q = flip3 ? -post_q : post_q;
+
+	// Calc by Expmap in from_q space.
+	Quaternion ln_from = Quaternion(0, 0, 0, 0);
+	Quaternion ln_to = (from_q.inverse() * to_q).log();
+	Quaternion ln_pre = (from_q.inverse() * pre_q).log();
+	Quaternion ln_post = (from_q.inverse() * post_q).log();
+	Quaternion ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	Quaternion q1 = from_q * ln.exp();
+
+	// Calc by Expmap in to_q space.
+	ln_from = (to_q.inverse() * from_q).log();
+	ln_to = Quaternion(0, 0, 0, 0);
+	ln_pre = (to_q.inverse() * pre_q).log();
+	ln_post = (to_q.inverse() * post_q).log();
+	ln = Quaternion(0, 0, 0, 0);
+	ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	Quaternion q2 = to_q * ln.exp();
+
+	// To cancel error made by Expmap ambiguity, do blends.
+	return q1.slerp(q2, p_weight);
+}
+
 Quaternion::operator String() const {
 	return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")";
 }
diff --git a/core/math/quaternion.h b/core/math/quaternion.h
index cb54a6f540..43d7bffcfc 100644
--- a/core/math/quaternion.h
+++ b/core/math/quaternion.h
@@ -72,6 +72,7 @@ struct _NO_DISCARD_ Quaternion {
 	Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const;
 	Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const;
 	Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const;
+	Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
 
 	Vector3 get_axis() const;
 	real_t get_angle() const;
diff --git a/core/math/vector2.h b/core/math/vector2.h
index 91d3d3a56b..caa6b226e7 100644
--- a/core/math/vector2.h
+++ b/core/math/vector2.h
@@ -114,6 +114,7 @@ struct _NO_DISCARD_ Vector2 {
 	_FORCE_INLINE_ Vector2 lerp(const Vector2 &p_to, const real_t p_weight) const;
 	_FORCE_INLINE_ Vector2 slerp(const Vector2 &p_to, const real_t p_weight) const;
 	_FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight) const;
+	_FORCE_INLINE_ Vector2 cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
 	_FORCE_INLINE_ Vector2 bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const;
 
 	Vector2 move_toward(const Vector2 &p_to, const real_t p_delta) const;
@@ -270,6 +271,13 @@ Vector2 Vector2::cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, c
 	return res;
 }
 
+Vector2 Vector2::cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
+	Vector2 res = *this;
+	res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	return res;
+}
+
 Vector2 Vector2::bezier_interpolate(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) const {
 	Vector2 res = *this;
 
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 4ce01da60e..eb654008b2 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -105,6 +105,7 @@ struct _NO_DISCARD_ Vector3 {
 	_FORCE_INLINE_ Vector3 lerp(const Vector3 &p_to, const real_t p_weight) const;
 	_FORCE_INLINE_ Vector3 slerp(const Vector3 &p_to, const real_t p_weight) const;
 	_FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight) const;
+	_FORCE_INLINE_ Vector3 cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
 	_FORCE_INLINE_ Vector3 bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const;
 
 	Vector3 move_toward(const Vector3 &p_to, const real_t p_delta) const;
@@ -246,6 +247,14 @@ Vector3 Vector3::cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, c
 	return res;
 }
 
+Vector3 Vector3::cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
+	Vector3 res = *this;
+	res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	return res;
+}
+
 Vector3 Vector3::bezier_interpolate(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) const {
 	Vector3 res = *this;
 
diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp
index 1dd5adad2b..273a111891 100644
--- a/core/math/vector4.cpp
+++ b/core/math/vector4.cpp
@@ -138,6 +138,15 @@ Vector4 Vector4::cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, c
 	return res;
 }
 
+Vector4 Vector4::cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
+	Vector4 res = *this;
+	res.x = Math::cubic_interpolate_in_time(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.y = Math::cubic_interpolate_in_time(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.z = Math::cubic_interpolate_in_time(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	res.w = Math::cubic_interpolate_in_time(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
+	return res;
+}
+
 Vector4 Vector4::posmod(const real_t p_mod) const {
 	return Vector4(Math::fposmod(x, p_mod), Math::fposmod(y, p_mod), Math::fposmod(z, p_mod), Math::fposmod(w, p_mod));
 }
diff --git a/core/math/vector4.h b/core/math/vector4.h
index d26fe15941..17d0de18e1 100644
--- a/core/math/vector4.h
+++ b/core/math/vector4.h
@@ -89,6 +89,7 @@ struct _NO_DISCARD_ Vector4 {
 	Vector4 round() const;
 	Vector4 lerp(const Vector4 &p_to, const real_t p_weight) const;
 	Vector4 cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight) const;
+	Vector4 cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, const real_t p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
 
 	Vector4 posmod(const real_t p_mod) const;
 	Vector4 posmodv(const Vector4 &p_modv) const;