From f21213c903261c63594f3759f532bdd5027ea005 Mon Sep 17 00:00:00 2001 From: Antao Almada Date: Tue, 13 Jun 2023 13:52:25 +0100 Subject: [PATCH] Add geodetic distance --- .../Geodetic2/Point.cs | 155 ++++++++++++++++++ 1 file changed, 155 insertions(+) diff --git a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs index cbfb68a..c08cafe 100644 --- a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs +++ b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs @@ -109,4 +109,159 @@ object IPoint>.this[int index] 1 => Longitude, _ => Throw.ArgumentOutOfRangeException(nameof(index), index, "index out of range") }; +} + +/// +/// Provides static methods for point operations. +/// +public static class Point +{ + /// + /// Calculates the distance between two geodetic points. + /// + /// The first geodetic point. + /// The second geodetic point. + /// The distance between the two geodetic points, in meters. + /// + /// + /// 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. + /// + /// + /// 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. + /// + /// + /// 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. + /// + /// + public static TAngle DistanceSpherical(Point from, Point to) + where TDatum : IDatum + where TAngle : struct, IFloatingPointIeee754, IMinMaxValue + { + 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); + } + } + + /// + /// Calculates the distance between two geodetic points. + /// + /// The first geodetic point. + /// The second geodetic point. + /// The distance between the two geodetic points, in meters. + /// The iteration did not converge." + /// + /// + /// 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. + /// + /// + /// 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 is thrown. + /// + /// + public static TAngle DistanceEllipsoid(Point from, Point to) + where TDatum : IDatum + where TAngle : struct, IFloatingPointIeee754, IMinMaxValue + { + 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 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); + } } \ No newline at end of file