From a94168459004e93a9c9431d29bac04fec3850631 Mon Sep 17 00:00:00 2001
From: Kelly Thomas <kelly.thomas@hotmail.com.au>
Date: Sun, 19 Aug 2018 21:27:15 +0800
Subject: mono: Quat - add some missing constructors and methods

---
 core/math/quat.cpp                  |  2 +-
 modules/mono/glue/cs_files/Basis.cs |  2 +-
 modules/mono/glue/cs_files/Quat.cs  | 97 ++++++++++++++++++++++++++-----------
 3 files changed, 71 insertions(+), 30 deletions(-)

diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index 67c9048a41..2251571146 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -134,7 +134,7 @@ Quat Quat::normalized() const {
 }
 
 bool Quat::is_normalized() const {
-	return Math::is_equal_approx(length(), 1.0);
+	return Math::is_equal_approx(length_squared(), 1.0);
 }
 
 Quat Quat::inverse() const {
diff --git a/modules/mono/glue/cs_files/Basis.cs b/modules/mono/glue/cs_files/Basis.cs
index c280d32c61..10286f3832 100644
--- a/modules/mono/glue/cs_files/Basis.cs
+++ b/modules/mono/glue/cs_files/Basis.cs
@@ -426,7 +426,7 @@ namespace Godot
 
         public Basis(Quat quat)
         {
-            real_t s = 2.0f / quat.LengthSquared();
+            real_t s = 2.0f / quat.LengthSquared;
 
             real_t xs = quat.x * s;
             real_t ys = quat.y * s;
diff --git a/modules/mono/glue/cs_files/Quat.cs b/modules/mono/glue/cs_files/Quat.cs
index c69c55d997..eaa027eb69 100644
--- a/modules/mono/glue/cs_files/Quat.cs
+++ b/modules/mono/glue/cs_files/Quat.cs
@@ -11,18 +11,11 @@ namespace Godot
     [StructLayout(LayoutKind.Sequential)]
     public struct Quat : IEquatable<Quat>
     {
-        private static readonly Quat identity = new Quat(0f, 0f, 0f, 1f);
-
         public real_t x;
         public real_t y;
         public real_t z;
         public real_t w;
 
-        public static Quat Identity
-        {
-            get { return identity; }
-        }
-
         public real_t this[int index]
         {
             get
@@ -63,6 +56,16 @@ namespace Godot
             }
         }
 
+        public real_t Length
+        {
+            get { return Mathf.Sqrt(LengthSquared); }
+        }
+
+        public real_t LengthSquared
+        {
+            get { return Dot(this); }
+        }
+
         public Quat CubicSlerp(Quat b, Quat preA, Quat postB, real_t t)
         {
             real_t t2 = (1.0f - t) * t * 2f;
@@ -76,24 +79,20 @@ namespace Godot
             return x * b.x + y * b.y + z * b.z + w * b.w;
         }
 
-        public Quat Inverse()
-        {
-            return new Quat(-x, -y, -z, w);
-        }
-
-        public real_t Length()
+        public Vector3 GetEuler()
         {
-            return Mathf.Sqrt(LengthSquared());
+            var basis = new Basis(this);
+            return basis.GetEuler();
         }
 
-        public real_t LengthSquared()
+        public Quat Inverse()
         {
-            return Dot(this);
+            return new Quat(-x, -y, -z, w);
         }
 
         public Quat Normalized()
         {
-            return this / Length();
+            return this / Length;
         }
 
         public void Set(real_t x, real_t y, real_t z, real_t w)
@@ -103,12 +102,20 @@ namespace Godot
             this.z = z;
             this.w = w;
         }
+
         public void Set(Quat q)
         {
-            x = q.x;
-            y = q.y;
-            z = q.z;
-            w = q.w;
+            this = q;
+        }
+
+        public void SetAxisAngle(Vector3 axis, real_t angle)
+        {
+            this = new Quat(axis, angle);
+        }
+
+        public void SetEuler(Vector3 eulerYXZ)
+        {
+            this = new Quat(eulerYXZ);
         }
 
         public Quat Slerp(Quat b, real_t t)
@@ -192,6 +199,9 @@ namespace Godot
             return new Vector3(q.x, q.y, q.z);
         }
 
+        // Static Readonly Properties
+        public static Quat Identity { get; } = new Quat(0f, 0f, 0f, 1f);
+
         // Constructors 
         public Quat(real_t x, real_t y, real_t z, real_t w)
         {
@@ -199,15 +209,46 @@ namespace Godot
             this.y = y;
             this.z = z;
             this.w = w;
-        }   
+        }
+
+        public bool IsNormalized()
+        {
+            return Mathf.Abs(LengthSquared - 1) <= Mathf.Epsilon;
+        }
+
         public Quat(Quat q)
-        {                     
-            x = q.x;
-            y = q.y;
-            z = q.z;
-            w = q.w;
+        {
+            this = q;
+        }
+
+        public Quat(Basis basis)
+        {
+            this = basis.Quat();
         }
-        
+
+        public Quat(Vector3 eulerYXZ)
+        {
+            real_t half_a1 = eulerYXZ.y * (real_t)0.5;
+            real_t half_a2 = eulerYXZ.x * (real_t)0.5;
+            real_t half_a3 = eulerYXZ.z * (real_t)0.5;
+
+            // R = Y(a1).X(a2).Z(a3) convention for Euler angles.
+            // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6)
+            // a3 is the angle of the first rotation, following the notation in this reference.
+
+            real_t cos_a1 = Mathf.Cos(half_a1);
+            real_t sin_a1 = Mathf.Sin(half_a1);
+            real_t cos_a2 = Mathf.Cos(half_a2);
+            real_t sin_a2 = Mathf.Sin(half_a2);
+            real_t cos_a3 = Mathf.Cos(half_a3);
+            real_t sin_a3 = Mathf.Sin(half_a3);
+
+            x = sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3;
+            y = sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3;
+            z = -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3;
+            w = sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3;
+        }
+
         public Quat(Vector3 axis, real_t angle)
         {
             real_t d = axis.Length();
-- 
cgit v1.2.3