Merge pull request #107618 from DanielGSilva/quat-arc

Fix `Quaternion(arc_from: Vector3, arc_to: Vector3)` behaves differently in gdscript and c#

modules/mono/glue/GodotSharp/GodotSharp/Core/Basis.cs

@@ -652,12 +652,10 @@ namespace Godot
                 column2 = -column2;
             }
             Vector3 column0 = up.Value.Cross(column2);
-#if DEBUG
             if (column0.IsZeroApprox())
             {
-                throw new ArgumentException("The target vector and up vector can't be parallel to each other.");
+                throw new ArgumentException("Target and up vectors are colinear. This is not advised as it may cause unwanted rotation around local Z axis.");
             }
-#endif
             column0.Normalize();
             Vector3 column1 = column2.Cross(column0);
             return new Basis(column0, column1, column2);

modules/mono/glue/GodotSharp/GodotSharp/Core/Quaternion.cs

@@ -558,18 +558,35 @@ namespace Godot
 
         public Quaternion(Vector3 arcFrom, Vector3 arcTo)
         {
-            Vector3 c = arcFrom.Cross(arcTo);
-            real_t d = arcFrom.Dot(arcTo);
-
-            if (d < -1.0f + Mathf.Epsilon)
+#if DEBUG
+            if (arcFrom.IsZeroApprox() || arcTo.IsZeroApprox())
             {
-                X = 0f;
-                Y = 1f;
-                Z = 0f;
-                W = 0f;
+                throw new ArgumentException("The vectors must not be zero.");
+            }
+#endif
+#if REAL_T_IS_DOUBLE
+            const real_t AlmostOne = 0.999999999999999;
+#else
+            const real_t AlmostOne = 0.99999975f;
+#endif
+            Vector3 n0 = arcFrom.Normalized();
+            Vector3 n1 = arcTo.Normalized();
+            real_t d = n0.Dot(n1);
+            if (Mathf.Abs(d) > AlmostOne)
+            {
+                if (d >= 0.0f)
+                {
+                    return; // Vectors are same.
+                }
+                Vector3 axis = n0.GetAnyPerpendicular();
+                X = axis.X;
+                Y = axis.Y;
+                Z = axis.Z;
+                W = 0.0f;
             }
             else
             {
+                Vector3 c = n0.Cross(n1);
                 real_t s = Mathf.Sqrt((1.0f + d) * 2.0f);
                 real_t rs = 1.0f / s;
 
@@ -578,6 +595,7 @@ namespace Godot
                 Z = c.Z * rs;
                 W = s * 0.5f;
             }
+            this = Normalized();
         }
 
         /// <summary>

modules/mono/glue/GodotSharp/GodotSharp/Core/Vector3.cs

@@ -1283,5 +1283,19 @@ namespace Godot
         {
             return $"({X.ToString(format, CultureInfo.InvariantCulture)}, {Y.ToString(format, CultureInfo.InvariantCulture)}, {Z.ToString(format, CultureInfo.InvariantCulture)})";
         }
+
+        internal readonly Vector3 GetAnyPerpendicular()
+        {
+            // Return the any perpendicular vector by cross product with the Vector3.RIGHT or Vector3.UP,
+            // whichever has the greater angle to the current vector with the sign of each element positive.
+            // The only essence is "to avoid being parallel to the current vector", and there is no mathematical basis for using Vector3.RIGHT and Vector3.UP,
+            // since it could be a different vector depending on the prior branching code Math::abs(x) <= Math::abs(y) && Math::abs(x) <= Math::abs(z).
+            // However, it would be reasonable to use any of the axes of the basis, as it is simpler to calculate.
+            if (IsZeroApprox())
+            {
+                throw new ArgumentException("The Vector3 must not be zero.");
+            }
+            return Cross((Mathf.Abs(X) <= Mathf.Abs(Y) && Mathf.Abs(X) <= Mathf.Abs(Z)) ? new Vector3(1, 0, 0) : new Vector3(0, 1, 0)).Normalized();
+        }
     }
 }