GH-854 Add Monotonic Cubic Interpolation

core/math/math_funcs.h

@@ -827,4 +827,137 @@ static _ALWAYS_INLINE_ float sigmoid_affine_approx(float p_x, float p_amplitude,
 	return p_amplitude * (0.5f + p_x / (4.0f + fabsf(p_x))) + p_y_translation;
 }
 
+// TODO once upgrade to >= C++20 add floating_point constraint to these templates
+template <typename T>
+constexpr T monotonic_cubic_interpolate(T p_from, T p_to, T p_pre, T p_post, T p_weight) {
+	const T d0 = p_from - p_pre;
+	const T d1 = p_to - p_from;
+	const T d2 = p_post - p_to;
+
+	T m1 = 0.0;
+	T m2 = 0.0;
+
+	if (!is_zero_approx(d1)) {
+		m1 = (d0 + d1) * (T)0.5;
+		m2 = (d1 + d2) * (T)0.5;
+
+		if (m1 * d1 <= 0.0) {
+			m1 = 0.0;
+		}
+		if (m2 * d1 <= 0.0) {
+			m2 = 0.0;
+		}
+
+		const T a = m1 / d1;
+		const T b = m2 / d1;
+
+		const T h = sqrt(a * a + b * b);
+
+		if (h > (T)3.0) {
+			const T scale_factor = (T)3.0 / h;
+			m1 *= scale_factor;
+			m2 *= scale_factor;
+		}
+	}
+
+	const T t2 = p_weight * p_weight;
+	const T t3 = t2 * p_weight;
+
+	const T h00 = 2 * t3 - 3 * t2 + 1;
+	const T h10 = t3 - 2 * t2 + p_weight;
+	const T h01 = -2 * t3 + 3 * t2;
+	const T h11 = t3 - t2;
+
+	return (
+			h00 * p_from +
+			h10 * m1 +
+			h01 * p_to +
+			h11 * m2);
+}
+
+template <typename T>
+constexpr T monotonic_cubic_interpolate_in_time(T p_from, T p_to, T p_pre, T p_post, T p_weight, T p_to_t, T p_pre_t, T p_post_t) {
+	if (is_zero_approx(p_to_t)) {
+		return (p_from + p_to) * (T)0.5;
+	}
+
+	const T t_from = (T)0;
+	const T t_to = p_to_t;
+	const T t_pre = p_pre_t;
+	const T t_post = p_post_t;
+
+	const T t = p_weight * t_to;
+
+	const T h0 = t_from - t_pre;
+	const T h1 = t_to - t_from;
+	const T h2 = t_post - t_to;
+
+	const T d0 = is_zero_approx(h0) ? (T)0 : (p_from - p_pre) / h0;
+	const T d1 = (p_to - p_from) / h1;
+	const T d2 = is_zero_approx(h2) ? (T)0 : (p_post - p_to) / h2;
+
+	// Fritsch–Carlson monotonic tangents
+	auto tangent = [](T a, T b, T ha, T hb) -> T {
+		if (a * b <= (T)0) {
+			return (T)0;
+		}
+
+		const T w1 = (T)2 * hb + ha;
+		const T w2 = hb + (T)2 * ha;
+
+		return (w1 + w2) / (w1 / a + w2 / b);
+	};
+
+	const T m1 = tangent(d0, d1, h0, h1);
+	const T m2 = tangent(d1, d2, h1, h2);
+
+	const T s = t / h1;
+
+	const T s2 = s * s;
+	const T s3 = s2 * s;
+
+	// Hermite basis
+	const T h00 = (T)2 * s3 - (T)3 * s2 + (T)1;
+	const T h10 = s3 - (T)2 * s2 + s;
+	const T h01 = -(T)2 * s3 + (T)3 * s2;
+	const T h11 = s3 - s2;
+
+	return h00 * p_from +
+			h10 * h1 * m1 +
+			h01 * p_to +
+			h11 * h1 * m2;
+}
+
+template <typename T>
+constexpr T monotonic_cubic_interpolate_angle(T p_from, T p_to, T p_pre, T p_post, T p_weight) {
+	T from_rot = fmod(p_from, (T)TAU);
+
+	T pre_diff = fmod(p_pre - from_rot, (T)TAU);
+	T pre_rot = from_rot + fmod(2.0f * pre_diff, (T)TAU) - pre_diff;
+
+	T to_diff = fmod(p_to - from_rot, (T)TAU);
+	T to_rot = from_rot + fmod(2.0f * to_diff, (T)TAU) - to_diff;
+
+	T post_diff = fmod(p_post - to_rot, (T)TAU);
+	T post_rot = to_rot + fmod(2.0f * post_diff, (T)TAU) - post_diff;
+
+	return monotonic_cubic_interpolate(from_rot, to_rot, pre_rot, post_rot, p_weight);
+}
+
+template <typename T>
+constexpr T monotonic_cubic_interpolate_angle_in_time(T p_from, T p_to, T p_pre, T p_post, T p_weight, T p_to_t, T p_pre_t, T p_post_t) {
+	T from_rot = fmod(p_from, (T)TAU);
+
+	T pre_diff = fmod(p_pre - from_rot, (T)TAU);
+	T pre_rot = from_rot + fmod((T)2 * pre_diff, (T)TAU) - pre_diff;
+
+	T to_diff = fmod(p_to - from_rot, (T)TAU);
+	T to_rot = from_rot + fmod((T)2 * to_diff, (T)TAU) - to_diff;
+
+	T post_diff = fmod(p_post - to_rot, (T)TAU);
+	T post_rot = to_rot + fmod((T)2 * post_diff, (T)TAU) - post_diff;
+
+	return monotonic_cubic_interpolate_in_time(from_rot, to_rot, pre_rot, post_rot, p_weight, p_to_t, p_pre_t, p_post_t);
+}
+
 }; // namespace Math

