From 11b60390b6caddf9d26da581473a2448b8465936 Mon Sep 17 00:00:00 2001
From: Wojtek Figat <wojtek@figat.pl>
Date: Wed, 8 Nov 2023 14:41:36 +0100
Subject: [PATCH] Add `GetRotationFromTo` and `FindBetween` utilities to C#
 `Quaternion` API

#1885
---
 Source/Engine/Core/Math/Quaternion.cpp |   3 +-
 Source/Engine/Core/Math/Quaternion.cs  | 109 +++++++++++++++++++++++++
 Source/Shaders/Math.hlsl               |   8 +-
 3 files changed, 113 insertions(+), 7 deletions(-)

diff --git a/Source/Engine/Core/Math/Quaternion.cpp b/Source/Engine/Core/Math/Quaternion.cpp
index bdbe3037d..1ca05a9c3 100644
--- a/Source/Engine/Core/Math/Quaternion.cpp
+++ b/Source/Engine/Core/Math/Quaternion.cpp
@@ -382,9 +382,8 @@ void Quaternion::GetRotationFromTo(const Float3& from, const Float3& to, Quatern
     v0.Normalize();
     v1.Normalize();
 
-    const float d = Float3::Dot(v0, v1);
-
     // If dot == 1, vectors are the same
+    const float d = Float3::Dot(v0, v1);
     if (d >= 1.0f)
     {
         result = Identity;
diff --git a/Source/Engine/Core/Math/Quaternion.cs b/Source/Engine/Core/Math/Quaternion.cs
index ff50a98bf..34ee160f0 100644
--- a/Source/Engine/Core/Math/Quaternion.cs
+++ b/Source/Engine/Core/Math/Quaternion.cs
@@ -1077,6 +1077,115 @@ namespace FlaxEngine
             }
         }
 
+        /// <summary>
+        /// Gets the shortest arc quaternion to rotate this vector to the destination vector.
+        /// </summary>
+        /// <param name="from">The source vector.</param>
+        /// <param name="to">The destination vector.</param>
+        /// <param name="result">The result.</param>
+        /// <param name="fallbackAxis">The fallback axis.</param>
+        public static void GetRotationFromTo(ref Float3 from, ref Float3 to, out Quaternion result, ref Float3 fallbackAxis)
+        {
+            // Based on Stan Melax's article in Game Programming Gems
+
+            Float3 v0 = from;
+            Float3 v1 = to;
+            v0.Normalize();
+            v1.Normalize();
+
+            // If dot == 1, vectors are the same
+            float d = Float3.Dot(ref v0, ref v1);
+            if (d >= 1.0f)
+            {
+                result = Identity;
+                return;
+            }
+
+            if (d < 1e-6f - 1.0f)
+            {
+                if (fallbackAxis != Float3.Zero)
+                {
+                    // Rotate 180 degrees about the fallback axis
+                    RotationAxis(ref fallbackAxis, Mathf.Pi, out result);
+                }
+                else
+                {
+                    // Generate an axis
+                    Float3 axis = Float3.Cross(Float3.UnitX, from);
+                    if (axis.LengthSquared < Mathf.Epsilon) // Pick another if colinear
+                        axis = Float3.Cross(Float3.UnitY, from);
+                    axis.Normalize();
+                    RotationAxis(ref axis, Mathf.Pi, out result);
+                }
+            }
+            else
+            {
+                float s = Mathf.Sqrt((1 + d) * 2);
+                float invS = 1 / s;
+                Float3.Cross(ref v0, ref v1, out var c);
+                result.X = c.X * invS;
+                result.Y = c.Y * invS;
+                result.Z = c.Z * invS;
+                result.W = s * 0.5f;
+                result.Normalize();
+            }
+        }
+
+        /// <summary>
+        /// Gets the shortest arc quaternion to rotate this vector to the destination vector.
+        /// </summary>
+        /// <param name="from">The source vector.</param>
+        /// <param name="to">The destination vector.</param>
+        /// <param name="fallbackAxis">The fallback axis.</param>
+        /// <returns>The rotation.</returns>
+        public static Quaternion GetRotationFromTo(Float3 from, Float3 to, Float3 fallbackAxis)
+        {
+            GetRotationFromTo(ref from, ref to, out var result, ref fallbackAxis);
+            return result;
+        }
+
+        /// <summary>
+        /// Gets the quaternion that will rotate vector from into vector to, around their plan perpendicular axis.The input vectors don't need to be normalized.
+        /// </summary>
+        /// <param name="from">The source vector.</param>
+        /// <param name="to">The destination vector.</param>
+        /// <param name="result">The result.</param>
+        public static void FindBetween(ref Float3 from, ref Float3 to, out Quaternion result)
+        {
+            // http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
+            float normFromNormTo = Mathf.Sqrt(from.LengthSquared * to.LengthSquared);
+            if (normFromNormTo < Mathf.Epsilon)
+            {
+                result = Identity;
+                return;
+            }
+            float w = normFromNormTo + Float3.Dot(from, to);
+            if (w < 1.0-6f * normFromNormTo)
+            {
+                result = Mathf.Abs(from.X) > Mathf.Abs(from.Z)
+                         ? new Quaternion(-from.Y, from.X, 0.0f, 0.0f)
+                         : new Quaternion(0.0f, -from.Z, from.Y, 0.0f);
+            }
+            else
+            {
+                Float3 cross = Float3.Cross(from, to);
+                result = new Quaternion(cross.X, cross.Y, cross.Z, w);
+            }
+            result.Normalize();
+        }
+
+        /// <summary>
+        /// Gets the quaternion that will rotate vector from into vector to, around their plan perpendicular axis.The input vectors don't need to be normalized.
+        /// </summary>
+        /// <param name="from">The source vector.</param>
+        /// <param name="to">The destination vector.</param>
+        /// <returns>The rotation.</returns>
+        public static Quaternion FindBetween(Float3 from, Float3 to)
+        {
+            FindBetween(ref from, ref to, out var result);
+            return result;
+        }
+
         /// <summary>
         /// Creates a left-handed spherical billboard that rotates around a specified object position.
         /// </summary>
diff --git a/Source/Shaders/Math.hlsl b/Source/Shaders/Math.hlsl
index ad3168016..67ddc1887 100644
--- a/Source/Shaders/Math.hlsl
+++ b/Source/Shaders/Math.hlsl
@@ -218,10 +218,10 @@ float4 ClampedPow(float4 x, float4 y)
 
 float4 FindQuatBetween(float3 from, float3 to)
 {
+    // http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
     float normAB = 1.0f;
     float w = normAB + dot(from, to);
     float4 result;
-
     if (w >= 1e-6f * normAB)
     {
         result = float4
@@ -234,12 +234,10 @@ float4 FindQuatBetween(float3 from, float3 to)
     }
     else
     {
-        w = 0.f;
         result = abs(from.x) > abs(from.y)
-                     ? float4(-from.z, 0.f, from.x, w)
-                     : float4(0.f, -from.z, from.y, w);
+                     ? float4(-from.z, 0.f, from.x, 0.0f)
+                     : float4(0.f, -from.z, from.y, 0.0f);
     }
-
     return normalize(result);
 }