From 6dcd040e16e0ee7ac835e11a8d953f482e04bdbb Mon Sep 17 00:00:00 2001 From: Antao Almada Date: Tue, 13 Jun 2023 13:52:25 +0100 Subject: [PATCH 1/4] 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 c64a91a..ae86d2b 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 From 49528eef15d9a0a429edfe82ef671f43aadd5dad Mon Sep 17 00:00:00 2001 From: Antao Almada Date: Tue, 13 Jun 2023 20:13:00 +0100 Subject: [PATCH 2/4] Rename --- src/NetFabric.Numerics.Geography/Geodetic2/Point.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs index ae86d2b..12a0358 100644 --- a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs +++ b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs @@ -136,7 +136,7 @@ public static class Point /// sphere, also known as the great-circle distance. /// /// - public static TAngle DistanceSpherical(Point from, Point to) + public static TAngle DistanceSphere(Point from, Point to) where TDatum : IDatum where TAngle : struct, IFloatingPointIeee754, IMinMaxValue { From 3e4f0427d0578e631a02a2a63de680ee4264d456 Mon Sep 17 00:00:00 2001 From: Antao Almada Date: Sun, 18 Jun 2023 18:16:34 +0100 Subject: [PATCH 3/4] Refactor ellipsoid --- .../Geodetic2/PointTests.cs | 15 --------- .../Geodetic2/Point.cs | 31 ++++++------------- 2 files changed, 10 insertions(+), 36 deletions(-) diff --git a/src/NetFabric.Numerics.Geography.UnitTests/Geodetic2/PointTests.cs b/src/NetFabric.Numerics.Geography.UnitTests/Geodetic2/PointTests.cs index 5af74ad..d5403b9 100644 --- a/src/NetFabric.Numerics.Geography.UnitTests/Geodetic2/PointTests.cs +++ b/src/NetFabric.Numerics.Geography.UnitTests/Geodetic2/PointTests.cs @@ -28,20 +28,5 @@ public void CoordinateSystem_Should_Succeed() //result.Coordinates.Should().Equal( // new Coordinate("Latitude", typeof(Angle)), // new Coordinate("Longitude", typeof(Angle))); - -var wgs84Point = new Point(new(0.0), new(0.0)); // Geodetic point using WGS84 datum -var wgs1972Point = new Point(new(0.0), new(0.0)); // Geodetic point using WGS1972 datum -var nad83Point = new Point(new(0.0), new(0.0)); // Geodetic point using NAD83 datum -var nad1927ConusPoint = new Point(new(0.0), new(0.0)); // Geodetic point using NAD1927CONUS datum - -var doublePrecisionPoint = new Point(new(0.0), new(0.0)); // Geodetic point with double precision -var singlePrecisionPoint = new Point(new(0.0f), new(0.0f)); // Geodetic point with single precision - -var minutesPoint = new Point(Angle.ToDegrees(0, 0.0), Angle.ToDegrees(0, 0.0)); // Geodetic point using degrees and minutes -var minutesSecondsPoint = new Point(Angle.ToDegrees(0, 0, 0.0), Angle.ToDegrees(0, 0, 0.0)); // Geodetic point using degrees, minutes and seconds - -var (degreesLatitude, minutesLatitude) = Angle.ToDegreesMinutes(wgs84Point.Latitude); // Convert latitude to degrees and minutes -var (degreesLatitude2, minutesLatitude2, secondsLatitude) = Angle.ToDegreesMinutesSeconds(wgs84Point.Latitude); // Convert latitude to degrees, minutes, and seconds - } } diff --git a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs index 12a0358..52949a4 100644 --- a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs +++ b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs @@ -149,15 +149,6 @@ public static TAngle DistanceSphere(Point @@ -193,16 +184,14 @@ public static TAngle DistanceEllipsoid(Point(Point sigma; TAngle cosSigma, sinSigma, cos2SigmaM, sinSigmaPrev; TAngle sigmaP = TAngle.Zero; + TAngle two = TAngle.One + TAngle.One; do { - sinLambda = Angle.Sin(lambda); - cosLambda = Angle.Cos(lambda); + (sinLambda, cosLambda) = Angle.SinCos(lambda); sinSigma = TAngle.Sqrt((cosU2 * sinLambda * (cosU2 * sinLambda)) + (((cosU1 * sinU2) - (sinU1 * cosU2 * cosLambda)) * ((cosU1 * sinU2) - (sinU1 * cosU2 * cosLambda)))); @@ -227,13 +216,13 @@ public static TAngle DistanceEllipsoid(Point 1e-12 && --iterationLimit > 0); From 79674b46ce38967f3844aaf6f41176ac34b4bcdb Mon Sep 17 00:00:00 2001 From: Antao Almada Date: Sun, 18 Jun 2023 19:07:31 +0100 Subject: [PATCH 4/4] Fix --- src/NetFabric.Numerics.Geography/Geodetic2/Point.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs index 52949a4..123fdc4 100644 --- a/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs +++ b/src/NetFabric.Numerics.Geography/Geodetic2/Point.cs @@ -148,7 +148,7 @@ public static TAngle DistanceSphere(Point