[Mono] implement Transform.InterpolateWith()

2 files changed, 51 insertions, 0 deletions

diff --git a/modules/mono/glue/Managed/Files/Basis.cs b/modules/mono/glue/Managed/Files/Basis.cs
index ec96a9e2fa..a5618cb28d 100644
--- a/modules/mono/glue/Managed/Files/Basis.cs
+++ b/modules/mono/glue/Managed/Files/Basis.cs
@@ -165,6 +165,38 @@ namespace Godot
             );
         }
 
+        internal Quat RotationQuat()
+        {
+            Basis orthonormalizedBasis = Orthonormalized();
+            real_t det = orthonormalizedBasis.Determinant();
+            if (det < 0)
+            {
+                // Ensure that the determinant is 1, such that result is a proper rotation matrix which can be represented by Euler angles.
+                orthonormalizedBasis = orthonormalizedBasis.Scaled(Vector3.NegOne);
+            }
+
+            return orthonormalizedBasis.Quat();
+        }
+
+        internal void SetQuantScale(Quat quat, Vector3 scale)
+        {
+            SetDiagonal(scale);
+            Rotate(quat);
+        }
+
+        private void Rotate(Quat quat)
+        {
+            this *= new Basis(quat);
+        }
+
+        private void SetDiagonal(Vector3 diagonal)
+        {
+            _x = new Vector3(diagonal.x, 0, 0);
+            _y = new Vector3(0, diagonal.y, 0);
+            _z = new Vector3(0, 0, diagonal.z);
+
+        }
+
         public real_t Determinant()
         {
             return this[0, 0] * (this[1, 1] * this[2, 2] - this[2, 1] * this[1, 2]) -
diff --git a/modules/mono/glue/Managed/Files/Transform.cs b/modules/mono/glue/Managed/Files/Transform.cs
index e432d5b52c..068007d7f0 100644
--- a/modules/mono/glue/Managed/Files/Transform.cs
+++ b/modules/mono/glue/Managed/Files/Transform.cs
@@ -20,6 +20,25 @@ namespace Godot
             return new Transform(basisInv, basisInv.Xform(-origin));
         }
 
+        public Transform InterpolateWith(Transform transform, real_t c)
+        {
+            /* not sure if very "efficient" but good enough? */
+
+            Vector3 sourceScale = basis.Scale;
+            Quat sourceRotation = basis.RotationQuat();
+            Vector3 sourceLocation = origin;
+
+            Vector3 destinationScale = transform.basis.Scale;
+            Quat destinationRotation = transform.basis.RotationQuat();
+            Vector3 destinationLocation = transform.origin;
+
+            var interpolated = new Transform();
+            interpolated.basis.SetQuantScale(sourceRotation.Slerp(destinationRotation, c).Normalized(), sourceScale.LinearInterpolate(destinationScale, c));
+            interpolated.origin = sourceLocation.LinearInterpolate(destinationLocation, c);
+
+            return interpolated;
+        }
+
         public Transform Inverse()
         {
             Basis basisTr = basis.Transposed();