core/math/quaternion.cpp

@@ -266,6 +266,107 @@ Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b
 	return q1.slerp(q2, p_weight);
 }
 
+Quaternion Quaternion::spherical_monotonic_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " 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 = std::signbit(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = std::signbit(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : std::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::monotonic_cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+	ln.y = Math::monotonic_cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+	ln.z = Math::monotonic_cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+	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::monotonic_cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
+	ln.y = Math::monotonic_cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
+	ln.z = Math::monotonic_cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
+	Quaternion q2 = to_q * ln.exp();
+
+	// To cancel error made by Expmap ambiguity, do blending.
+	return q1.slerp(q2, p_weight);
+}
+
+Quaternion Quaternion::spherical_monotonic_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight,
+		real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+#ifdef MATH_CHECKS
+	ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion " + operator String() + " must be normalized.");
+	ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion " + p_b.operator String() + " 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 = std::signbit(from_q.dot(pre_q));
+	pre_q = flip1 ? -pre_q : pre_q;
+	bool flip2 = std::signbit(from_q.dot(to_q));
+	to_q = flip2 ? -to_q : to_q;
+	bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : std::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::monotonic_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::monotonic_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::monotonic_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::monotonic_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::monotonic_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::monotonic_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 blending.
+	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) + ")";
 }

core/math/quaternion.h

@@ -76,6 +76,8 @@ struct [[nodiscard]] Quaternion {
 	Quaternion slerpni(const Quaternion &p_to, real_t p_weight) const;
 	Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, 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, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	Quaternion spherical_monotonic_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight) const;
+	Quaternion spherical_monotonic_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
 
 	Vector3 get_axis() const;
 	real_t get_angle() const;

core/math/vector2.h

@@ -121,6 +121,8 @@ struct [[nodiscard]] Vector2 {
 	_FORCE_INLINE_ Vector2 slerp(const Vector2 &p_to, real_t p_weight) const;
 	_FORCE_INLINE_ Vector2 cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, 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, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	_FORCE_INLINE_ Vector2 monotonic_cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight) const;
