diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs index 87adf9efe5d..ed20067a922 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs @@ -498,6 +498,15 @@ namespace Godot ); } + 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/Mathf.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs index 00e775e6adc..b30012d2145 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs @@ -175,7 +175,8 @@ namespace Godot } /// - /// 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,93 @@ namespace Godot (-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. + /// 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); + } + + /// + /// 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. diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs index 999500ca13c..5cc478ca710 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs @@ -132,7 +132,7 @@ namespace Godot } /// - /// 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,12 +140,128 @@ namespace Godot /// 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) { - real_t t2 = (1.0f - weight) * weight * 2f; - Quaternion sp = Slerp(b, weight); - Quaternion sq = preA.Slerpni(postB, weight); - return sp.Slerpni(sq, t2); +#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.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); + } + + /// + /// 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); } /// @@ -158,6 +274,34 @@ namespace Godot 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); + } + + 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 @@ -201,6 +345,12 @@ namespace Godot 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. /// @@ -233,7 +383,7 @@ namespace Godot #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(); @@ -241,17 +391,11 @@ namespace Godot 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; @@ -292,6 +436,17 @@ namespace Godot /// 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) diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs index 4b739bb86bc..3c017ecc9f2 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Transform3D.cs @@ -119,23 +119,9 @@ namespace Godot /// 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 @@ namespace Godot 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 diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs index 03ee12884b3..b2964db8cd6 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector2.cs @@ -215,6 +215,29 @@ namespace Godot ); } + /// + /// 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 cdba06c0892..b53ca5e45a2 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs @@ -208,6 +208,30 @@ namespace Godot ); } + /// + /// 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 705da046929..b6f243dfb40 100644 --- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs +++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector4.cs @@ -192,6 +192,31 @@ namespace Godot ); } + /// + /// 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 . ///