From 82b0899e54a74075fa96a68eff12fc9266c7df81 Mon Sep 17 00:00:00 2001
From: Ignacio Etcheverry <ignalfonsore@gmail.com>
Date: Thu, 23 Jan 2020 15:54:34 +0100
Subject: [PATCH] Mono/C#: Add Basis.Slerp, update Quat.Xform and add some math
 checks

---
 .../glue/GodotSharp/GodotSharp/Core/Basis.cs  | 13 +++++++
 .../glue/GodotSharp/GodotSharp/Core/Quat.cs   | 37 ++++++++++++++++---
 .../GodotSharp/GodotSharp/Core/Vector3.cs     |  6 ++-
 3 files changed, 49 insertions(+), 7 deletions(-)

diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs
index c5e62b77c83..d38589013ec 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs
@@ -387,6 +387,19 @@ namespace Godot
             return b;
         }
 
+        public Basis Slerp(Basis target, real_t t)
+        {
+            var from = new Quat(this);
+            var to = new Quat(target);
+
+            var b = new Basis(from.Slerp(to, t));
+            b.Row0 *= Mathf.Lerp(Row0.Length(), target.Row0.Length(), t);
+            b.Row1 *= Mathf.Lerp(Row1.Length(), target.Row1.Length(), t);
+            b.Row2 *= Mathf.Lerp(Row2.Length(), target.Row2.Length(), t);
+
+            return b;
+        }
+
         public real_t Tdotx(Vector3 with)
         {
             return this.Row0[0] * with[0] + this.Row1[0] * with[1] + this.Row2[0] * with[2];
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs
index 8f60867ac3a..6702634c510 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs
@@ -82,12 +82,20 @@ namespace Godot
 
         public Vector3 GetEuler()
         {
+#if DEBUG
+            if (!IsNormalized())
+                throw new InvalidOperationException("Quat is not normalized");
+#endif
             var basis = new Basis(this);
             return basis.GetEuler();
         }
 
         public Quat Inverse()
         {
+#if DEBUG
+            if (!IsNormalized())
+                throw new InvalidOperationException("Quat is not normalized");
+#endif
             return new Quat(-x, -y, -z, w);
         }
 
@@ -125,6 +133,13 @@ namespace Godot
 
         public Quat Slerp(Quat b, real_t t)
         {
+#if DEBUG
+            if (!IsNormalized())
+                throw new InvalidOperationException("Quat is not normalized");
+            if (!b.IsNormalized())
+                throw new ArgumentException("Argument is not normalized", nameof(b));
+#endif
+
             // Calculate cosine
             real_t cosom = x * b.x + y * b.y + z * b.z + w * b.w;
 
@@ -200,9 +215,13 @@ namespace Godot
 
         public Vector3 Xform(Vector3 v)
         {
-            Quat q = this * v;
-            q *= Inverse();
-            return new Vector3(q.x, q.y, q.z);
+#if DEBUG
+            if (!IsNormalized())
+                throw new InvalidOperationException("Quat is not normalized");
+#endif
+            var u = new Vector3(x, y, z);
+            Vector3 uv = u.Cross(v);
+            return v + ((uv * w) + u.Cross(uv)) * 2;
         }
 
         // Static Readonly Properties
@@ -257,8 +276,12 @@ namespace Godot
 
         public Quat(Vector3 axis, real_t angle)
         {
+#if DEBUG
+            if (!axis.IsNormalized())
+                throw new ArgumentException("Argument is not normalized", nameof(axis));
+#endif
+
             real_t d = axis.Length();
-            real_t angle_t = angle;
 
             if (d == 0f)
             {
@@ -269,12 +292,14 @@ namespace Godot
             }
             else
             {
-                real_t s = Mathf.Sin(angle_t * 0.5f) / d;
+                real_t sinAngle = Mathf.Sin(angle * 0.5f);
+                real_t cosAngle = Mathf.Cos(angle * 0.5f);
+                real_t s = sinAngle / d;
 
                 x = axis.x * s;
                 y = axis.y * s;
                 z = axis.z * s;
-                w = Mathf.Cos(angle_t * 0.5f);
+                w = cosAngle;
             }
         }
 
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs
index 025b09199f8..fded34002df 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs
@@ -255,7 +255,7 @@ namespace Godot
         {
 #if DEBUG
             if (!n.IsNormalized())
-                throw new ArgumentException(String.Format("{0} is not normalized", n), nameof(n));
+                throw new ArgumentException("Argument  is not normalized", nameof(n));
 #endif
             return 2.0f * n * Dot(n) - this;
         }
@@ -296,6 +296,10 @@ namespace Godot
 
         public Vector3 Slerp(Vector3 b, real_t t)
         {
+#if DEBUG
+            if (!IsNormalized())
+                throw new InvalidOperationException("Vector3 is not normalized");
+#endif
             real_t theta = AngleTo(b);
             return Rotated(Cross(b), theta * t);
         }