From b35fcf362027d147dba359911d0e94d3160ff238 Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 12:36:46 +0200 Subject: [PATCH 1/8] C#: Add missing match check in `Quaternion.Slerpni` --- .../glue/GodotSharp/GodotSharp/Core/Quaternion.cs | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 999500ca13cc..529ddc2cfb70 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -292,6 +292,17 @@ public Quaternion Slerp(Quaternion to, real_t weight) /// The resulting quaternion of the interpolation. public Quaternion Slerpni(Quaternion to, real_t weight) { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } + if (!to.IsNormalized()) + { + throw new ArgumentException("Argument is not normalized", nameof(to)); + } +#endif + real_t dot = Dot(to); if (Mathf.Abs(dot) > 0.9999f) From 961a086d208ce3082a2ddb43894ffe413b3ff5fe Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 11:46:38 +0200 Subject: [PATCH 2/8] C#: Fix `Transform3D` interpolation and add spherical interpolation --- .../glue/GodotSharp/GodotSharp/Core/Basis.cs | 9 ++++ .../GodotSharp/GodotSharp/Core/Transform3D.cs | 48 ++++++++++++------- 2 files changed, 40 insertions(+), 17 deletions(-) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs index 87adf9efe5d6..ed20067a9228 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs @@ -498,6 +498,15 @@ public Basis Inverse() ); } + internal Basis Lerp(Basis to, real_t weight) + { + Basis b = this; + b.Row0 = Row0.Lerp(to.Row0, weight); + b.Row1 = Row1.Lerp(to.Row1, weight); + b.Row2 = Row2.Lerp(to.Row2, weight); + return b; + } + /// /// Returns the orthonormalized version of the basis matrix (useful to /// call occasionally to avoid rounding errors for orthogonal matrices). diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs index 4b739bb86bc2..3c017ecc9f27 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs @@ -119,23 +119,9 @@ public Transform3D AffineInverse() /// The interpolated transform. public Transform3D InterpolateWith(Transform3D transform, real_t weight) { - /* not sure if very "efficient" but good enough? */ - - Vector3 sourceScale = basis.Scale; - Quaternion sourceRotation = basis.GetRotationQuaternion(); - Vector3 sourceLocation = origin; - - Vector3 destinationScale = transform.basis.Scale; - Quaternion destinationRotation = transform.basis.GetRotationQuaternion(); - Vector3 destinationLocation = transform.origin; - - var interpolated = new Transform3D(); - Quaternion quaternion = sourceRotation.Slerp(destinationRotation, weight).Normalized(); - Vector3 scale = sourceScale.Lerp(destinationScale, weight); - interpolated.basis.SetQuaternionScale(quaternion, scale); - interpolated.origin = sourceLocation.Lerp(destinationLocation, weight); - - return interpolated; + Basis retBasis = basis.Lerp(transform.basis, weight); + Vector3 retOrigin = origin.Lerp(transform.origin, weight); + return new Transform3D(retBasis, retOrigin); } /// @@ -234,6 +220,34 @@ public Transform3D ScaledLocal(Vector3 scale) return new Transform3D(basis * tmpBasis, origin); } + /// + /// Returns a transform spherically interpolated between this transform and + /// another by . + /// + /// The other transform. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// The interpolated transform. + public Transform3D SphericalInterpolateWith(Transform3D transform, real_t weight) + { + /* not sure if very "efficient" but good enough? */ + + Vector3 sourceScale = basis.Scale; + Quaternion sourceRotation = basis.GetRotationQuaternion(); + Vector3 sourceLocation = origin; + + Vector3 destinationScale = transform.basis.Scale; + Quaternion destinationRotation = transform.basis.GetRotationQuaternion(); + Vector3 destinationLocation = transform.origin; + + var interpolated = new Transform3D(); + Quaternion quaternion = sourceRotation.Slerp(destinationRotation, weight).Normalized(); + Vector3 scale = sourceScale.Lerp(destinationScale, weight); + interpolated.basis.SetQuaternionScale(quaternion, scale); + interpolated.origin = sourceLocation.Lerp(destinationLocation, weight); + + return interpolated; + } + private void SetLookAt(Vector3 eye, Vector3 target, Vector3 up) { // Make rotation matrix From b526a0d824939b0f33897858721ad45f3fb0fecb Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 11:10:48 +0200 Subject: [PATCH 3/8] C#: Add `GetAngle` and `GetAxis` to Quaternion --- .../GodotSharp/GodotSharp/Core/Quaternion.cs | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 529ddc2cfb70..6c926386517a 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -158,6 +158,22 @@ public real_t Dot(Quaternion b) return (x * b.x) + (y * b.y) + (z * b.z) + (w * b.w); } + public real_t GetAngle() + { + return 2 * Mathf.Acos(w); + } + + public Vector3 GetAxis() + { + if (Mathf.Abs(w) > 1 - Mathf.Epsilon) + { + return new Vector3(x, y, z); + } + + real_t r = 1 / Mathf.Sqrt(1 - w * w); + return new Vector3(x * r, y * r, z * r); + } + /// /// Returns Euler angles (in the YXZ convention: when decomposing, /// first Z, then X, and Y last) corresponding to the rotation From 8ad0ef75b8f0e4e9f60cc1b2fe71b30197c61f98 Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 13:00:43 +0200 Subject: [PATCH 4/8] C#: Add `Exp` and `Log` to Quaternion --- .../GodotSharp/GodotSharp/Core/Quaternion.cs | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 6c926386517a..fea6a49b7d6b 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -158,6 +158,18 @@ public real_t Dot(Quaternion b) return (x * b.x) + (y * b.y) + (z * b.z) + (w * b.w); } + public Quaternion Exp() + { + Vector3 v = new Vector3(x, y, z); + real_t theta = v.Length(); + v = v.Normalized(); + if (theta < Mathf.Epsilon || !v.IsNormalized()) + { + return new Quaternion(0, 0, 0, 1); + } + return new Quaternion(v, theta); + } + public real_t GetAngle() { return 2 * Mathf.Acos(w); @@ -217,6 +229,12 @@ public bool IsNormalized() return Mathf.Abs(LengthSquared - 1) <= Mathf.Epsilon; } + public Quaternion Log() + { + Vector3 v = GetAxis() * GetAngle(); + return new Quaternion(v.x, v.y, v.z, 0); + } + /// /// Returns a copy of the quaternion, normalized to unit length. /// From f72b7a1595f687c72fdf781a97e68b71de7e3f52 Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 11:29:47 +0200 Subject: [PATCH 5/8] C#: Fix `Quaternion.CubicSlerp` --- .../GodotSharp/GodotSharp/Core/Quaternion.cs | 53 ++++++++++++++----- 1 file changed, 40 insertions(+), 13 deletions(-) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index fea6a49b7d6b..7c54571be8fc 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -142,10 +142,43 @@ public real_t AngleTo(Quaternion to) /// The interpolated quaternion. public Quaternion CubicSlerp(Quaternion b, Quaternion preA, Quaternion postB, real_t weight) { - real_t t2 = (1.0f - weight) * weight * 2f; - Quaternion sp = Slerp(b, weight); - Quaternion sq = preA.Slerpni(postB, weight); - return sp.Slerpni(sq, t2); + // Align flip phases. + Quaternion retQ = new Basis(this).GetRotationQuaternion(); + Quaternion preQ = new Basis(preA).GetRotationQuaternion(); + Quaternion toQ = new Basis(b).GetRotationQuaternion(); + Quaternion postQ = new Basis(postB).GetRotationQuaternion(); + + // Flip quaternions to shortest path if necessary. + bool flip1 = Math.Sign(retQ.Dot(preQ)) < 0; + preQ = flip1 ? -preQ : preQ; + bool flip2 = Math.Sign(retQ.Dot(toQ)) < 0; + toQ = flip2 ? -toQ : toQ; + bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0; + postQ = flip3 ? -postQ : postQ; + + if (flip1 || flip2 || flip3) + { + // Angle is too large, calc by Approximate. + retQ.x = Mathf.CubicInterpolate(retQ.x, toQ.x, preQ.x, postQ.x, weight); + retQ.y = Mathf.CubicInterpolate(retQ.y, toQ.y, preQ.y, postQ.y, weight); + retQ.z = Mathf.CubicInterpolate(retQ.z, toQ.z, preQ.z, postQ.z, weight); + retQ.w = Mathf.CubicInterpolate(retQ.w, toQ.w, preQ.w, postQ.w, weight); + retQ = retQ.Normalized(); + } + else + { + // Calc by Expmap. + Quaternion ln_ret = retQ.Log(); + Quaternion ln_to = toQ.Log(); + Quaternion ln_pre = preQ.Log(); + Quaternion ln_post = postQ.Log(); + Quaternion ln = new Quaternion(0, 0, 0, 0); + ln.x = Mathf.CubicInterpolate(ln_ret.x, ln_to.x, ln_pre.x, ln_post.x, weight); + ln.y = Mathf.CubicInterpolate(ln_ret.y, ln_to.y, ln_pre.y, ln_post.y, weight); + ln.z = Mathf.CubicInterpolate(ln_ret.z, ln_to.z, ln_pre.z, ln_post.z, weight); + retQ = ln.Exp(); + } + return retQ; } /// @@ -267,7 +300,7 @@ public Quaternion Slerp(Quaternion to, real_t weight) #endif // Calculate cosine. - real_t cosom = x * to.x + y * to.y + z * to.z + w * to.w; + real_t cosom = Dot(to); var to1 = new Quaternion(); @@ -275,17 +308,11 @@ public Quaternion Slerp(Quaternion to, real_t weight) if (cosom < 0.0) { cosom = -cosom; - to1.x = -to.x; - to1.y = -to.y; - to1.z = -to.z; - to1.w = -to.w; + to1 = -to; } else { - to1.x = to.x; - to1.y = to.y; - to1.z = to.z; - to1.w = to.w; + to1 = to; } real_t sinom, scale0, scale1; From 4c5cefe4fe84f76e841e38b21dc165d16e1e4812 Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 12:03:53 +0200 Subject: [PATCH 6/8] C#: Rename and fix `Quaternion.SphericalCubicInterpolate` --- .../GodotSharp/GodotSharp/Core/Quaternion.cs | 70 +++++++++++-------- 1 file changed, 42 insertions(+), 28 deletions(-) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 7c54571be8fc..8568ecf29983 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -132,7 +132,7 @@ public real_t AngleTo(Quaternion to) } /// - /// Performs a cubic spherical interpolation between quaternions , this quaternion, + /// Performs a spherical cubic interpolation between quaternions , this quaternion, /// , and , by the given amount . /// /// The destination quaternion. @@ -140,45 +140,59 @@ public real_t AngleTo(Quaternion to) /// A quaternion after . /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. /// The interpolated quaternion. - public Quaternion CubicSlerp(Quaternion b, Quaternion preA, Quaternion postB, real_t weight) + public Quaternion SphericalCubicInterpolate(Quaternion b, Quaternion preA, Quaternion postB, real_t weight) { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } + if (!b.IsNormalized()) + { + throw new ArgumentException("Argument is not normalized", nameof(b)); + } +#endif + // Align flip phases. - Quaternion retQ = new Basis(this).GetRotationQuaternion(); + Quaternion fromQ = new Basis(this).GetRotationQuaternion(); Quaternion preQ = new Basis(preA).GetRotationQuaternion(); Quaternion toQ = new Basis(b).GetRotationQuaternion(); Quaternion postQ = new Basis(postB).GetRotationQuaternion(); // Flip quaternions to shortest path if necessary. - bool flip1 = Math.Sign(retQ.Dot(preQ)) < 0; + bool flip1 = Math.Sign(fromQ.Dot(preQ)) < 0; preQ = flip1 ? -preQ : preQ; - bool flip2 = Math.Sign(retQ.Dot(toQ)) < 0; + bool flip2 = Math.Sign(fromQ.Dot(toQ)) < 0; toQ = flip2 ? -toQ : toQ; bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0; postQ = flip3 ? -postQ : postQ; - if (flip1 || flip2 || flip3) - { - // Angle is too large, calc by Approximate. - retQ.x = Mathf.CubicInterpolate(retQ.x, toQ.x, preQ.x, postQ.x, weight); - retQ.y = Mathf.CubicInterpolate(retQ.y, toQ.y, preQ.y, postQ.y, weight); - retQ.z = Mathf.CubicInterpolate(retQ.z, toQ.z, preQ.z, postQ.z, weight); - retQ.w = Mathf.CubicInterpolate(retQ.w, toQ.w, preQ.w, postQ.w, weight); - retQ = retQ.Normalized(); - } - else - { - // Calc by Expmap. - Quaternion ln_ret = retQ.Log(); - Quaternion ln_to = toQ.Log(); - Quaternion ln_pre = preQ.Log(); - Quaternion ln_post = postQ.Log(); - Quaternion ln = new Quaternion(0, 0, 0, 0); - ln.x = Mathf.CubicInterpolate(ln_ret.x, ln_to.x, ln_pre.x, ln_post.x, weight); - ln.y = Mathf.CubicInterpolate(ln_ret.y, ln_to.y, ln_pre.y, ln_post.y, weight); - ln.z = Mathf.CubicInterpolate(ln_ret.z, ln_to.z, ln_pre.z, ln_post.z, weight); - retQ = ln.Exp(); - } - return retQ; + // Calc by Expmap in fromQ space. + Quaternion lnFrom = new Quaternion(0, 0, 0, 0); + Quaternion lnTo = (fromQ.Inverse() * toQ).Log(); + Quaternion lnPre = (fromQ.Inverse() * preQ).Log(); + Quaternion lnPost = (fromQ.Inverse() * postQ).Log(); + Quaternion ln = new Quaternion( + Mathf.CubicInterpolate(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight), + Mathf.CubicInterpolate(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight), + Mathf.CubicInterpolate(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight), + 0); + Quaternion q1 = fromQ * ln.Exp(); + + // Calc by Expmap in toQ space. + lnFrom = (toQ.Inverse() * fromQ).Log(); + lnTo = new Quaternion(0, 0, 0, 0); + lnPre = (toQ.Inverse() * preQ).Log(); + lnPost = (toQ.Inverse() * postQ).Log(); + ln = new Quaternion( + Mathf.CubicInterpolate(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight), + Mathf.CubicInterpolate(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight), + Mathf.CubicInterpolate(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight), + 0); + Quaternion q2 = toQ * ln.Exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.Slerp(q2, weight); } /// From 623f4a52ca7294a5a0271783bceb2442806a21a3 Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Wed, 24 Aug 2022 12:30:14 +0200 Subject: [PATCH 7/8] C#: Add `CubicInterpolateInTime` --- .../glue/GodotSharp/GodotSharp/Core/Mathf.cs | 30 +++++++- .../GodotSharp/GodotSharp/Core/Quaternion.cs | 69 +++++++++++++++++++ .../GodotSharp/GodotSharp/Core/Vector2.cs | 23 +++++++ .../GodotSharp/GodotSharp/Core/Vector3.cs | 24 +++++++ .../GodotSharp/GodotSharp/Core/Vector4.cs | 25 +++++++ 5 files changed, 170 insertions(+), 1 deletion(-) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs index 00e775e6adc1..124410a1c2d0 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs @@ -175,7 +175,8 @@ public static real_t Cosh(real_t s) } /// - /// Cubic interpolates between two values by a normalized value with pre and post values. + /// Cubic interpolates between two values by the factor defined in + /// with pre and post values. /// /// The start value for interpolation. /// The destination value for interpolation. @@ -192,6 +193,33 @@ public static real_t CubicInterpolate(real_t from, real_t to, real_t pre, real_t (-pre + 3.0f * from - 3.0f * to + post) * (weight * weight * weight)); } + /// + /// Cubic interpolates between two values by the factor defined in + /// with pre and post values. + /// It can perform smoother interpolation than + /// by the time values. + /// + /// The start value for interpolation. + /// The destination value for interpolation. + /// The value which before "from" value for interpolation. + /// The value which after "to" value for interpolation. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// + /// + /// + /// The resulting value of the interpolation. + public static real_t CubicInterpolateInTime(real_t from, real_t to, real_t pre, real_t post, real_t weight, real_t toT, real_t preT, real_t postT) + { + /* Barry-Goldman method */ + real_t t = Lerp(0.0f, toT, weight); + real_t a1 = Lerp(pre, from, preT == 0 ? 0.0f : (t - preT) / -preT); + real_t a2 = Lerp(from, to, toT == 0 ? 0.5f : t / toT); + real_t a3 = Lerp(to, post, postT - toT == 0 ? 1.0f : (t - toT) / (postT - toT)); + real_t b1 = Lerp(a1, a2, toT - preT == 0 ? 0.0f : (t - preT) / (toT - preT)); + real_t b2 = Lerp(a2, a3, postT == 0 ? 1.0f : t / postT); + return Lerp(b1, b2, toT == 0 ? 0.5f : t / toT); + } + /// /// Returns the point at the given on a one-dimensional Bezier curve defined by /// the given , and points. diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 8568ecf29983..5cc478ca710a 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -195,6 +195,75 @@ public Quaternion SphericalCubicInterpolate(Quaternion b, Quaternion preA, Quate return q1.Slerp(q2, weight); } + /// + /// Performs a spherical cubic interpolation between quaternions , this quaternion, + /// , and , by the given amount . + /// It can perform smoother interpolation than + /// by the time values. + /// + /// The destination quaternion. + /// A quaternion before this quaternion. + /// A quaternion after . + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// + /// + /// + /// The interpolated quaternion. + public Quaternion SphericalCubicInterpolateInTime(Quaternion b, Quaternion preA, Quaternion postB, real_t weight, real_t bT, real_t preAT, real_t postBT) + { +#if DEBUG + if (!IsNormalized()) + { + throw new InvalidOperationException("Quaternion is not normalized"); + } + if (!b.IsNormalized()) + { + throw new ArgumentException("Argument is not normalized", nameof(b)); + } +#endif + + // Align flip phases. + Quaternion fromQ = new Basis(this).GetRotationQuaternion(); + Quaternion preQ = new Basis(preA).GetRotationQuaternion(); + Quaternion toQ = new Basis(b).GetRotationQuaternion(); + Quaternion postQ = new Basis(postB).GetRotationQuaternion(); + + // Flip quaternions to shortest path if necessary. + bool flip1 = Math.Sign(fromQ.Dot(preQ)) < 0; + preQ = flip1 ? -preQ : preQ; + bool flip2 = Math.Sign(fromQ.Dot(toQ)) < 0; + toQ = flip2 ? -toQ : toQ; + bool flip3 = flip2 ? toQ.Dot(postQ) <= 0 : Math.Sign(toQ.Dot(postQ)) < 0; + postQ = flip3 ? -postQ : postQ; + + // Calc by Expmap in fromQ space. + Quaternion lnFrom = new Quaternion(0, 0, 0, 0); + Quaternion lnTo = (fromQ.Inverse() * toQ).Log(); + Quaternion lnPre = (fromQ.Inverse() * preQ).Log(); + Quaternion lnPost = (fromQ.Inverse() * postQ).Log(); + Quaternion ln = new Quaternion( + Mathf.CubicInterpolateInTime(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight, bT, preAT, postBT), + Mathf.CubicInterpolateInTime(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight, bT, preAT, postBT), + Mathf.CubicInterpolateInTime(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight, bT, preAT, postBT), + 0); + Quaternion q1 = fromQ * ln.Exp(); + + // Calc by Expmap in toQ space. + lnFrom = (toQ.Inverse() * fromQ).Log(); + lnTo = new Quaternion(0, 0, 0, 0); + lnPre = (toQ.Inverse() * preQ).Log(); + lnPost = (toQ.Inverse() * postQ).Log(); + ln = new Quaternion( + Mathf.CubicInterpolateInTime(lnFrom.x, lnTo.x, lnPre.x, lnPost.x, weight, bT, preAT, postBT), + Mathf.CubicInterpolateInTime(lnFrom.y, lnTo.y, lnPre.y, lnPost.y, weight, bT, preAT, postBT), + Mathf.CubicInterpolateInTime(lnFrom.z, lnTo.z, lnPre.z, lnPost.z, weight, bT, preAT, postBT), + 0); + Quaternion q2 = toQ * ln.Exp(); + + // To cancel error made by Expmap ambiguity, do blends. + return q1.Slerp(q2, weight); + } + /// /// Returns the dot product of two quaternions. /// diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs index 03ee12884b32..b2964db8cd69 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs @@ -215,6 +215,29 @@ public Vector2 CubicInterpolate(Vector2 b, Vector2 preA, Vector2 postB, real_t w ); } + /// + /// Performs a cubic interpolation between vectors , this vector, + /// , and , by the given amount . + /// It can perform smoother interpolation than + /// by the time values. + /// + /// The destination vector. + /// A vector before this vector. + /// A vector after . + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// + /// + /// + /// The interpolated vector. + public Vector2 CubicInterpolateInTime(Vector2 b, Vector2 preA, Vector2 postB, real_t weight, real_t t, real_t preAT, real_t postBT) + { + return new Vector2 + ( + Mathf.CubicInterpolateInTime(x, b.x, preA.x, postB.x, weight, t, preAT, postBT), + Mathf.CubicInterpolateInTime(y, b.y, preA.y, postB.y, weight, t, preAT, postBT) + ); + } + /// /// Returns the point at the given on a one-dimensional Bezier curve defined by this vector /// and the given , and points. diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs index cdba06c08920..b53ca5e45a21 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs @@ -208,6 +208,30 @@ public Vector3 CubicInterpolate(Vector3 b, Vector3 preA, Vector3 postB, real_t w ); } + /// + /// Performs a cubic interpolation between vectors , this vector, + /// , and , by the given amount . + /// It can perform smoother interpolation than + /// by the time values. + /// + /// The destination vector. + /// A vector before this vector. + /// A vector after . + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// + /// + /// + /// The interpolated vector. + public Vector3 CubicInterpolateInTime(Vector3 b, Vector3 preA, Vector3 postB, real_t weight, real_t t, real_t preAT, real_t postBT) + { + return new Vector3 + ( + Mathf.CubicInterpolateInTime(x, b.x, preA.x, postB.x, weight, t, preAT, postBT), + Mathf.CubicInterpolateInTime(y, b.y, preA.y, postB.y, weight, t, preAT, postBT), + Mathf.CubicInterpolateInTime(z, b.z, preA.z, postB.z, weight, t, preAT, postBT) + ); + } + /// /// Returns the point at the given on a one-dimensional Bezier curve defined by this vector /// and the given , and points. diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs index 705da0469290..b6f243dfb40f 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs @@ -192,6 +192,31 @@ public Vector4 CubicInterpolate(Vector4 b, Vector4 preA, Vector4 postB, real_t w ); } + /// + /// Performs a cubic interpolation between vectors , this vector, + /// , and , by the given amount . + /// It can perform smoother interpolation than + /// by the time values. + /// + /// The destination vector. + /// A vector before this vector. + /// A vector after . + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// + /// + /// + /// The interpolated vector. + public Vector4 CubicInterpolateInTime(Vector4 b, Vector4 preA, Vector4 postB, real_t weight, real_t t, real_t preAT, real_t postBT) + { + return new Vector4 + ( + Mathf.CubicInterpolateInTime(x, b.x, preA.x, postB.x, weight, t, preAT, postBT), + Mathf.CubicInterpolateInTime(y, b.y, preA.y, postB.y, weight, t, preAT, postBT), + Mathf.CubicInterpolateInTime(y, b.z, preA.z, postB.z, weight, t, preAT, postBT), + Mathf.CubicInterpolateInTime(w, b.w, preA.w, postB.w, weight, t, preAT, postBT) + ); + } + /// /// Returns the normalized vector pointing from this vector to . /// From f64b84551423b02ce2f66092769f3ab3bfbb7740 Mon Sep 17 00:00:00 2001 From: Raul Santos Date: Sat, 27 Aug 2022 10:28:00 +0200 Subject: [PATCH 8/8] C#: Add `CubicInterpolateAngle` --- .../glue/GodotSharp/GodotSharp/Core/Mathf.cs | 60 +++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs index 124410a1c2d0..b30012d21452 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs @@ -193,6 +193,33 @@ public static real_t CubicInterpolate(real_t from, real_t to, real_t pre, real_t (-pre + 3.0f * from - 3.0f * to + post) * (weight * weight * weight)); } + /// + /// Cubic interpolates between two rotation values with shortest path + /// by the factor defined in with pre and post values. + /// See also . + /// + /// The start value for interpolation. + /// The destination value for interpolation. + /// The value which before "from" value for interpolation. + /// The value which after "to" value for interpolation. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// The resulting value of the interpolation. + public static real_t CubicInterpolateAngle(real_t from, real_t to, real_t pre, real_t post, real_t weight) + { + real_t fromRot = from % Mathf.Tau; + + real_t preDiff = (pre - fromRot) % Mathf.Tau; + real_t preRot = fromRot + (2.0f * preDiff) % Mathf.Tau - preDiff; + + real_t toDiff = (to - fromRot) % Mathf.Tau; + real_t toRot = fromRot + (2.0f * toDiff) % Mathf.Tau - toDiff; + + real_t postDiff = (post - toRot) % Mathf.Tau; + real_t postRot = toRot + (2.0f * postDiff) % Mathf.Tau - postDiff; + + return CubicInterpolate(fromRot, toRot, preRot, postRot, weight); + } + /// /// Cubic interpolates between two values by the factor defined in /// with pre and post values. @@ -220,6 +247,39 @@ public static real_t CubicInterpolateInTime(real_t from, real_t to, real_t pre, return Lerp(b1, b2, toT == 0 ? 0.5f : t / toT); } + /// + /// Cubic interpolates between two rotation values with shortest path + /// by the factor defined in with pre and post values. + /// See also . + /// It can perform smoother interpolation than + /// by the time values. + /// + /// The start value for interpolation. + /// The destination value for interpolation. + /// The value which before "from" value for interpolation. + /// The value which after "to" value for interpolation. + /// A value on the range of 0.0 to 1.0, representing the amount of interpolation. + /// + /// + /// + /// The resulting value of the interpolation. + public static real_t CubicInterpolateAngleInTime(real_t from, real_t to, real_t pre, real_t post, real_t weight, + real_t toT, real_t preT, real_t postT) + { + real_t fromRot = from % Mathf.Tau; + + real_t preDiff = (pre - fromRot) % Mathf.Tau; + real_t preRot = fromRot + (2.0f * preDiff) % Mathf.Tau - preDiff; + + real_t toDiff = (to - fromRot) % Mathf.Tau; + real_t toRot = fromRot + (2.0f * toDiff) % Mathf.Tau - toDiff; + + real_t postDiff = (post - toRot) % Mathf.Tau; + real_t postRot = toRot + (2.0f * postDiff) % Mathf.Tau - postDiff; + + return CubicInterpolateInTime(fromRot, toRot, preRot, postRot, weight, toT, preT, postT); + } + /// /// Returns the point at the given on a one-dimensional Bezier curve defined by /// the given , and points.