Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add geodetic distance #5

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Add geodetic distance #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6dcd040
Add geodetic distance
aalmada Jun 13, 2023
49528ee
Rename
aalmada Jun 13, 2023
3e4f042
Refactor ellipsoid
aalmada Jun 18, 2023
79674b4
Fix
aalmada Jun 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 0 additions & 15 deletions src/NetFabric.Numerics.Geography.UnitTests/Geodetic2/PointTests.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,20 +28,5 @@ public void CoordinateSystem_Should_Succeed()
//result.Coordinates.Should().Equal(
// new Coordinate("Latitude", typeof(Angle<Degrees, double>)),
// new Coordinate("Longitude", typeof(Angle<Degrees, double>)));

var wgs84Point = new Point<WGS84, Degrees, double>(new(0.0), new(0.0)); // Geodetic point using WGS84 datum
var wgs1972Point = new Point<WGS1972, Degrees, double>(new(0.0), new(0.0)); // Geodetic point using WGS1972 datum
var nad83Point = new Point<NAD83, Degrees, double>(new(0.0), new(0.0)); // Geodetic point using NAD83 datum
var nad1927ConusPoint = new Point<NAD1927CONUS, Degrees, double>(new(0.0), new(0.0)); // Geodetic point using NAD1927CONUS datum

var doublePrecisionPoint = new Point<WGS84, Degrees, double>(new(0.0), new(0.0)); // Geodetic point with double precision
var singlePrecisionPoint = new Point<WGS84, Degrees, float>(new(0.0f), new(0.0f)); // Geodetic point with single precision

var minutesPoint = new Point<WGS84, Degrees, double>(Angle.ToDegrees<double>(0, 0.0), Angle.ToDegrees<double>(0, 0.0)); // Geodetic point using degrees and minutes
var minutesSecondsPoint = new Point<WGS84, Degrees, double>(Angle.ToDegrees<double>(0, 0, 0.0), Angle.ToDegrees<double>(0, 0, 0.0)); // Geodetic point using degrees, minutes and seconds

var (degreesLatitude, minutesLatitude) = Angle.ToDegreesMinutes<double, int, double>(wgs84Point.Latitude); // Convert latitude to degrees and minutes
var (degreesLatitude2, minutesLatitude2, secondsLatitude) = Angle.ToDegreesMinutesSeconds<double, int, int, double>(wgs84Point.Latitude); // Convert latitude to degrees, minutes, and seconds

}
}
144 changes: 144 additions & 0 deletions src/NetFabric.Numerics.Geography/Geodetic2/Point.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,4 +109,148 @@ object IPoint<Point<TDatum, TAngleUnits, T>>.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 DistanceSphere<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(Ellipsoid.ArithmeticMeanRadius(TDatum.Ellipsoid)) * c.Value;
}

/// <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 = TAngle.CreateChecked(TDatum.Ellipsoid.EquatorialRadius);
var flatteningInverse = TAngle.CreateChecked(1.0 / TDatum.Ellipsoid.Flattening);
var semiMinorAxis = semiMajorAxis * (TAngle.One - flatteningInverse);

var uSquared = TAngle.CreateChecked(((semiMajorAxis * semiMajorAxis) - (semiMinorAxis * semiMinorAxis)) / (semiMinorAxis * semiMinorAxis));

var (sinU1, cosU1) = Angle.SinCos(from.Latitude);
var (sinU2, cosU2) = Angle.SinCos(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;
TAngle two = TAngle.One + TAngle.One;

do
{
(sinLambda, cosLambda) = Angle.SinCos(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 - (two * sinU1 * sinU2 / ((cosU1 * cosU2) + (sinU1 * sinU2)));

var cSquared = uSquared * cosSigma * cosSigma;
var lambdaP = lambda;
lambda = longitudeDifference + ((TAngle.One - cSquared) * uSquared * sinSigma *
(sigma + (uSquared * sinSigmaPrev * (cos2SigmaM +
(uSquared * cosSigma * (-TAngle.One + (two * 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);
}
}