diff --git a/core/math/math_funcs.h b/core/math/math_funcs.h index 55b91beba1f..34cc3ec6bc9 100644 --- a/core/math/math_funcs.h +++ b/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 +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 +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 +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 +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 diff --git a/core/math/quaternion.cpp b/core/math/quaternion.cpp index 7098187af1b..7bff872175a 100644 --- a/core/math/quaternion.cpp +++ b/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) + ")"; } diff --git a/core/math/quaternion.h b/core/math/quaternion.h index 717e8ff0a21..d99cdae6221 100644 --- a/core/math/quaternion.h +++ b/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; diff --git a/core/math/vector2.h b/core/math/vector2.h index 1f6dfec2224..84a7984f127 100644 --- a/core/math/vector2.h +++ b/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); diff --git a/core/math/vector3.h b/core/math/vector3.h index c78f0024aa1..fc335a64e9f 100644 --- a/core/math/vector3.h +++ b/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); diff --git a/core/math/vector4.cpp b/core/math/vector4.cpp index db7947bc1b2..25d0f316c1d 100644 --- a/core/math/vector4.cpp +++ b/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)); } diff --git a/core/math/vector4.h b/core/math/vector4.h index 032b32b3f5e..f880cecb083 100644 --- a/core/math/vector4.h +++ b/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; diff --git a/core/variant/variant_call.cpp b/core/variant/variant_call.cpp index 0a7bb7d5988..31b22a542f0 100644 --- a/core/variant/variant_call.cpp +++ b/core/variant/variant_call.cpp @@ -1986,6 +1986,8 @@ static void _register_variant_builtin_methods_math() { bind_method(Vector2, slerp, sarray("to", "weight"), varray()); bind_method(Vector2, cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); bind_method(Vector2, cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); + bind_method(Vector2, monotonic_cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); + bind_method(Vector2, monotonic_cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); bind_method(Vector2, bezier_interpolate, sarray("control_1", "control_2", "end", "t"), varray()); bind_method(Vector2, bezier_derivative, sarray("control_1", "control_2", "end", "t"), varray()); bind_method(Vector2, max_axis_index, sarray(), varray()); @@ -2097,6 +2099,8 @@ static void _register_variant_builtin_methods_math() { bind_method(Vector3, slerp, sarray("to", "weight"), varray()); bind_method(Vector3, cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); bind_method(Vector3, cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); + bind_method(Vector3, monotonic_cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); + bind_method(Vector3, monotonic_cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); bind_method(Vector3, bezier_interpolate, sarray("control_1", "control_2", "end", "t"), varray()); bind_method(Vector3, bezier_derivative, sarray("control_1", "control_2", "end", "t"), varray()); bind_method(Vector3, move_toward, sarray("to", "delta"), varray()); @@ -2154,6 +2158,8 @@ static void _register_variant_builtin_methods_math() { bind_method(Vector4, lerp, sarray("to", "weight"), varray()); bind_method(Vector4, cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); bind_method(Vector4, cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); + bind_method(Vector4, monotonic_cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); + bind_method(Vector4, monotonic_cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); bind_method(Vector4, posmod, sarray("mod"), varray()); bind_method(Vector4, posmodv, sarray("modv"), varray()); bind_method(Vector4, snapped, sarray("step"), varray()); @@ -2225,6 +2231,8 @@ static void _register_variant_builtin_methods_math() { bind_method(Quaternion, slerpni, sarray("to", "weight"), varray()); bind_method(Quaternion, spherical_cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); bind_method(Quaternion, spherical_cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); + bind_method(Quaternion, spherical_monotonic_cubic_interpolate, sarray("b", "pre_a", "post_b", "weight"), varray()); + bind_method(Quaternion, spherical_monotonic_cubic_interpolate_in_time, sarray("b", "pre_a", "post_b", "weight", "b_t", "pre_a_t", "post_b_t"), varray()); bind_method(Quaternion, get_euler, sarray("order"), varray((int64_t)EulerOrder::YXZ)); bind_static_method(Quaternion, from_euler, sarray("euler"), varray()); bind_method(Quaternion, get_axis, sarray(), varray()); diff --git a/core/variant/variant_utility.cpp b/core/variant/variant_utility.cpp index b4f3d1f608e..fcfa7ea81b3 100644 --- a/core/variant/variant_utility.cpp +++ b/core/variant/variant_utility.cpp @@ -533,6 +533,24 @@ double VariantUtilityFunctions::cubic_interpolate_angle_in_time(double from, dou return Math::cubic_interpolate_angle_in_time(from, to, pre, post, weight, to_t, pre_t, post_t); } +double VariantUtilityFunctions::monotonic_cubic_interpolate(double from, double to, double pre, double post, double weight) { + return Math::monotonic_cubic_interpolate(from, to, pre, post, weight); +} + +double VariantUtilityFunctions::monotonic_cubic_interpolate_angle(double from, double to, double pre, double post, double weight) { + return Math::monotonic_cubic_interpolate_angle(from, to, pre, post, weight); +} + +double VariantUtilityFunctions::monotonic_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::monotonic_cubic_interpolate_in_time(from, to, pre, post, weight, to_t, pre_t, post_t); +} + +double VariantUtilityFunctions::monotonic_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::monotonic_cubic_interpolate_angle_in_time(from, to, pre, post, weight, to_t, pre_t, post_t); +} + double VariantUtilityFunctions::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); } @@ -1688,6 +1706,10 @@ void Variant::_register_variant_utility_functions() { bind_fn_vuf(inverse_lerp, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "weight"); bind_fn_vuf(remap, Variant::UTILITY_FUNC_TYPE_MATH, "value", "istart", "istop", "ostart", "ostop"); bind_fn_vuf(remap_default, Variant::UTILITY_FUNC_TYPE_MATH, "value", "istart", "istop", "ostart", "ostop", "default_value"); + bind_fn_vuf(monotonic_cubic_interpolate, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "pre", "post", "weight"); + bind_fn_vuf(monotonic_cubic_interpolate_angle, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "pre", "post", "weight"); + bind_fn_vuf(monotonic_cubic_interpolate_in_time, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "pre", "post", "weight", "to_t", "pre_t", "post_t"); + bind_fn_vuf(monotonic_cubic_interpolate_angle_in_time, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "pre", "post", "weight", "to_t", "pre_t", "post_t"); bind_fn_vuf(smoothstep, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "x"); bind_fn_vuf(move_toward, Variant::UTILITY_FUNC_TYPE_MATH, "from", "to", "delta"); diff --git a/core/variant/variant_utility.h b/core/variant/variant_utility.h index ee2d11fb540..366cf04ef79 100644 --- a/core/variant/variant_utility.h +++ b/core/variant/variant_utility.h @@ -89,6 +89,12 @@ struct VariantUtilityFunctions { double to_t, double pre_t, double post_t); static 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); + static double monotonic_cubic_interpolate(double from, double to, double pre, double post, double weight); + static double monotonic_cubic_interpolate_angle(double from, double to, double pre, double post, double weight); + static double monotonic_cubic_interpolate_in_time(double from, double to, double pre, double post, double weight, + double to_t, double pre_t, double post_t); + static double monotonic_cubic_interpolate_angle_in_time(double from, double to, double pre, double post, double weight, + double to_t, double pre_t, double post_t); static double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t); static double bezier_derivative(double p_start, double p_control_1, double p_control_2, double p_end, double p_t); static double angle_difference(double from, double to); diff --git a/doc/classes/@GlobalScope.xml b/doc/classes/@GlobalScope.xml index cceff1a5852..3d6e28eeabb 100644 --- a/doc/classes/@GlobalScope.xml +++ b/doc/classes/@GlobalScope.xml @@ -754,6 +754,79 @@ [/codeblock] + + + + + + + + + Performs monotonic cubic interpolation between [param from] and [param to] + using neighboring values [param pre] and [param post]. + The interpolation factor [param weight] is typically between 0.0 and 1.0. + + Unlike [method cubic_interpolate], this method preserves monotonicity + by automatically limiting tangents to prevent overshoot between key values. + This makes it suitable for animation tracks and other data where values + should not exceed surrounding keyframes. + + + + + + + + + + + Performs monotonic cubic interpolation between angular values, rotating + along the shortest path between [param from] and [param to]. + + The neighboring angles [param pre] and [param post] are used to compute + shape-preserving tangents while accounting for angle wrapping. + This prevents overshoot while maintaining continuous rotation. + See also [method lerp_angle]. + + + + + + + + + + + + + + Time-aware version of [method monotonic_cubic_interpolate_angle]. + + The interpolation factor is derived from the provided keyframe times, + allowing unevenly spaced keyframes to influence tangent calculation. + This produces consistent motion when animation keys are not uniformly + distributed in time while still preventing overshoot. + + + + + + + + + + + + + + Time-aware version of [method monotonic_cubic_interpolate]. + + The interpolation parameter is normalized using the supplied keyframe + times, allowing interpolation to account for non-uniform spacing between + values. Tangents are computed in a way that preserves monotonicity and + prevents overshoot between keyframes. + + diff --git a/doc/classes/Animation.xml b/doc/classes/Animation.xml index bc83de1bbe6..56377584915 100644 --- a/doc/classes/Animation.xml +++ b/doc/classes/Animation.xml @@ -730,6 +730,9 @@ Cubic interpolation. This looks smoother than linear interpolation, but is more expensive to interpolate. Stick to [constant INTERPOLATION_LINEAR] for complex 3D animations imported from external software, even if it requires using a higher animation framerate in return. + + Monotonic Cubic interpolation. This is an alternate version of [constant INTERPOLATION_CUBIC] that ensures no overshooting of key points. + Linear interpolation with shortest path rotation. [b]Note:[/b] The result value is always normalized and may not match the key value. @@ -738,6 +741,9 @@ Cubic interpolation with shortest path rotation. [b]Note:[/b] The result value is always normalized and may not match the key value. + + Monotonic Cubic interpolation with shortest path rotation. + Update between keyframes and hold the value. diff --git a/doc/classes/Quaternion.xml b/doc/classes/Quaternion.xml index f51fe8eee07..de8d1b2382d 100644 --- a/doc/classes/Quaternion.xml +++ b/doc/classes/Quaternion.xml @@ -209,6 +209,30 @@ It can perform smoother interpolation than [method spherical_cubic_interpolate] by the time values. + + + + + + + + Performs a spherical monotonic cubic interpolation between quaternions [param pre_a], this vector, [param b], and [param post_b], by the given amount [param weight]. + + + + + + + + + + + + + Performs a spherical monotonic cubic interpolation between quaternions [param pre_a], this vector, [param b], and [param post_b], by the given amount [param weight]. + It can perform smoother interpolation than [method spherical_monotonic_cubic_interpolate] by the time values. + + diff --git a/doc/classes/Vector2.xml b/doc/classes/Vector2.xml index 7b235042ab3..aeed8d42f25 100644 --- a/doc/classes/Vector2.xml +++ b/doc/classes/Vector2.xml @@ -314,6 +314,30 @@ Returns the component-wise minimum of this and [param with], equivalent to [code]Vector2(minf(x, with), minf(y, with))[/code]. + + + + + + + + Performs a monotonic cubic interpolation between this vector and [param b] using [param pre_a] and [param post_b] as handles, and returns the result at position [param weight]. [param weight] is on the range of 0.0 to 1.0, representing the amount of interpolation. + + + + + + + + + + + + + Performs a monotonic cubic interpolation between this vector and [param b] using [param pre_a] and [param post_b] as handles, and returns the result at position [param weight]. [param weight] is on the range of 0.0 to 1.0, representing the amount of interpolation. + It can perform smoother interpolation than [method monotonic_cubic_interpolate] by the time values. + + diff --git a/doc/classes/Vector3.xml b/doc/classes/Vector3.xml index 63774f1339d..759428f89f4 100644 --- a/doc/classes/Vector3.xml +++ b/doc/classes/Vector3.xml @@ -282,6 +282,30 @@ Returns the component-wise minimum of this and [param with], equivalent to [code]Vector3(minf(x, with), minf(y, with), minf(z, with))[/code]. + + + + + + + + Performs a monotonic cubic interpolation between this vector and [param b] using [param pre_a] and [param post_b] as handles, and returns the result at position [param weight]. [param weight] is on the range of 0.0 to 1.0, representing the amount of interpolation. + + + + + + + + + + + + + Performs a monotonic cubic interpolation between this vector and [param b] using [param pre_a] and [param post_b] as handles, and returns the result at position [param weight]. [param weight] is on the range of 0.0 to 1.0, representing the amount of interpolation. + It can perform smoother interpolation than [method monotonic_cubic_interpolate] by the time values. + + diff --git a/doc/classes/Vector4.xml b/doc/classes/Vector4.xml index bc5201baac5..9af23c1ea19 100644 --- a/doc/classes/Vector4.xml +++ b/doc/classes/Vector4.xml @@ -224,6 +224,30 @@ Returns the component-wise minimum of this and [param with], equivalent to [code]Vector4(minf(x, with), minf(y, with), minf(z, with), minf(w, with))[/code]. + + + + + + + + Performs a monotonic cubic interpolation between this vector and [param b] using [param pre_a] and [param post_b] as handles, and returns the result at position [param weight]. [param weight] is on the range of 0.0 to 1.0, representing the amount of interpolation. + + + + + + + + + + + + + Performs a monotonic cubic interpolation between this vector and [param b] using [param pre_a] and [param post_b] as handles, and returns the result at position [param weight]. [param weight] is on the range of 0.0 to 1.0, representing the amount of interpolation. + It can perform smoother interpolation than [method monotonic_cubic_interpolate] by the time values. + + diff --git a/editor/animation/animation_track_editor.cpp b/editor/animation/animation_track_editor.cpp index 88444b93a79..79f45081590 100644 --- a/editor/animation/animation_track_editor.cpp +++ b/editor/animation/animation_track_editor.cpp @@ -3089,6 +3089,7 @@ void AnimationTrackEdit::gui_input(const Ref &p_event) { menu->add_icon_item(get_editor_theme_icon(SNAME("InterpRaw")), TTR("Nearest"), MENU_INTERPOLATION_NEAREST); menu->add_icon_item(get_editor_theme_icon(SNAME("InterpLinear")), TTR("Linear"), MENU_INTERPOLATION_LINEAR); menu->add_icon_item(get_editor_theme_icon(SNAME("InterpCubic")), TTR("Cubic"), MENU_INTERPOLATION_CUBIC); + menu->add_icon_item(get_editor_theme_icon(SNAME("InterpMonotonic")), TTR("Monotonic Cubic"), MENU_INTERPOLATION_CUBIC_MONOTONIC); // Check whether it is angle property. AnimationPlayerEditor *ape = AnimationPlayerEditor::get_singleton(); if (ape) { @@ -3113,6 +3114,7 @@ void AnimationTrackEdit::gui_input(const Ref &p_event) { if (is_angle) { menu->add_icon_item(get_editor_theme_icon(SNAME("InterpLinearAngle")), TTR("Linear Angle"), MENU_INTERPOLATION_LINEAR_ANGLE); menu->add_icon_item(get_editor_theme_icon(SNAME("InterpCubicAngle")), TTR("Cubic Angle"), MENU_INTERPOLATION_CUBIC_ANGLE); + menu->add_icon_item(get_editor_theme_icon(SNAME("InterpMonotonicAngle")), TTR("Monotonic Cubic Angle"), MENU_INTERPOLATION_CUBIC_MONOTONIC_ANGLE); } } } @@ -3563,8 +3565,10 @@ void AnimationTrackEdit::_menu_selected(int p_index) { case MENU_INTERPOLATION_NEAREST: case MENU_INTERPOLATION_LINEAR: case MENU_INTERPOLATION_CUBIC: + case MENU_INTERPOLATION_CUBIC_MONOTONIC: case MENU_INTERPOLATION_LINEAR_ANGLE: - case MENU_INTERPOLATION_CUBIC_ANGLE: { + case MENU_INTERPOLATION_CUBIC_ANGLE: + case MENU_INTERPOLATION_CUBIC_MONOTONIC_ANGLE: { Animation::InterpolationType interp_mode = Animation::InterpolationType(p_index - MENU_INTERPOLATION_NEAREST); EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); undo_redo->create_action(TTR("Change Animation Interpolation Mode")); @@ -7071,7 +7075,8 @@ void AnimationTrackEditor::_edit_menu_pressed(int p_option) { int len = keys.size() - 1; // Special case for angle interpolation. - bool is_using_angle = animation->track_get_interpolation_type(track) == Animation::INTERPOLATION_LINEAR_ANGLE || animation->track_get_interpolation_type(track) == Animation::INTERPOLATION_CUBIC_ANGLE; + Animation::InterpolationType interp_type = animation->track_get_interpolation_type(track); + bool is_using_angle = interp_type == Animation::INTERPOLATION_LINEAR_ANGLE || interp_type == Animation::INTERPOLATION_CUBIC_ANGLE || interp_type == Animation::INTERPOLATION_CUBIC_MONOTONIC_ANGLE; // Make insert queue. Vector> insert_queue_new; @@ -7304,7 +7309,7 @@ void AnimationTrackEditor::_edit_menu_pressed(int p_option) { } // Special case for angle interpolation. - bool is_using_angle = it == Animation::INTERPOLATION_LINEAR_ANGLE || it == Animation::INTERPOLATION_CUBIC_ANGLE; + bool is_using_angle = it == Animation::INTERPOLATION_LINEAR_ANGLE || it == Animation::INTERPOLATION_CUBIC_ANGLE || it == Animation::INTERPOLATION_CUBIC_MONOTONIC_ANGLE; // Make insert queue. Vector> insert_queue_new; diff --git a/editor/animation/animation_track_editor.h b/editor/animation/animation_track_editor.h index e6fedc75df4..6bf1feb1a38 100644 --- a/editor/animation/animation_track_editor.h +++ b/editor/animation/animation_track_editor.h @@ -422,6 +422,8 @@ class AnimationTrackEdit : public Control { MENU_INTERPOLATION_CUBIC, MENU_INTERPOLATION_LINEAR_ANGLE, MENU_INTERPOLATION_CUBIC_ANGLE, + MENU_INTERPOLATION_CUBIC_MONOTONIC, + MENU_INTERPOLATION_CUBIC_MONOTONIC_ANGLE, MENU_LOOP_WRAP, MENU_LOOP_CLAMP, MENU_KEY_INSERT, diff --git a/editor/icons/InterpMonotonic.svg b/editor/icons/InterpMonotonic.svg new file mode 100644 index 00000000000..e326e9e5f49 --- /dev/null +++ b/editor/icons/InterpMonotonic.svg @@ -0,0 +1 @@ + diff --git a/editor/icons/InterpMonotonicAngle.svg b/editor/icons/InterpMonotonicAngle.svg new file mode 100644 index 00000000000..b101280e1fa --- /dev/null +++ b/editor/icons/InterpMonotonicAngle.svg @@ -0,0 +1 @@ + diff --git a/modules/gltf/structures/gltf_animation.cpp b/modules/gltf/structures/gltf_animation.cpp index ada70d1a79b..a0fcc85ff68 100644 --- a/modules/gltf/structures/gltf_animation.cpp +++ b/modules/gltf/structures/gltf_animation.cpp @@ -54,6 +54,8 @@ GLTFAnimation::Interpolation GLTFAnimation::godot_to_gltf_interpolation(const Re return INTERP_STEP; case Animation::INTERPOLATION_CUBIC: case Animation::INTERPOLATION_CUBIC_ANGLE: + case Animation::INTERPOLATION_CUBIC_MONOTONIC: + case Animation::INTERPOLATION_CUBIC_MONOTONIC_ANGLE: return INTERP_CUBIC_SPLINE; } return INTERP_LINEAR; diff --git a/scene/animation/animation_mixer.cpp b/scene/animation/animation_mixer.cpp index 3ed6ef81bf4..13b9742e004 100644 --- a/scene/animation/animation_mixer.cpp +++ b/scene/animation/animation_mixer.cpp @@ -713,7 +713,10 @@ bool AnimationMixer::_update_caches() { track_value->object_id = child->get_instance_id(); } - track_value->is_using_angle = anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_LINEAR_ANGLE || anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_CUBIC_ANGLE; + track_value->is_using_angle = + anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_LINEAR_ANGLE || + anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_CUBIC_ANGLE || + anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_CUBIC_MONOTONIC_ANGLE; track_value->subpath = leftover_path; @@ -926,7 +929,10 @@ bool AnimationMixer::_update_caches() { if (track_value->init_value.is_string() && anim->value_track_get_update_mode(i) != Animation::UPDATE_DISCRETE) { WARN_PRINT_ONCE_ED(mixer_name + ": '" + String(E) + "', Value Track: '" + String(path) + "' blends String types. This is an experimental algorithm."); } - track_value->is_using_angle = track_value->is_using_angle || anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_LINEAR_ANGLE || anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_CUBIC_ANGLE; + track_value->is_using_angle = track_value->is_using_angle || + anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_LINEAR_ANGLE || + anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_CUBIC_ANGLE || + anim->track_get_interpolation_type(i) == Animation::INTERPOLATION_CUBIC_MONOTONIC_ANGLE; } if (check_angle_interpolation && (was_using_angle != track_value->is_using_angle)) { WARN_PRINT_ED(mixer_name + ": '" + String(E) + "', Value Track: '" + String(path) + "' has different interpolation types for rotation between some animations which may be blended together. Blending prioritizes angle interpolation, so the blending result uses the shortest path referenced to the initial (RESET animation) value."); diff --git a/scene/resources/animation.cpp b/scene/resources/animation.cpp index 39bb4e4039c..cfaf5b72b12 100644 --- a/scene/resources/animation.cpp +++ b/scene/resources/animation.cpp @@ -2522,6 +2522,41 @@ Variant Animation::_cubic_interpolate_angle_in_time(const Variant &p_pre_a, cons return _cubic_interpolate_in_time(p_pre_a, p_a, p_b, p_post_b, p_c, p_pre_a_t, p_b_t, p_post_b_t); } +Vector3 Animation::_monotonic_cubic_interpolate_in_time(const Vector3 &p_pre_a, const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const { + return p_a.monotonic_cubic_interpolate_in_time(p_b, p_pre_a, p_post_b, p_c, p_b_t, p_pre_a_t, p_post_b_t); +} + +Quaternion Animation::_monotonic_cubic_interpolate_in_time(const Quaternion &p_pre_a, const Quaternion &p_a, const Quaternion &p_b, const Quaternion &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const { + return p_a.spherical_monotonic_cubic_interpolate_in_time(p_b, p_pre_a, p_post_b, p_c, p_b_t, p_pre_a_t, p_post_b_t); +} + +Variant Animation::_monotonic_cubic_interpolate_in_time(const Variant &p_pre_a, const Variant &p_a, const Variant &p_b, const Variant &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const { + return monotonic_cubic_interpolate_in_time_variant(p_pre_a, p_a, p_b, p_post_b, p_c, p_pre_a_t, p_b_t, p_post_b_t); +} + +real_t Animation::_monotonic_cubic_interpolate_in_time(const real_t &p_pre_a, const real_t &p_a, const real_t &p_b, const real_t &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const { + return Math::monotonic_cubic_interpolate_in_time(p_a, p_b, p_pre_a, p_post_b, p_c, p_b_t, p_pre_a_t, p_post_b_t); +} + +Variant Animation::_monotonic_cubic_interpolate_angle_in_time(const Variant &p_pre_a, const Variant &p_a, const Variant &p_b, const Variant &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const { + Variant::Type type_a = p_a.get_type(); + Variant::Type type_b = p_b.get_type(); + Variant::Type type_pa = p_pre_a.get_type(); + Variant::Type type_pb = p_post_b.get_type(); + uint32_t vformat = 1 << type_a; + vformat |= 1 << type_b; + vformat |= 1 << type_pa; + vformat |= 1 << type_pb; + if (vformat == ((1 << Variant::INT) | (1 << Variant::FLOAT)) || vformat == (1 << Variant::FLOAT)) { + real_t a = p_a; + real_t b = p_b; + real_t pa = p_pre_a; + real_t pb = p_post_b; + return Math::fposmod((float)Math::monotonic_cubic_interpolate_angle_in_time(a, b, pa, pb, p_c, p_b_t, p_pre_a_t, p_post_b_t), (float)Math::TAU); + } + return _monotonic_cubic_interpolate_in_time(p_pre_a, p_a, p_b, p_post_b, p_c, p_pre_a_t, p_b_t, p_post_b_t); +} + template T Animation::_interpolate(const Vector> &p_keys, double p_time, InterpolationType p_interp, bool p_loop_wrap, bool *p_ok, bool p_backward) const { int len = _find(p_keys, length) + 1; // try to find last key (there may be more past the end) @@ -2560,7 +2595,17 @@ T Animation::_interpolate(const Vector> &p_keys, double p_time, Interpol real_t to_t = 0.0; real_t post_t = 0.0; - bool use_cubic = p_interp == INTERPOLATION_CUBIC || p_interp == INTERPOLATION_CUBIC_ANGLE; + bool use_cubic = false; + switch (p_interp) { + case INTERPOLATION_CUBIC: + case INTERPOLATION_CUBIC_ANGLE: + case INTERPOLATION_CUBIC_MONOTONIC: + case INTERPOLATION_CUBIC_MONOTONIC_ANGLE: + use_cubic = true; + break; + default: + use_cubic = false; + } if (!p_loop_wrap || loop_mode == LOOP_NONE) { if (is_start_edge) { @@ -2683,7 +2728,9 @@ T Animation::_interpolate(const Vector> &p_keys, double p_time, Interpol return _interpolate_angle(p_keys[idx].value, p_keys[next].value, c); } break; case INTERPOLATION_CUBIC: - case INTERPOLATION_CUBIC_ANGLE: { + case INTERPOLATION_CUBIC_ANGLE: + case INTERPOLATION_CUBIC_MONOTONIC: + case INTERPOLATION_CUBIC_MONOTONIC_ANGLE: { if (!p_loop_wrap || loop_mode == LOOP_NONE) { pre_t = p_keys[pre].time - p_keys[idx].time; to_t = p_keys[next].time - p_keys[idx].time; @@ -2711,19 +2758,32 @@ T Animation::_interpolate(const Vector> &p_keys, double p_time, Interpol } } - if (p_interp == INTERPOLATION_CUBIC_ANGLE) { - return _cubic_interpolate_angle_in_time( - p_keys[pre].value, p_keys[idx].value, p_keys[next].value, p_keys[post].value, c, - pre_t, to_t, post_t); + switch (p_interp) { + case INTERPOLATION_CUBIC: + return _cubic_interpolate_in_time( + p_keys[pre].value, p_keys[idx].value, p_keys[next].value, p_keys[post].value, c, + pre_t, to_t, post_t); + case INTERPOLATION_CUBIC_ANGLE: + return _cubic_interpolate_angle_in_time( + p_keys[pre].value, p_keys[idx].value, p_keys[next].value, p_keys[post].value, c, + pre_t, to_t, post_t); + case INTERPOLATION_CUBIC_MONOTONIC: + return _monotonic_cubic_interpolate_in_time( + p_keys[pre].value, p_keys[idx].value, p_keys[next].value, p_keys[post].value, c, + pre_t, to_t, post_t); + case INTERPOLATION_CUBIC_MONOTONIC_ANGLE: + return _monotonic_cubic_interpolate_angle_in_time( + p_keys[pre].value, p_keys[idx].value, p_keys[next].value, p_keys[post].value, c, + pre_t, to_t, post_t); + default: { + } } - return _cubic_interpolate_in_time( - p_keys[pre].value, p_keys[idx].value, p_keys[next].value, p_keys[post].value, c, - pre_t, to_t, post_t); } break; - default: - return p_keys[idx].value; + default: { + } } + return p_keys[idx].value; // do a barrel roll } @@ -4090,8 +4150,10 @@ void Animation::_bind_methods() { BIND_ENUM_CONSTANT(INTERPOLATION_NEAREST); BIND_ENUM_CONSTANT(INTERPOLATION_LINEAR); BIND_ENUM_CONSTANT(INTERPOLATION_CUBIC); + BIND_ENUM_CONSTANT(INTERPOLATION_CUBIC_MONOTONIC); BIND_ENUM_CONSTANT(INTERPOLATION_LINEAR_ANGLE); BIND_ENUM_CONSTANT(INTERPOLATION_CUBIC_ANGLE); + BIND_ENUM_CONSTANT(INTERPOLATION_CUBIC_MONOTONIC_ANGLE); BIND_ENUM_CONSTANT(UPDATE_CONTINUOUS); BIND_ENUM_CONSTANT(UPDATE_DISCRETE); @@ -4398,7 +4460,7 @@ void Animation::_value_track_optimize(int p_idx, real_t p_allowed_velocity_err, Variant::Type type = vt->values[0].value.get_type(); // Special case for angle interpolation. - bool is_using_angle = vt->interpolation == Animation::INTERPOLATION_LINEAR_ANGLE || vt->interpolation == Animation::INTERPOLATION_CUBIC_ANGLE; + bool is_using_angle = vt->interpolation == Animation::INTERPOLATION_LINEAR_ANGLE || vt->interpolation == Animation::INTERPOLATION_CUBIC_ANGLE || vt->interpolation == Animation::INTERPOLATION_CUBIC_MONOTONIC_ANGLE; int i = 0; while (i < vt->values.size() - 2) { bool erase = false; @@ -6501,6 +6563,196 @@ Variant Animation::cubic_interpolate_in_time_variant(const Variant &pre_a, const return c < 0.5 ? a : b; } +// TODO this is a nearly exact copy of cubic_interpolate_in_time_variant() +// Maybe someway to refactor this +Variant Animation::monotonic_cubic_interpolate_in_time_variant(const Variant &pre_a, const Variant &a, const Variant &b, const Variant &post_b, float c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t, bool p_snap_array_element) { + if (pre_a.get_type() != a.get_type() || pre_a.get_type() != b.get_type() || pre_a.get_type() != post_b.get_type()) { + if (pre_a.is_num() && a.is_num() && b.is_num() && post_b.is_num()) { + return monotonic_cubic_interpolate_in_time_variant(cast_to_blendwise(pre_a), cast_to_blendwise(a), cast_to_blendwise(b), cast_to_blendwise(post_b), c, p_pre_a_t, p_b_t, p_post_b_t, p_snap_array_element); + } else if (!a.is_array()) { + return a; + } + } + + switch (a.get_type()) { + case Variant::NIL: { + return Variant(); + } break; + case Variant::FLOAT: { + return Math::monotonic_cubic_interpolate_in_time(a.operator double(), b.operator double(), pre_a.operator double(), post_b.operator double(), (double)c, (double)p_b_t, (double)p_pre_a_t, (double)p_post_b_t); + } break; + case Variant::VECTOR2: { + return (a.operator Vector2()).monotonic_cubic_interpolate_in_time(b.operator Vector2(), pre_a.operator Vector2(), post_b.operator Vector2(), c, p_b_t, p_pre_a_t, p_post_b_t); + } break; + case Variant::RECT2: { + const Rect2 rpa = pre_a.operator Rect2(); + const Rect2 ra = a.operator Rect2(); + const Rect2 rb = b.operator Rect2(); + const Rect2 rpb = post_b.operator Rect2(); + return Rect2( + ra.position.monotonic_cubic_interpolate_in_time(rb.position, rpa.position, rpb.position, c, p_b_t, p_pre_a_t, p_post_b_t), + ra.size.monotonic_cubic_interpolate_in_time(rb.size, rpa.size, rpb.size, c, p_b_t, p_pre_a_t, p_post_b_t)); + } break; + case Variant::VECTOR3: { + return (a.operator Vector3()).monotonic_cubic_interpolate_in_time(b.operator Vector3(), pre_a.operator Vector3(), post_b.operator Vector3(), c, p_b_t, p_pre_a_t, p_post_b_t); + } break; + case Variant::VECTOR4: { + return (a.operator Vector4()).monotonic_cubic_interpolate_in_time(b.operator Vector4(), pre_a.operator Vector4(), post_b.operator Vector4(), c, p_b_t, p_pre_a_t, p_post_b_t); + } break; + case Variant::PLANE: { + const Plane ppa = pre_a.operator Plane(); + const Plane pa = a.operator Plane(); + const Plane pb = b.operator Plane(); + const Plane ppb = post_b.operator Plane(); + return Plane( + pa.normal.monotonic_cubic_interpolate_in_time(pb.normal, ppa.normal, ppb.normal, c, p_b_t, p_pre_a_t, p_post_b_t), + Math::monotonic_cubic_interpolate_in_time((double)pa.d, (double)pb.d, (double)ppa.d, (double)ppb.d, (double)c, (double)p_b_t, (double)p_pre_a_t, (double)p_post_b_t)); + } break; + case Variant::COLOR: { + const Color cpa = pre_a.operator Color(); + const Color ca = a.operator Color(); + const Color cb = b.operator Color(); + const Color cpb = post_b.operator Color(); + return Color( + Math::monotonic_cubic_interpolate_in_time((double)ca.r, (double)cb.r, (double)cpa.r, (double)cpb.r, (double)c, (double)p_b_t, (double)p_pre_a_t, (double)p_post_b_t), + Math::monotonic_cubic_interpolate_in_time((double)ca.g, (double)cb.g, (double)cpa.g, (double)cpb.g, (double)c, (double)p_b_t, (double)p_pre_a_t, (double)p_post_b_t), + Math::monotonic_cubic_interpolate_in_time((double)ca.b, (double)cb.b, (double)cpa.b, (double)cpb.b, (double)c, (double)p_b_t, (double)p_pre_a_t, (double)p_post_b_t), + Math::monotonic_cubic_interpolate_in_time((double)ca.a, (double)cb.a, (double)cpa.a, (double)cpb.a, (double)c, (double)p_b_t, (double)p_pre_a_t, (double)p_post_b_t)); + } break; + case Variant::AABB: { + const ::AABB apa = pre_a.operator ::AABB(); + const ::AABB aa = a.operator ::AABB(); + const ::AABB ab = b.operator ::AABB(); + const ::AABB apb = post_b.operator ::AABB(); + return AABB( + aa.position.monotonic_cubic_interpolate_in_time(ab.position, apa.position, apb.position, c, p_b_t, p_pre_a_t, p_post_b_t), + aa.size.monotonic_cubic_interpolate_in_time(ab.size, apa.size, apb.size, c, p_b_t, p_pre_a_t, p_post_b_t)); + } break; + case Variant::BASIS: { + const Basis bpa = pre_a.operator Basis(); + const Basis ba = a.operator Basis(); + const Basis bb = b.operator Basis(); + const Basis bpb = post_b.operator Basis(); + return Basis( + ba.rows[0].monotonic_cubic_interpolate_in_time(bb.rows[0], bpa.rows[0], bpb.rows[0], c, p_b_t, p_pre_a_t, p_post_b_t), + ba.rows[1].monotonic_cubic_interpolate_in_time(bb.rows[1], bpa.rows[1], bpb.rows[1], c, p_b_t, p_pre_a_t, p_post_b_t), + ba.rows[2].monotonic_cubic_interpolate_in_time(bb.rows[2], bpa.rows[2], bpb.rows[2], c, p_b_t, p_pre_a_t, p_post_b_t)); + } break; + case Variant::QUATERNION: { + return (a.operator Quaternion()).spherical_monotonic_cubic_interpolate_in_time(b.operator Quaternion(), pre_a.operator Quaternion(), post_b.operator Quaternion(), c, p_b_t, p_pre_a_t, p_post_b_t); + } break; + case Variant::TRANSFORM2D: { + const Transform2D tpa = pre_a.operator Transform2D(); + const Transform2D ta = a.operator Transform2D(); + const Transform2D tb = b.operator Transform2D(); + const Transform2D tpb = post_b.operator Transform2D(); + // TODO: May cause unintended skew, we needs spherical_cubic_interpolate_in_time() for angle and Transform2D::cubic_interpolate_with(). + return Transform2D( + ta[0].monotonic_cubic_interpolate_in_time(tb[0], tpa[0], tpb[0], c, p_b_t, p_pre_a_t, p_post_b_t), + ta[1].monotonic_cubic_interpolate_in_time(tb[1], tpa[1], tpb[1], c, p_b_t, p_pre_a_t, p_post_b_t), + ta[2].monotonic_cubic_interpolate_in_time(tb[2], tpa[2], tpb[2], c, p_b_t, p_pre_a_t, p_post_b_t)); + } break; + case Variant::TRANSFORM3D: { + const Transform3D tpa = pre_a.operator Transform3D(); + const Transform3D ta = a.operator Transform3D(); + const Transform3D tb = b.operator Transform3D(); + const Transform3D tpb = post_b.operator Transform3D(); + // TODO: May cause unintended skew, we needs Transform3D::cubic_interpolate_with(). + return Transform3D( + ta.basis.rows[0].monotonic_cubic_interpolate_in_time(tb.basis.rows[0], tpa.basis.rows[0], tpb.basis.rows[0], c, p_b_t, p_pre_a_t, p_post_b_t), + ta.basis.rows[1].monotonic_cubic_interpolate_in_time(tb.basis.rows[1], tpa.basis.rows[1], tpb.basis.rows[1], c, p_b_t, p_pre_a_t, p_post_b_t), + ta.basis.rows[2].monotonic_cubic_interpolate_in_time(tb.basis.rows[2], tpa.basis.rows[2], tpb.basis.rows[2], c, p_b_t, p_pre_a_t, p_post_b_t), + ta.origin.monotonic_cubic_interpolate_in_time(tb.origin, tpa.origin, tpb.origin, c, p_b_t, p_pre_a_t, p_post_b_t)); + } break; + case Variant::BOOL: + case Variant::INT: + case Variant::RECT2I: + case Variant::VECTOR2I: + case Variant::VECTOR3I: + case Variant::VECTOR4I: + case Variant::PACKED_INT32_ARRAY: + case Variant::PACKED_INT64_ARRAY: { + // Fallback the interpolatable value which needs casting. + return cast_from_blendwise(monotonic_cubic_interpolate_in_time_variant(cast_to_blendwise(pre_a), cast_to_blendwise(a), cast_to_blendwise(b), cast_to_blendwise(post_b), c, p_pre_a_t, p_b_t, p_post_b_t, p_snap_array_element), a.get_type()); + } break; + case Variant::STRING: + case Variant::STRING_NAME: { + // TODO: + // String interpolation works on both the character array size and the character code, to apply cubic interpolation neatly, + // we need to figure out how to interpolate well in cases where there are fewer than 4 keys. So, for now, fallback to linear interpolation. + return interpolate_variant(a, b, c); + } break; + case Variant::PACKED_BYTE_ARRAY: { + // Skip. + } break; + default: { + if (a.is_array()) { + const Array arr_pa = pre_a.operator Array(); + const Array arr_a = a.operator Array(); + const Array arr_b = b.operator Array(); + const Array arr_pb = post_b.operator Array(); + + int min_size = arr_a.size(); + int max_size = arr_b.size(); + bool is_a_larger = inform_variant_array(min_size, max_size); + + Array result; + result.set_typed(MAX(arr_a.get_typed_builtin(), arr_b.get_typed_builtin()), StringName(), Variant()); + result.resize(min_size); + + if (min_size == 0 && max_size == 0) { + return result; + } + + Variant vz; + if (is_a_larger) { + vz = arr_a[0]; + } else { + vz = arr_b[0]; + } + vz.zero(); + Variant pre_last = arr_pa.size() ? arr_pa[arr_pa.size() - 1] : vz; + Variant post_last = arr_pb.size() ? arr_pb[arr_pb.size() - 1] : vz; + + int i = 0; + for (; i < min_size; i++) { + result[i] = monotonic_cubic_interpolate_in_time_variant(i >= arr_pa.size() ? pre_last : arr_pa[i], arr_a[i], arr_b[i], i >= arr_pb.size() ? post_last : arr_pb[i], c, p_pre_a_t, p_b_t, p_post_b_t); + } + if (min_size != max_size) { + // Process with last element of the lesser array. + // This is pretty funny and bizarre, but artists like to use it for polygon animation. + Variant lesser_last = vz; + if (is_a_larger && !Math::is_equal_approx(c, 1.0f)) { + result.resize(max_size); + if (p_snap_array_element) { + c = 0; + } + if (i > 0) { + lesser_last = arr_b[i - 1]; + } + for (; i < max_size; i++) { + result[i] = monotonic_cubic_interpolate_in_time_variant(i >= arr_pa.size() ? pre_last : arr_pa[i], arr_a[i], lesser_last, i >= arr_pb.size() ? post_last : arr_pb[i], c, p_pre_a_t, p_b_t, p_post_b_t); + } + } else if (!is_a_larger && !Math::is_zero_approx(c)) { + result.resize(max_size); + if (p_snap_array_element) { + c = 1; + } + if (i > 0) { + lesser_last = arr_a[i - 1]; + } + for (; i < max_size; i++) { + result[i] = monotonic_cubic_interpolate_in_time_variant(i >= arr_pa.size() ? pre_last : arr_pa[i], lesser_last, arr_b[i], i >= arr_pb.size() ? post_last : arr_pb[i], c, p_pre_a_t, p_b_t, p_post_b_t); + } + } + } + return result; + } + } break; + } + return c < 0.5 ? a : b; +} + bool Animation::inform_variant_array(int &r_min, int &r_max) { if (r_min <= r_max) { return false; diff --git a/scene/resources/animation.h b/scene/resources/animation.h index 652604dd791..f6feb1c2b5b 100644 --- a/scene/resources/animation.h +++ b/scene/resources/animation.h @@ -65,6 +65,8 @@ class Animation : public Resource { INTERPOLATION_CUBIC, INTERPOLATION_LINEAR_ANGLE, INTERPOLATION_CUBIC_ANGLE, + INTERPOLATION_CUBIC_MONOTONIC, + INTERPOLATION_CUBIC_MONOTONIC_ANGLE, }; enum UpdateMode : uint8_t { @@ -275,6 +277,12 @@ class Animation : public Resource { _FORCE_INLINE_ real_t _cubic_interpolate_in_time(const real_t &p_pre_a, const real_t &p_a, const real_t &p_b, const real_t &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; _FORCE_INLINE_ Variant _cubic_interpolate_angle_in_time(const Variant &p_pre_a, const Variant &p_a, const Variant &p_b, const Variant &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; + _FORCE_INLINE_ Vector3 _monotonic_cubic_interpolate_in_time(const Vector3 &p_pre_a, const Vector3 &p_a, const Vector3 &p_b, const Vector3 &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; + _FORCE_INLINE_ Quaternion _monotonic_cubic_interpolate_in_time(const Quaternion &p_pre_a, const Quaternion &p_a, const Quaternion &p_b, const Quaternion &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; + _FORCE_INLINE_ Variant _monotonic_cubic_interpolate_in_time(const Variant &p_pre_a, const Variant &p_a, const Variant &p_b, const Variant &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; + _FORCE_INLINE_ real_t _monotonic_cubic_interpolate_in_time(const real_t &p_pre_a, const real_t &p_a, const real_t &p_b, const real_t &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; + _FORCE_INLINE_ Variant _monotonic_cubic_interpolate_angle_in_time(const Variant &p_pre_a, const Variant &p_a, const Variant &p_b, const Variant &p_post_b, real_t p_c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t) const; + template _FORCE_INLINE_ T _interpolate(const Vector> &p_keys, double p_time, InterpolationType p_interp, bool p_loop_wrap, bool *p_ok, bool p_backward = false) const; @@ -557,6 +565,7 @@ class Animation : public Resource { static Variant blend_variant(const Variant &a, const Variant &b, float c); static Variant interpolate_variant(const Variant &a, const Variant &b, float c, bool p_snap_array_element = false); static Variant cubic_interpolate_in_time_variant(const Variant &pre_a, const Variant &a, const Variant &b, const Variant &post_b, float c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t, bool p_snap_array_element = false); + static Variant monotonic_cubic_interpolate_in_time_variant(const Variant &pre_a, const Variant &a, const Variant &b, const Variant &post_b, float c, real_t p_pre_a_t, real_t p_b_t, real_t p_post_b_t, bool p_snap_array_element = false); static bool is_less_or_equal_approx(double a, double b) { return a < b || Math::is_equal_approx(a, b); diff --git a/tests/core/math/test_math_funcs.h b/tests/core/math/test_math_funcs.h index df871745dae..972a39b11dd 100644 --- a/tests/core/math/test_math_funcs.h +++ b/tests/core/math/test_math_funcs.h @@ -32,6 +32,7 @@ #pragma once +#include "core/math/math_funcs.h" #include "tests/test_macros.h" namespace TestMath { @@ -654,4 +655,48 @@ TEST_CASE_TEMPLATE("[Math] sigmoid_affine_approx", T, float, double) { CHECK(Math::sigmoid_affine_approx((T)1.0, (T)2.0, (T)2.5) == doctest::Approx((T)3.9)); } +TEST_CASE_TEMPLATE("[Math] monotonic_cubic_interpolate", T, float, double) { + CHECK(Math::monotonic_cubic_interpolate((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.0) == doctest::Approx((T)0.2)); + CHECK(Math::monotonic_cubic_interpolate((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.25) == doctest::Approx((T)0.33125)); + CHECK(Math::monotonic_cubic_interpolate((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.5) == doctest::Approx((T)0.5)); + CHECK(Math::monotonic_cubic_interpolate((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.75) == doctest::Approx((T)0.66875)); + CHECK(Math::monotonic_cubic_interpolate((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)1.0) == doctest::Approx((T)0.8)); + + CHECK(Math::monotonic_cubic_interpolate((T)20.2, (T)30.1, (T)-100.0, (T)32.0, (T)-50.0) == doctest::Approx((T)-1639477.1)); + CHECK(Math::monotonic_cubic_interpolate((T)20.2, (T)30.1, (T)-100.0, (T)32.0, (T)-5.0) == doctest::Approx((T)-2488.86)); + CHECK(Math::monotonic_cubic_interpolate((T)20.2, (T)30.1, (T)-100.0, (T)32.0, (T)0.0) == doctest::Approx((T)20.2)); + CHECK(Math::monotonic_cubic_interpolate((T)20.2, (T)30.1, (T)-100.0, (T)32.0, (T)1.0) == doctest::Approx((T)30.1)); + CHECK(Math::monotonic_cubic_interpolate((T)20.2, (T)30.1, (T)-100.0, (T)32.0, (T)4.0) == doctest::Approx((T)421.8)); +} + +TEST_CASE_TEMPLATE("[Math] monotonic_cubic_interpolate_angle", T, float, double) { + CHECK(Math::monotonic_cubic_interpolate_angle((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.0) == doctest::Approx((T)Math::PI * (1.0 / 6.0))); + CHECK(Math::monotonic_cubic_interpolate_angle((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.25) == doctest::Approx((T)0.973566)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.5) == doctest::Approx((T)Math::PI / 2.0)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.75) == doctest::Approx((T)2.16803)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)1.0) == doctest::Approx((T)Math::PI * (5.0 / 6.0))); + + CHECK(Math::monotonic_cubic_interpolate_angle((T)0.0, (T)0.0, (T)(Math::PI / 2.0), (T)(Math::PI / 2.0), (T)0.0) == doctest::Approx((T)0.0)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)0.0, (T)0.0, (T)(Math::PI / 2.0), (T)(Math::PI / 2.0), (T)0.25) == doctest::Approx((T)0.0)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)0.0, (T)0.0, (T)(Math::PI / 2.0), (T)(Math::PI / 2.0), (T)0.5) == doctest::Approx((T)0.0)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)0.0, (T)0.0, (T)(Math::PI / 2.0), (T)(Math::PI / 2.0), (T)0.75) == doctest::Approx((T)0.0)); + CHECK(Math::monotonic_cubic_interpolate_angle((T)0.0, (T)0.0, (T)(Math::PI / 2.0), (T)(Math::PI / 2.0), (T)1.0) == doctest::Approx((T)0.0)); +} + +TEST_CASE_TEMPLATE("[Math] monotonic_cubic_interpolate_in_time", T, float, double) { + CHECK(Math::monotonic_cubic_interpolate_in_time((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.0, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)0.2)); + CHECK(Math::monotonic_cubic_interpolate_in_time((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.25, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)0.279687)); + CHECK(Math::monotonic_cubic_interpolate_in_time((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.5, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)0.4625)); + CHECK(Math::monotonic_cubic_interpolate_in_time((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)0.75, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)0.664062)); + CHECK(Math::monotonic_cubic_interpolate_in_time((T)0.2, (T)0.8, (T)0.0, (T)1.0, (T)1.0, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)0.8)); +} + +TEST_CASE_TEMPLATE("[Math] monotonic_cubic_interpolate_angle_in_time", T, float, double) { + CHECK(Math::monotonic_cubic_interpolate_angle_in_time((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.0, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)(Math::PI * (1.0 / 6.0)))); + CHECK(Math::monotonic_cubic_interpolate_angle_in_time((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.25, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)0.811578)); + CHECK(Math::monotonic_cubic_interpolate_angle_in_time((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.5, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)1.46608)); + CHECK(Math::monotonic_cubic_interpolate_angle_in_time((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)0.75, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)2.17293)); + CHECK(Math::monotonic_cubic_interpolate_angle_in_time((T)(Math::PI * (1.0 / 6.0)), (T)(Math::PI * (5.0 / 6.0)), (T)0.0, (T)Math::PI, (T)1.0, (T)0.5, (T)0.0, (T)1.0) == doctest::Approx((T)Math::PI * (5.0 / 6.0))); +} + } // namespace TestMath