summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/math_funcs.h23
-rw-r--r--core/math/quaternion.cpp51
-rw-r--r--core/math/quaternion.h1
-rw-r--r--core/math/vector2.h8
-rw-r--r--core/math/vector3.h9
-rw-r--r--core/math/vector4.cpp9
-rw-r--r--core/math/vector4.h1
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;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 12:08:59 +0000