Add geodetic distance

Author: aalmada

src/NetFabric.Numerics.Geography/Geodetic2/Point.cs

@@ -109,4 +109,159 @@ object IPoint<Point<TDatum, TAngleUnits, TAngle>>.this[int index]
             1 => Longitude,
             _ => Throw.ArgumentOutOfRangeException<object>(nameof(index), index, "index out of range")
         };
+}
+
+/// <summary>
+/// Provides static methods for point operations.
+/// </summary>
+public static class Point
+{
+    /// <summary>
+    /// Calculates the distance between two geodetic points.
+    /// </summary>
+    /// <param name="from">The first geodetic point.</param>
+    /// <param name="to">The second geodetic point.</param>
+    /// <returns>The distance between the two geodetic points, in meters.</returns>
+    /// <remarks>
+    /// <para>
+    /// This method calculates the distance between two geodesic points on the Earth's surface using the spherical formula.
+    /// The geodesic points are specified by their latitude and longitude coordinates in degrees.
+    /// </para>
+    /// <para>
+    /// The distance is calculated by treating the Earth as a perfect sphere, which is a simplification and may introduce
+    /// some degree of error for large distances or near the poles. The result is returned in kilometers.
+    /// </para>
+    /// <para>
+    /// Note: The result of this method represents the shortest distance between the two points along the surface of the
+    /// sphere, also known as the great-circle distance.
+    /// </para>
+    /// </remarks>
+    public static TAngle DistanceSpherical<TDatum, TAngle>(Point<TDatum, Radians, TAngle> from, Point<TDatum, Radians, TAngle> to)
+        where TDatum : IDatum<TDatum>
+        where TAngle : struct, IFloatingPointIeee754<TAngle>, IMinMaxValue<TAngle>
+    {
+        var half = TAngle.CreateChecked(0.5);
+        var halfLatitudeDifference = half * (to.Latitude - from.Latitude);
+        var halfLongitudeDifference = half * (to.Latitude - from.Latitude);
+        var a = (Angle.Sin(halfLatitudeDifference) * Angle.Sin(halfLatitudeDifference)) +
+                   (Angle.Cos(from.Latitude) * Angle.Cos(to.Latitude) *
+                   Angle.Sin(halfLongitudeDifference) * Angle.Sin(halfLongitudeDifference));
+        var c = TAngle.CreateChecked(2) * Angle.Atan2(TAngle.Sqrt(a), TAngle.Sqrt(TAngle.One - a));
+
+        return TAngle.CreateChecked(MedianRadius(TDatum.Ellipsoid)) * c.Value;
+
+        static double MedianRadius(Ellipsoid ellipsoid)
+        {
+            var semiMajorAxis = ellipsoid.EquatorialRadius;
+            var flatteningInverse = 1.0 / ellipsoid.Flattening;
+            var semiMinorAxis = semiMajorAxis * (1 - flatteningInverse);
+
+            return double.Sqrt(semiMajorAxis * semiMinorAxis);
+        }
+    }
+
+    /// <summary>
+    /// Calculates the distance between two geodetic points.
+    /// </summary>
+    /// <param name="from">The first geodetic point.</param>
+    /// <param name="to">The second geodetic point.</param>
+    /// <returns>The distance between the two geodetic points, in meters.</returns>
+    /// <exception cref="InvalidOperationException">The iteration did not converge.</exception>"
+    /// <remarks>
+    /// <para>
+    /// This method calculates the distance between two geodetic points on the Earth's surface using the datum equatorial radius
+    /// and flattening. The geodetic points are defined by their latitude and longitude coordinates. The calculation assumes the Earth
+    /// is an ellipsoid, and the provided equatorial radius and flattening define its shape. The resulting distance is returned in meters.
+    /// </para>
+    /// <para>
+    /// The algorithm performs an iterative procedure to converge to the accurate distance calculation. In rare cases where the
+    /// iteration does not converge within the defined limit, an <see cref="InvalidOperationException"/> is thrown.
+    /// </para>
+    /// </remarks>
+    public static TAngle DistanceEllipsoid<TDatum, TAngle>(Point<TDatum, Radians, TAngle> from, Point<TDatum, Radians, TAngle> to)
+        where TDatum : IDatum<TDatum>
+        where TAngle : struct, IFloatingPointIeee754<TAngle>, IMinMaxValue<TAngle>
+    {
+        var latitudeDifference = to.Latitude - from.Latitude;
+        var longitudeDifference = to.Longitude - from.Longitude;
+
+        var half = TAngle.CreateChecked(0.5);
+        var halfLatitudeDifference = half * latitudeDifference;
+        var halfLongitudeDifference = half * longitudeDifference;
+        var a = (Angle.Sin(halfLatitudeDifference) * Angle.Sin(halfLatitudeDifference)) +
+                   (Angle.Cos(from.Latitude) * Angle.Cos(to.Latitude) *
+                   Angle.Sin(halfLongitudeDifference) * Angle.Sin(halfLongitudeDifference));
+        var c = TAngle.CreateChecked(2) * Angle.Atan2(TAngle.Sqrt(a), TAngle.Sqrt(TAngle.One - a));
+
+        var semiMajorAxis = TDatum.Ellipsoid.EquatorialRadius;
+        var flatteningInverse = 1.0 / TDatum.Ellipsoid.Flattening;
+        var semiMinorAxis = semiMajorAxis * (1 - flatteningInverse);
+
+        var uSquared = TAngle.CreateChecked(((semiMajorAxis * semiMajorAxis) - (semiMinorAxis * semiMinorAxis)) / (semiMinorAxis * semiMinorAxis));
+
+        var sinU1 = Angle.Sin(from.Latitude);
+        var cosU1 = Angle.Cos(from.Latitude);
+        var sinU2 = Angle.Sin(to.Latitude);
+        var cosU2 = Angle.Cos(to.Latitude);
+
+        var lambda = longitudeDifference;
+
+        var iterationLimit = 100;
+        var cosLambda = TAngle.Zero;
+        var sinLambda = TAngle.Zero;
+        Angle<Radians, TAngle> sigma;
+        TAngle cosSigma, sinSigma, cos2SigmaM, sinSigmaPrev;
+        TAngle sigmaP = TAngle.Zero;
+
+        do
+        {
+            sinLambda = Angle.Sin(lambda);
+            cosLambda = Angle.Cos(lambda);
+            sinSigma = TAngle.Sqrt((cosU2 * sinLambda * (cosU2 * sinLambda)) +
+                (((cosU1 * sinU2) - (sinU1 * cosU2 * cosLambda)) * ((cosU1 * sinU2) - (sinU1 * cosU2 * cosLambda))));
+
+            if (sinSigma == TAngle.Zero)
+                return TAngle.Zero; // Coincident points
+
+            cosSigma = (sinU1 * sinU2) + cosU1 * cosU2 * cosLambda;
+            sigma = Angle.Atan2(sinSigma, cosSigma);
+            sinSigmaPrev = sinSigma;
+
+            cos2SigmaM = cosSigma - (TAngle.CreateChecked(2) * sinU1 * sinU2 / ((cosU1 * cosU2) + (sinU1 * sinU2)));
+
+            var cSquared = uSquared * cosSigma * cosSigma;
+            var lambdaP = lambda;
+            lambda = longitudeDifference + ((1 - cSquared) * uSquared * sinSigma *
+                (sigma + (uSquared * sinSigmaPrev * (cos2SigmaM +
+                (uSquared * cosSigma * (-1 + (2 * cos2SigmaM * cos2SigmaM)))))));
+        }
+        while (TAngle.Abs((lambda - lambdaP) / lambda) > 1e-12 && --iterationLimit > 0);
+
+        if (iterationLimit == 0)
+            throw new InvalidOperationException("Distance calculation did not converge.");
+
+        var uSquaredTimesC = uSquared * cSquared;
+        var aTimesB = semiMinorAxis * semiMinorAxis * cosSigma * cosSigma;
+        var bTimesA = semiMajorAxis * semiMajorAxis * sinSigma * sinSigmaPrev;
+        var sigmaP2 = sigmaP;
+        sigmaP = sigma;
+
+        var phi = Angle.Atan2(semiMinorAxis * cosU1 * sinLambda, semiMajorAxis * cosU1 * cosLambda);
+
+        var sinPhi = Angle.Sin(phi);
+        var cosPhi = Angle.Cos(phi);
+
+        var x = Angle.Atan2((semiMinorAxis / semiMajorAxis) * sinPhi + aTimesB * sinSigma * (cos2SigmaM +
+            aTimesB * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) / 4), (1 - uSquared) * (sinPhi - bTimesA *
+            sinSigmaPrev * (cos2SigmaM - bTimesA * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) / 4)));
+
+        var y = Angle.Atan2((1 - uSquared) * sinPhi + uSquaredTimesC * sinSigma * (cosSigma - uSquared *
+            sinSigmaPrev * (cos2SigmaM - uSquared * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) / 4)), (semiMajorAxis /
+            semiMinorAxis) * (cosPhi - bTimesA * sinSigma * (cos2SigmaM - bTimesA * cosSigma * (-1 + 2 *
+            cos2SigmaM * cos2SigmaM) / 4)));
+
+        var z = TAngle.Sqrt(x * x + y * y) * TAngle.Sign((semiMinorAxis - semiMajorAxis) * sinSigma * sinSigmaPrev);
+
+        return TAngle.Sqrt(x * x + y * y + z * z);
+    }
 }