From 85a0345d57a9ed84bd79f0dbcd2a3851d6435c06 Mon Sep 17 00:00:00 2001
From: Aaron Franke <arnfranke@yahoo.com>
Date: Mon, 7 Dec 2020 18:42:28 -0500
Subject: [PATCH] [3.x] Add Quat angle_to method

---
 core/math/quat.cpp                              |  5 +++++
 core/math/quat.h                                |  1 +
 core/variant_call.cpp                           |  2 ++
 doc/classes/Quat.xml                            | 10 ++++++++++
 .../glue/GodotSharp/GodotSharp/Core/Quat.cs     | 17 +++++++++++++++++
 5 files changed, 35 insertions(+)

diff --git a/core/math/quat.cpp b/core/math/quat.cpp
index d67dd2b3e68..fae2e2b78c5 100644
--- a/core/math/quat.cpp
+++ b/core/math/quat.cpp
@@ -33,6 +33,11 @@
 #include "core/math/basis.h"
 #include "core/print_string.h"
 
+real_t Quat::angle_to(const Quat &p_to) const {
+	real_t d = dot(p_to);
+	return Math::acos(CLAMP(d * d * 2 - 1, -1, 1));
+}
+
 // set_euler_xyz expects a vector containing the Euler angles in the format
 // (ax,ay,az), where ax is the angle of rotation around x axis,
 // and similar for other axes.
diff --git a/core/math/quat.h b/core/math/quat.h
index ad0b95c75d1..eb68de1dbf3 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -48,6 +48,7 @@ public:
 	bool is_normalized() const;
 	Quat inverse() const;
 	_FORCE_INLINE_ real_t dot(const Quat &p_q) const;
+	real_t angle_to(const Quat &p_to) const;
 
 	void set_euler_xyz(const Vector3 &p_euler);
 	Vector3 get_euler_xyz() const;
diff --git a/core/variant_call.cpp b/core/variant_call.cpp
index dd0d110a8ae..0064507ca1f 100644
--- a/core/variant_call.cpp
+++ b/core/variant_call.cpp
@@ -497,6 +497,7 @@ struct _VariantCall {
 	VCALL_LOCALMEM0R(Quat, is_normalized);
 	VCALL_LOCALMEM1R(Quat, is_equal_approx);
 	VCALL_LOCALMEM0R(Quat, inverse);
+	VCALL_LOCALMEM1R(Quat, angle_to);
 	VCALL_LOCALMEM1R(Quat, dot);
 	VCALL_LOCALMEM1R(Quat, xform);
 	VCALL_LOCALMEM2R(Quat, slerp);
@@ -1816,6 +1817,7 @@ void register_variant_methods() {
 	ADDFUNC0R(QUAT, BOOL, Quat, is_normalized, varray());
 	ADDFUNC1R(QUAT, BOOL, Quat, is_equal_approx, QUAT, "quat", varray());
 	ADDFUNC0R(QUAT, QUAT, Quat, inverse, varray());
+	ADDFUNC1R(QUAT, REAL, Quat, angle_to, QUAT, "to", varray());
 	ADDFUNC1R(QUAT, REAL, Quat, dot, QUAT, "b", varray());
 	ADDFUNC1R(QUAT, VECTOR3, Quat, xform, VECTOR3, "v", varray());
 	ADDFUNC2R(QUAT, QUAT, Quat, slerp, QUAT, "to", REAL, "weight", varray());
diff --git a/doc/classes/Quat.xml b/doc/classes/Quat.xml
index 4b4e460b5db..e396d5d474a 100644
--- a/doc/classes/Quat.xml
+++ b/doc/classes/Quat.xml
@@ -57,6 +57,16 @@
 				Constructs a quaternion defined by the given values.
 			</description>
 		</method>
+		<method name="angle_to">
+			<return type="float">
+			</return>
+			<argument index="0" name="to" type="Quat">
+			</argument>
+			<description>
+				Returns the angle between this quaternion and [code]to[/code]. This is the magnitude of the angle you would need to rotate by to get from one to the other.
+				[b]Note:[/b] This method has an abnormally high amount of floating-point error, so methods such as [method @GDScript.is_zero_approx] will not work reliably.
+			</description>
+		</method>
 		<method name="cubic_slerp">
 			<return type="Quat">
 			</return>
diff --git a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs
index 91c27c39f47..11fee88cad2 100644
--- a/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs
+++ b/modules/mono/glue/GodotSharp/GodotSharp/Core/Quat.cs
@@ -113,6 +113,23 @@ namespace Godot
             get { return Dot(this); }
         }
 
+        /// <summary>
+        /// Returns the angle between this quaternion and `to`.
+        /// This is the magnitude of the angle you would need to rotate
+        /// by to get from one to the other.
+        ///
+        /// Note: This method has an abnormally high amount
+        /// of floating-point error, so methods such as
+        /// <see cref="Mathf.IsZeroApprox"/> will not work reliably.
+        /// </summary>
+        /// <param name="to">The other quaternion.</param>
+        /// <returns>The angle between the quaternions.</returns>
+        public real_t AngleTo(Quat to)
+        {
+            real_t dot = Dot(to);
+            return Mathf.Acos(Mathf.Clamp(dot * dot * 2 - 1, -1, 1));
+        }
+
         /// <summary>
         /// Performs a cubic spherical interpolation between quaternions `preA`,
         /// this vector, `b`, and `postB`, by the given amount `t`.