Fix folder case in src

src/Math/Ray.cs

@@ -0,0 +1,128 @@
+using Godot;
+
+namespace Quadratic.Carto.MathExt;
+
+public struct Ray
+{
+    public Vector3 origin;
+    public Vector3 direction;
+
+    /// <summary>
+    /// Creates a new ray, using origin and <b>normalized</b> direction.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="direction">The direction vector, normalized.</param>
+    public Ray(Vector3 origin, Vector3 direction)
+    {
+        this.origin = origin;
+        this.direction = direction;
+    }
+
+    /// <summary>
+    /// Creates a new ray, using start and end points.
+    /// </summary>
+    /// <param name="start">The start point vector.</param>
+    /// <param name="end">The end point vector.</param>
+    public static Ray FromStartAndEnd(Vector3 start, Vector3 end)
+    {
+        return new Ray(start, (end - start).Normalized());
+    }
+
+    /// <summary>
+    /// Creates a new ray, using origin and non-normalized direction.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="direction">The direction vector, non-normalized.</param>
+    public static Ray FromOriginAndDirection(Vector3 origin, Vector3 direction)
+    {
+        return new Ray(origin, direction.Normalized());
+    }
+
+    /// <summary>
+    /// Creates a new ray, using origin and normalized direction.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="direction">The direction vector, normalized.</param>
+    public static Ray FromOriginAndNormalizedDirection(Vector3 origin, Vector3 direction)
+    {
+        return new Ray(origin, direction);
+    }
+
+    /// <summary>
+    /// Creates a new ray, using origin and rotation quaternion.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="rotation">The rotation quaternion.</param>
+    public static Ray FromOriginAndRotation(Vector3 origin, Quaternion rotation)
+    {
+        return new Ray(origin, rotation * Vector3.Forward);
+    }
+
+    /// <summary>
+    /// Get the point at the given distance along the ray.
+    /// </summary>
+    /// <param name="distance">The distance along the ray.</param>
+    public readonly Vector3 GetPoint(float distance)
+    {
+        return this.origin + this.direction * distance;
+    }
+
+    /// <summary>
+    /// Returns the point along the ray that is closest to the given point.
+    /// </summary>
+    /// <param name="point"></param>
+    /// <returns></returns>
+    public readonly float MinDistancePoint(Vector3 point)
+    {
+        /*
+             The minimum distance between a ray and a point:
+
+                                  d: direction
+             ------+--------------<----* O: origin
+                   |                    \
+                   * P                   * P'
+
+             If the line between origin and target point (OP) is on the same
+             side as d, the distance is the magnitude of the vertical line
+             between P and Od. Else (OP'), the distance is simply the distance 
+             between O and P.
+         */
+
+        var op = point - this.origin; // vector OP
+        var opd = op.Dot(this.direction); // OP dot d is the projection of OP onto d
+        if (opd < 0) // OP' case
+        {
+            return op.Length();
+        }
+        else // OP case
+        {
+            var od = this.direction * opd; // vector Od
+            return (op - od).Length();
+        }
+    }
+
+    public readonly Vector3 ClosestPoint(Vector3 point)
+    {
+        var pointDist = MinDistancePoint(point);
+        return GetPoint(pointDist);
+    }
+
+    public readonly Ray Transform(Transform3D transform)
+    {
+        return new Ray(
+            transform.Origin + transform.Basis * this.origin,
+            (transform.Basis * this.direction).Normalized()
+        );
+    }
+
+    public readonly Ray InverseTransform(Transform3D transform)
+    {
+        var inv = transform.Inverse();
+        return Transform(inv);
+    }
+
+    public override readonly string ToString()
+    {
+        return $"Ray({this.origin}, {this.direction})";
+    }
+}

src/Math/VectorsExt.Conversion.cs

@@ -0,0 +1,48 @@
+namespace Quadratic.Carto.MathExt;
+
+
+public static partial class VectorsExt
+{
+    public static Godot.Vector3 AsGodot(this System.Numerics.Vector3 v)
+    {
+        return new Godot.Vector3(v.X, v.Y, v.Z);
+    }
+
+    public static System.Numerics.Vector3 AsSystem(this Godot.Vector3 v)
+    {
+        return new System.Numerics.Vector3(v.X, v.Y, v.Z);
+    }
+
+    public static Godot.Vector2 AsGodot(this System.Numerics.Vector2 v)
+    {
+        return new Godot.Vector2(v.X, v.Y);
+    }
+
+    public static System.Numerics.Vector2 AsSystem(this Godot.Vector2 v)
+    {
+        return new System.Numerics.Vector2(v.X, v.Y);
+    }
+
+    public static Godot.Quaternion AsGodot(this System.Numerics.Quaternion q)
+    {
+        return new Godot.Quaternion(q.X, q.Y, q.Z, q.W);
+    }
+
+    public static System.Numerics.Quaternion AsSystem(this Godot.Quaternion q)
+    {
+        return new System.Numerics.Quaternion(q.X, q.Y, q.Z, q.W);
+    }
+
+    public static Godot.Vector4 AsGodot(this System.Numerics.Vector4 v)
+    {
+        return new Godot.Vector4(v.X, v.Y, v.Z, v.W);
+    }
+
+    public static System.Numerics.Vector4 AsSystem(this Godot.Vector4 v)
+    {
+        return new System.Numerics.Vector4(v.X, v.Y, v.Z, v.W);
+    }
+}
+
+
+

