From 961a086d208ce3082a2ddb43894ffe413b3ff5fe Mon Sep 17 00:00:00 2001
From: Raul Santos <raulsntos@gmail.com>
Date: Wed, 24 Aug 2022 11:46:38 +0200
Subject: C#: Fix `Transform3D` interpolation and add spherical interpolation

---
 .../mono/glue/GodotSharp/GodotSharp/Core/Basis.cs  |  9 ++++
 .../glue/GodotSharp/GodotSharp/Core/Transform3D.cs | 48 ++++++++++++++--------
 2 files changed, 40 insertions(+), 17 deletions(-)

(limited to 'modules/mono')

diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs
index 87adf9efe5..ed20067a92 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;
+        }
+
         /// <summary>
         /// 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 4b739bb86b..3c017ecc9f 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
         /// <returns>The interpolated transform.</returns>
         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);
         }
 
         /// <summary>
@@ -234,6 +220,34 @@ namespace Godot
             return new Transform3D(basis * tmpBasis, origin);
         }
 
+        /// <summary>
+        /// Returns a transform spherically interpolated between this transform and
+        /// another <paramref name="transform"/> by <paramref name="weight"/>.
+        /// </summary>
+        /// <param name="transform">The other transform.</param>
+        /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param>
+        /// <returns>The interpolated transform.</returns>
+        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
-- 
cgit v1.2.3