From d4b24234283a022e4ae2fd124e6b94fc40572f99 Mon Sep 17 00:00:00 2001
From: Kelly Thomas <kelly.thomas@hotmail.com.au>
Date: Sat, 15 Sep 2018 10:26:11 +0800
Subject: [PATCH] [Mono] implement Transform.InterpolateWith()

---
 .gitignore                                   |  3 ++
 modules/mono/glue/Managed/Files/Basis.cs     | 32 ++++++++++++++++++++
 modules/mono/glue/Managed/Files/Transform.cs | 19 ++++++++++++
 3 files changed, 54 insertions(+)

diff --git a/.gitignore b/.gitignore
index 6db75f23249..420a25ac596 100644
--- a/.gitignore
+++ b/.gitignore
@@ -91,6 +91,9 @@ bld/
 *.debug
 *.dSYM
 
+# Visual Studio cache/options directory
+.vs/
+
 # MSTest test Results
 [Tt]est[Rr]esult*/
 [Bb]uild[Ll]og.*
diff --git a/modules/mono/glue/Managed/Files/Basis.cs b/modules/mono/glue/Managed/Files/Basis.cs
index ec96a9e2fac..a5618cb28d8 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 e432d5b52c5..068007d7f00 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();