src/Math/VectorsExt.Math.cs

@@ -0,0 +1,99 @@
+using Godot;
+
+namespace Quadratic.Carto.MathExt;
+
+public static partial class VectorsExt
+{
+    public static Quaternion FromToRotation(Vector3 from, Vector3 to)
+    {
+        var fromDotTo = from.Dot(to);
+        if (Mathf.IsEqualApprox(fromDotTo, 1))
+        {
+            return Quaternion.Identity;
+        }
+        else if (Mathf.IsEqualApprox(fromDotTo, -1))
+        {
+            return new Quaternion(Vector3.Right, Mathf.Pi);
+        }
+        var axis = from.Cross(to).Normalized();
+        var angle = Mathf.Acos(fromDotTo);
+        return new Quaternion(axis, angle);
+    }
+
+    /// <summary>
+    /// Calculates the closest distance between two lines, defined by origin and direction. This
+    /// method assumes normalized direction vectors.
+    /// </summary>
+    /// <param name="aOrigin"></param>
+    /// <param name="aDirection"></param>
+    /// <param name="bOrigin"></param>
+    /// <param name="bDirection"></param>
+    public static float ClosestDistanceBetweenLines(
+        Vector3 aOrigin,
+        Vector3 aDirection,
+        Vector3 bOrigin,
+        Vector3 bDirection)
+    {
+        // Rule out the case where the lines are parallel.
+        float aDotB = aDirection.Dot(bDirection);
+        if (Mathf.IsEqualApprox(aDotB, 1))
+        {
+            // Lines are parallel
+            var aToB = bOrigin - aOrigin;
+            var aPerp = aToB - aDirection * aToB.Dot(aDirection);
+            return aPerp.Length();
+        }
+        else
+        {
+            // The vector that's perpendicular to both lines.
+            // Since both lines are normalized, this vector is also normalized.
+            var normalAB = aDirection.Cross(bDirection);
+            var aToB = bOrigin - aOrigin;
+            return aToB.Dot(normalAB);
+        }
+    }
+
+    /// <summary>
+    /// Calculates the closest distance and points between two lines, defined by origin and
+    /// direction. This method assumes normalized direction vectors.
+    /// </summary>
+    /// <param name="aOrigin"></param>
+    /// <param name="aDirection"></param>
+    /// <param name="bOrigin"></param>
+    /// <param name="bDirection"></param>
+    /// <param name="aClosest"></param>
+    /// <param name="bClosest"></param>
+    public static float ClosestDistanceBetweenLines(
+        Vector3 aOrigin,
+        Vector3 aDirection,
+        Vector3 bOrigin,
+        Vector3 bDirection,
+        out Vector3 aClosest,
+        out Vector3 bClosest)
+    {
+        // Rule out the case where the lines are parallel.
+        float aDotB = aDirection.Dot(bDirection);
+        if (Mathf.IsEqualApprox(aDotB, 1))
+        {
+            // Lines are parallel
+            var aToB = bOrigin - aOrigin;
+            var aPerp = aToB - aDirection * aToB.Dot(aDirection);
+            aClosest = aOrigin;
+            bClosest = bOrigin + aPerp;
+            return aPerp.Length();
+        }
+        else
+        {
+            // https://en.wikipedia.org/wiki/Skew_lines#Nearest_Points
+            // The vector that's perpendicular to both lines.
+            // Since both lines are normalized, this vector is also normalized.
+            var n = aDirection.Cross(bDirection);
+            var n1 = aDirection.Cross(n);
+            var n2 = bDirection.Cross(n);
+            var aToB = bOrigin - aOrigin;
+            aClosest = aOrigin + aToB.Dot(n2) / aDirection.Dot(n2) * aDirection;
+            bClosest = bOrigin + aToB.Dot(n1) / bDirection.Dot(n1) * bDirection;
+            return aToB.Dot(n);
+        }
+    }
+}