+	_FORCE_INLINE_ Vector2 monotonic_cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, 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, real_t p_t) const;
 	_FORCE_INLINE_ Vector2 bezier_derivative(const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, real_t p_t) const;
 
@@ -289,6 +291,20 @@ Vector2 Vector2::cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_
 	return res;
 }
 
+Vector2 Vector2::monotonic_cubic_interpolate(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight) const {
+	Vector2 res = *this;
+	res.x = Math::monotonic_cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+	res.y = Math::monotonic_cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+	return res;
+}
+
+Vector2 Vector2::monotonic_cubic_interpolate_in_time(const Vector2 &p_b, const Vector2 &p_pre_a, const Vector2 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+	Vector2 res = *this;
+	res.x = Math::monotonic_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::monotonic_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, real_t p_t) const {
 	Vector2 res = *this;
 	res.x = Math::bezier_interpolate(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t);

core/math/vector3.h

@@ -120,6 +120,8 @@ struct [[nodiscard]] Vector3 {
 	_FORCE_INLINE_ Vector3 slerp(const Vector3 &p_to, real_t p_weight) const;
 	_FORCE_INLINE_ Vector3 cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, 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, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	_FORCE_INLINE_ Vector3 monotonic_cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight) const;
+	_FORCE_INLINE_ Vector3 monotonic_cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, 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, real_t p_t) const;
 	_FORCE_INLINE_ Vector3 bezier_derivative(const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, real_t p_t) const;
 
@@ -276,6 +278,22 @@ Vector3 Vector3::cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_
 	return res;
 }
 
+Vector3 Vector3::monotonic_cubic_interpolate(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight) const {
+	Vector3 res = *this;
+	res.x = Math::monotonic_cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+	res.y = Math::monotonic_cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+	res.z = Math::monotonic_cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight);
+	return res;
+}
+
+Vector3 Vector3::monotonic_cubic_interpolate_in_time(const Vector3 &p_b, const Vector3 &p_pre_a, const Vector3 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+	Vector3 res = *this;
+	res.x = Math::monotonic_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::monotonic_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::monotonic_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, real_t p_t) const {
 	Vector3 res = *this;
 	res.x = Math::bezier_interpolate(res.x, p_control_1.x, p_control_2.x, p_end.x, p_t);

core/math/vector4.cpp

@@ -164,6 +164,24 @@ Vector4 Vector4::cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_
 	return res;
 }
 
+Vector4 Vector4::monotonic_cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight) const {
+	Vector4 res = *this;
+	res.x = Math::monotonic_cubic_interpolate(res.x, p_b.x, p_pre_a.x, p_post_b.x, p_weight);
+	res.y = Math::monotonic_cubic_interpolate(res.y, p_b.y, p_pre_a.y, p_post_b.y, p_weight);
+	res.z = Math::monotonic_cubic_interpolate(res.z, p_b.z, p_pre_a.z, p_post_b.z, p_weight);
+	res.w = Math::monotonic_cubic_interpolate(res.w, p_b.w, p_pre_a.w, p_post_b.w, p_weight);
+	return res;
+}
+
+Vector4 Vector4::monotonic_cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const {
+	Vector4 res = *this;
+	res.x = Math::monotonic_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::monotonic_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::monotonic_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::monotonic_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(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));
 }

core/math/vector4.h

@@ -111,6 +111,8 @@ struct [[nodiscard]] Vector4 {
 	Vector4 lerp(const Vector4 &p_to, real_t p_weight) const;
 	Vector4 cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight) const;
 	Vector4 cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
+	Vector4 monotonic_cubic_interpolate(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight) const;
+	Vector4 monotonic_cubic_interpolate_in_time(const Vector4 &p_b, const Vector4 &p_pre_a, const Vector4 &p_post_b, real_t p_weight, real_t p_b_t, real_t p_pre_a_t, real_t p_post_b_t) const;
 
 	Vector4 posmod(real_t p_mod) const;
 	Vector4 posmodv(const Vector4 &p_modv) const;