Sync C# cubic interpolation with core

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;
+        }
+
         /// <summary>
         /// Returns the orthonormalized version of the basis matrix (useful to
         /// call occasionally to avoid rounding errors for orthogonal matrices).

modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs

@@ -175,7 +175,8 @@ public static real_t Cosh(real_t s)
         }
 
         /// <summary>
-        /// Cubic interpolates between two values by a normalized value with pre and post values.
+        /// Cubic interpolates between two values by the factor defined in <paramref name="weight"/>
+        /// with pre and post values.
         /// </summary>
         /// <param name="from">The start value for interpolation.</param>
         /// <param name="to">The destination value for interpolation.</param>
@@ -192,6 +193,93 @@ 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));
         }
 
+        /// <summary>
+        /// Cubic interpolates between two rotation values with shortest path
+        /// by the factor defined in <paramref name="weight"/> with pre and post values.
+        /// See also <see cref="LerpAngle"/>.
+        /// </summary>
+        /// <param name="from">The start value for interpolation.</param>
+        /// <param name="to">The destination value for interpolation.</param>
+        /// <param name="pre">The value which before "from" value for interpolation.</param>
+        /// <param name="post">The value which after "to" value for interpolation.</param>
+        /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
+        /// <returns>The resulting value of the interpolation.</returns>
+        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);
+        }
+
+        /// <summary>
+        /// Cubic interpolates between two values by the factor defined in <paramref name="weight"/>
+        /// with pre and post values.
+        /// It can perform smoother interpolation than <see cref="CubicInterpolate"/>
+        /// by the time values.
+        /// </summary>
+        /// <param name="from">The start value for interpolation.</param>
+        /// <param name="to">The destination value for interpolation.</param>
+        /// <param name="pre">The value which before "from" value for interpolation.</param>
+        /// <param name="post">The value which after "to" value for interpolation.</param>
+        /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
+        /// <param name="toT"></param>
+        /// <param name="preT"></param>
+        /// <param name="postT"></param>
+        /// <returns>The resulting value of the interpolation.</returns>
+        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);
+        }
+
+        /// <summary>
+        /// Cubic interpolates between two rotation values with shortest path
+        /// by the factor defined in <paramref name="weight"/> with pre and post values.
+        /// See also <see cref="LerpAngle"/>.
+        /// It can perform smoother interpolation than <see cref="CubicInterpolateAngle"/>
+        /// by the time values.
+        /// </summary>
+        /// <param name="from">The start value for interpolation.</param>
+        /// <param name="to">The destination value for interpolation.</param>
+        /// <param name="pre">The value which before "from" value for interpolation.</param>
+        /// <param name="post">The value which after "to" value for interpolation.</param>
+        /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
+        /// <param name="toT"></param>
+        /// <param name="preT"></param>
+        /// <param name="postT"></param>
+        /// <returns>The resulting value of the interpolation.</returns>
+        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);
+        }
+
         /// <summary>
         /// Returns the point at the given <paramref name="t"/> on a one-dimensional Bezier curve defined by
         /// the given <paramref name="control1"/>, <paramref name="control2"/> and <paramref name="end"/> points.

modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs

@@ -132,20 +132,136 @@ public real_t AngleTo(Quaternion to)
         }
 
         /// <summary>
-        /// Performs a cubic spherical interpolation between quaternions <paramref name="preA"/>, this quaternion,
+        /// Performs a spherical cubic interpolation between quaternions <paramref name="preA"/>, this quaternion,
         /// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
         /// </summary>
         /// <param name="b">The destination quaternion.</param>
         /// <param name="preA">A quaternion before this quaternion.</param>
         /// <param name="postB">A quaternion after <paramref name="b"/>.</param>
         /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
         /// <returns>The interpolated quaternion.</returns>
-        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);
+        }
+
+        /// <summary>
+        /// Performs a spherical cubic interpolation between quaternions <paramref name="preA"/>, this quaternion,
+        /// <paramref name="b"/>, and <paramref name="postB"/>, by the given amount <paramref name="weight"/>.
+        /// It can perform smoother interpolation than <see cref="SphericalCubicInterpolate"/>
+        /// by the time values.
+        /// </summary>
+        /// <param name="b">The destination quaternion.</param>
+        /// <param name="preA">A quaternion before this quaternion.</param>
+        /// <param name="postB">A quaternion after <paramref name="b"/>.</param>
+        /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
+        /// <param name="bT"></param>
+        /// <param name="preAT"></param>
+        /// <param name="postBT"></param>
+        /// <returns>The interpolated quaternion.</returns>
+        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);
         }
 
         /// <summary>
@@ -158,6 +274,34 @@ 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);
+        }
+
+        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);
+        }
+
         /// <summary>
         /// Returns Euler angles (in the YXZ convention: when decomposing,
         /// first Z, then X, and Y last) corresponding to the rotation
@@ -201,6 +345,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);
+        }
+
         /// <summary>
         /// Returns a copy of the quaternion, normalized to unit length.
         /// </summary>
@@ -233,25 +383,19 @@ 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();
 
             // Adjust signs if necessary.
             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 @@ public Quaternion Slerp(Quaternion to, real_t weight)
         /// <returns>The resulting quaternion of the interpolation.</returns>
         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)