private static VincentyFormulaVariables Recalculateλ(VincentyFormulaVariables v)
{
var cosλ = Math.Cos(v.λ);
var sinλ = Math.Sin(v.λ);
v.Sinσ = Math.Sqrt(Math.Pow(v.CosU2 * Math.Sin(v.λ), 2.0) +
Math.Pow(v.CosU1 * v.SinU2 -
v.SinU1 * v.CosU2 * cosλ, 2.0));
v.Cosσ = v.SinU1 * v.SinU2 +
v.CosU1 * v.CosU2 * cosλ;
v.σ = Math.Atan2(v.Sinσ, v.Cosσ);
v.sinα = (v.CosU1 * v.CosU2 * sinλ) / v.Sinσ;
v.CosSqα = 1 - Math.Pow(v.sinα, 2.0);
v.Cos2σM = v.Cosσ - ((2.0 * v.SinU1 * v.SinU2) / v.CosSqα);
v.C = (f / 16.0) * v.CosSqα * (4 + f * (4.0 - 3.0 * v.CosSqα));
v.λ = v.L + (1.0 - v.C) * f * v.sinα * (v.σ + v.C * v.Sinσ * (v.Cos2σM + v.C * v.Cosσ * (-1.0 + 2.0 * Math.Pow(v.Cos2σM, 2.0))));
return(v);
}
private static double CalcaulateBackwardAzimuthInverseMethod(VincentyFormulaVariables v)
{
var dividend = Math.Cos(v.U1) * Math.Sin(v.λ);
var divisor = Math.Cos(v.U1) * Math.Sin(v.U2) * Math.Cos(v.λ) -
Math.Sin(v.U1) * Math.Cos(v.U2);
var azimuth = Math.Atan2(dividend, divisor);
return(azimuth >= 0 ? azimuth - Math.PI : azimuth + Math.PI);
}
private static double CalculateForwardAzimuth(VincentyFormulaVariables v)
{
var dividend = Math.Cos(v.U2) * Math.Sin(v.λ);
var divisor = Math.Cos(v.U1) * Math.Sin(v.U2) -
Math.Sin(v.U1) * Math.Cos(v.U2) * Math.Cos(v.λ);
var azimuth = Math.Atan2(dividend, divisor);
return(azimuth);
}
private static VincentyFormulaVariables VincentysDirectFormula(IGeographicCoordinate pointA, IAngle initialBearing, IDistance distance,
int maxInterations = 200, double tolerance = 1.0E-12)
{
// Source: https://www.movable-type.co.uk/scripts/latlong-vincenty.html
VincentyFormulaVariables v = PrepareσConvergenceLoop(pointA, initialBearing, distance);
v = RecalculateσUntilConvergence(distance, maxInterations, tolerance, v);
v.DestinationCoordinates = CalculateDestinationCoordinates(pointA, v);
v.FinalBearing = Math.Atan2(v.sinα, -1.0 * (v.SinU1 * v.Sinσ - v.CosU1 * v.Cosσ * v.Cosα1));
return(v);
}
private static VincentyFormulaVariables RecalculateλUntilConvergence(VincentyFormulaVariables v, int maxInterations, double tolerance)
{
for (var i = 0; i < maxInterations; i++)
{
double Lambda_prev = v.λ;
v = Recalculateλ(v);
var diff = Math.Abs(Lambda_prev - v.λ);
if (diff <= tolerance)
{
break;
}
}
return(v);
}
private static VincentyFormulaVariables CalculateGeodesicDistanceAzimuth(VincentyFormulaVariables v)
{
v.USquared = v.CosSqα * ((Math.Pow(a, 2.0) - Math.Pow(b, 2.0)) / Math.Pow(b, 2.0));
v.A = 1 + (v.USquared / 16384.0) * (4096.0 + v.USquared * (-768.0 + v.USquared * (320.0 - 175.0 * v.USquared)));
v.B = (v.USquared / 1024.0) * (256.0 + v.USquared * (-128.0 + v.USquared * (74.0 - 47.0 * v.USquared)));
v.Δσ = v.B * v.Sinσ * (v.Cos2σM + 0.25 * v.B * (v.Cosσ * (-1.0 + 2.0 * Math.Pow(v.Cos2σM, 2.0)) -
(1.0 / 6.0) * v.B * v.Cos2σM * (-3.0 + 4.0 * Math.Pow(v.Sinσ, 2.0)) *
(-3.0 + 4.0 * Math.Pow(v.Cos2σM, 2.0))));
v.GeodesicLength = b * v.A * (v.σ - v.Δσ);
v.ForwardAzimuth = CalculateForwardAzimuth(v);
v.BackwardAzimuth = CalcaulateBackwardAzimuthInverseMethod(v);
return(v);
}
private static VincentyFormulaVariables PrepareλConvergenceLoop(IGeographicCoordinate pointA, IGeographicCoordinate pointB)
{
var v = new VincentyFormulaVariables
{
U1 = Math.Atan((1.0 - f) * Math.Tan(pointA.Latitude.Angle.ToRadians())),
U2 = Math.Atan((1.0 - f) * Math.Tan(pointB.Latitude.Angle.ToRadians())),
L = Angle.ToRadians(pointB.Longitude.Angle.ToDegrees() - pointA.Longitude.Angle.ToDegrees())
};
v.λ = v.L;
v.SinU1 = Math.Sin(v.U1);
v.CosU1 = Math.Cos(v.U1);
v.SinU2 = Math.Sin(v.U2);
v.CosU2 = Math.Cos(v.U2);
return(v);
}
private static IBearingDistance VincentysInverseFormula(IGeographicCoordinate pointA, IGeographicCoordinate pointB, int maxInterations = 200, double tolerance = 1.0E-12)
{
// Source: https://nathanrooy.github.io/posts/2016-12-18/vincenty-formula-with-python/
VincentyFormulaVariables v = PrepareλConvergenceLoop(pointA, pointB);
v = RecalculateλUntilConvergence(v, maxInterations, tolerance);
v = CalculateGeodesicDistanceAzimuth(v);
IBearingDistance bearingDistance = new BearingDistance()
{
Distance = new Distance(v.GeodesicLength, DistanceMeasurement.Meters),
InitialBearing = new Angle(v.ForwardAzimuth, AngleMeasurement.Radians),
FinalBearing = new Angle(v.BackwardAzimuth, AngleMeasurement.Radians)
};
return(bearingDistance);
}
private static VincentyFormulaVariables PrepareσConvergenceLoop(IGeographicCoordinate pointA, IAngle initialBearing, IDistance distance)
{
VincentyFormulaVariables v = new VincentyFormulaVariables();
v.Sinα1 = Math.Sin(initialBearing.ToRadians());
v.Cosα1 = Math.Cos(initialBearing.ToRadians());
v.TanU1 = (1.0 - f) * Math.Tan(pointA.Latitude.Angle.ToRadians());
v.CosU1 = 1.0 / Math.Sqrt((1 + Math.Pow(v.TanU1, 2.0)));
v.SinU1 = v.TanU1 * v.CosU1;
v.σ1 = Math.Atan2(v.TanU1, v.Cosα1);
v.sinα = v.CosU1 * v.Sinα1;
v.CosSqα = 1.0 - Math.Pow(v.sinα, 2.0);
v.USquared = v.CosSqα * (Math.Pow(a, 2.0) - Math.Pow(b, 2.0)) / Math.Pow(b, 2.0);
v.A = 1 + v.USquared / 16384.0 * (4096.0 + v.USquared * (-768.0 + v.USquared * (320.0 - 175.0 * v.USquared)));
v.B = v.USquared / 1024.0 * (256.0 + v.USquared * (-128.0 + v.USquared * (74.0 - 47.0 * v.USquared)));
v.σ = distance.ToMeters() / (b * v.A);
return(v);
}
private static IGeographicCoordinate CalculateDestinationCoordinates(IGeographicCoordinate pointA, VincentyFormulaVariables v)
{
var tmp = v.SinU1 * v.Sinσ - v.CosU1 * v.Cosσ * v.Cosα1;
v.φ2 = Math.Atan2(v.SinU1 * v.Cosσ + v.CosU1 * v.Sinσ * v.Cosα1, (1 - f) * Math.Sqrt(Math.Pow(v.sinα, 2.0) + Math.Pow(tmp, 2.0)));
v.λ = Math.Atan2(v.Sinσ * v.Sinα1, v.CosU1 * v.Cosσ - v.SinU1 * v.Sinσ * v.Cosα1);
v.C = f / 16.0 * v.CosSqα * (4.0 + f * (4.0 - 3.0 * v.CosSqα));
v.L = v.λ - (1.0 - v.C) * f * v.sinα *
(v.σ + v.C * v.Sinσ * (v.Cos2σM + v.C * v.Cosσ * (-1.0 + 2.0 * Math.Pow(v.Cos2σM, 2.0))));
v.λ2 = (pointA.Longitude.Angle.ToRadians() + v.L + 3.0 * Math.PI) % (2.0 * Math.PI) - Math.PI; // normalise to -180...+180
var latitude = Angle.ToDegrees(v.φ2);
var longitude = Angle.ToDegrees(v.λ2);
return(new GeographicCoordinate(latitude, longitude));
}
private static VincentyFormulaVariables RecalculateσUntilConvergence(IDistance distance, int maxInterations, double tolerance, VincentyFormulaVariables v)
{
for (var i = 0; i < maxInterations; i++)
{
v.Cos2σM = Math.Cos(2 * v.σ1 + v.σ);
v.Sinσ = Math.Sin(v.σ);
v.Cosσ = Math.Cos(v.σ);
v.Δσ = v.B * v.Sinσ * (v.Cos2σM + v.B / 4.0 * (v.Cosσ * (-1.0 + 2.0 * Math.Pow(v.Cos2σM, 2.0)) -
v.B / 6.0 * v.Cos2σM * (-3.0 + 4.0 * Math.Pow(v.Sinσ, 2.0)) * (-3.0 + 4.0 * Math.Pow(v.Cos2σM, 2.0))));
v.σPrevious = v.σ;
v.σ = distance.ToMeters() / (b * v.A) + v.Δσ;
var diff = Math.Abs(v.σ - v.σPrevious);
if (diff <= tolerance)
{
break;
}
}
return(v);
}
