/// <summary>
/// Calculate the distance to another position
/// </summary>
/// <param name="position1">The first position. This argument can be implicitly given</param>
/// <param name="position2">The position to go to</param>
/// <returns>The distance between the two points</returns>
public static Length DistanceTo(this GeographicPosition position1, GeographicPosition position2)
{
Length result;
GreatCircle.DistAndDir(position1, position2, out result, out _);
return(result);
}
/// <summary>
/// Calculates the initial angle to travel to get to another position.
/// Calculates on the great circle, therefore the direction to the target is not constant along the path
/// </summary>
/// <param name="position1">The initial position. This argument can be implicitly given</param>
/// <param name="position2">The destination position</param>
/// <returns>The angle to travel</returns>
/// <remarks>If both distance and direction are required, prefer to use <see cref="GreatCircle.DistAndDir(Iot.Device.Common.GeographicPosition,Iot.Device.Common.GeographicPosition,out UnitsNet.Length,out UnitsNet.Angle)"/></remarks>
public static Angle DirectionTo(this GeographicPosition position1, GeographicPosition position2)
{
Angle result;
GreatCircle.DistAndDir(position1, position2, out _, out result);
return(result);
}
/// <summary>
/// Calculate a list of waypoints along the route from start to end.
/// </summary>
/// <param name="start">Starting position</param>
/// <param name="end">End position</param>
/// <param name="distanceStep">Distance between waypoints</param>
/// <returns>A list of waypoints</returns>
public static IList <GeographicPosition> CalculateRoute(GeographicPosition start, GeographicPosition end, double distanceStep)
{
if (start == null)
{
throw new ArgumentNullException(nameof(start));
}
if (end == null)
{
throw new ArgumentNullException(nameof(end));
}
IList <GeographicPosition> ret = new List <GeographicPosition>();
GeoidCalculations.geod_geodesicline line;
GeoidCalculations.geod_inverseline(out line, _geod, start.Latitude, start.Longitude, end.Latitude, end.Longitude, 0);
double distanceTotal = line.s13;
for (double d = 0; d <= distanceTotal; d += distanceStep)
{
GeoidCalculations.geod_position(line, d, out double lat2, out double lon2, out _);
ret.Add(new GeographicPosition(lat2, lon2, 0));
}
return(ret);
}
/// <summary>
/// Calculate the coordinate one will be when traveling for the given distance in the given direction
/// </summary>
/// <param name="start">Starting point</param>
/// <param name="direction">Initial direction</param>
/// <param name="distance">Distance to travel</param>
/// <returns>The new position</returns>
/// <exception cref="ArgumentNullException">The start position is null</exception>
public static GeographicPosition CalcCoords(GeographicPosition start, Angle direction, Length distance)
{
if (start == null)
{
throw new ArgumentNullException(nameof(start));
}
GeoidCalculations.geod_direct(_geod, start.Latitude, start.Longitude, direction.Degrees, distance.Meters, out double resultLatitude, out double resultLongitude, out _);
return(new GeographicPosition(resultLatitude, resultLongitude, start.EllipsoidalHeight));
}
/// <summary>
/// Returns the distance and direction between two points on the globe
/// </summary>
/// <param name="position1">Input position 1</param>
/// <param name="position2">Input position 2</param>
/// <param name="distance">Great circle distance between the positions</param>
/// <param name="direction">Initial direction to travel, in degrees true</param>
/// <exception cref="ArgumentNullException">Either <paramref name="position1"/> or <paramref name="position2"/> are null.</exception>
/// <remarks>The distance and direction are calculated for the great circle. That is the shortest distance between two points on the globe.
/// This path does not follow a constant direction (for large distances)</remarks>
public static void DistAndDir(GeographicPosition position1, GeographicPosition position2, out Length distance, out Angle direction)
{
if (position1 == null)
{
throw new ArgumentNullException(nameof(position1));
}
if (position2 == null)
{
throw new ArgumentNullException(nameof(position2));
}
DistAndDir(position1.Latitude, position1.Longitude, position2.Latitude, position2.Longitude, out double dist, out double dir);
distance = Length.FromMeters(dist);
direction = Angle.FromDegrees(dir).Normalize(true);
}
/// <summary>
/// Calculate the velocity towards (or away from) the target. This is often also called VMG (=Velocity made good)
/// </summary>
/// <param name="destination">Target waypoint</param>
/// <param name="currentPosition">Current position</param>
/// <param name="currentSpeed">Current speed over ground</param>
/// <param name="currentTrack">Current track (course over ground)</param>
/// <returns>Speed towards target. Negative if moving away from target</returns>
public static Speed CalculateVelocityTowardsTarget(GeographicPosition destination, GeographicPosition currentPosition, Speed currentSpeed, Angle currentTrack)
{
if (destination == null)
{
throw new ArgumentNullException(nameof(destination));
}
if (currentPosition == null)
{
throw new ArgumentNullException(nameof(currentPosition));
}
DistAndDir(currentPosition, destination, out Length distanceToDestination, out Angle currentToDestination);
Angle delta = AngleExtensions.Difference(currentToDestination, currentTrack);
return(currentSpeed * Math.Cos(delta.Radians));
}
/// <summary>
/// Computes cross-track error, that is the distance the current position is away from the route from origin to destination
/// </summary>
/// <param name="origin">Start of current leg</param>
/// <param name="destination">End of current leg</param>
/// <param name="currentPosition">Current position</param>
/// <param name="crossTrackError">The distance perpendicular to the leg. Positive if the current position is to the right of the leg.</param>
/// <param name="distanceTogoAlongRoute">Distance to go on track (with current position projected back to the leg)</param>
/// <remarks>Accuracy may be limited for distances > 100km</remarks>
public static void CrossTrackError(GeographicPosition origin, GeographicPosition destination, GeographicPosition currentPosition,
out Length crossTrackError, out Length distanceTogoAlongRoute)
{
if (origin == null)
{
throw new ArgumentNullException(nameof(origin));
}
if (destination == null)
{
throw new ArgumentNullException(nameof(destination));
}
DistAndDir(origin, destination, out _, out _, out Angle trackEndDirection);
DistAndDir(currentPosition, destination, out Length distanceToDestination, out Angle currentToDestination);
Angle angleDiff = AngleExtensions.Difference(trackEndDirection, currentToDestination);
distanceTogoAlongRoute = Length.FromMeters(Math.Cos(angleDiff.Radians) * distanceToDestination.Meters);
crossTrackError = Length.FromMeters(Math.Sin(angleDiff.Radians) * distanceToDestination.Meters);
}
/// <summary>
/// Normalizes the longitude to [0..360°)
/// This coordinate form is advised if working in an area near the date border in the pacific ocean.
/// </summary>
public static GeographicPosition NormalizeLongitudeTo360Degrees(this GeographicPosition position)
{
return(new GeographicPosition(position.Latitude, NormalizeAngleTo360Degrees(position.Longitude), position.EllipsoidalHeight));
}
/// <summary>
/// Move a certain distance into a direction. Where do I end?
/// </summary>
/// <param name="position">Start position.</param>
/// <param name="direction">Direction to travel</param>
/// <param name="distance">Distance to travel</param>
/// <returns>The destination position</returns>
public static GeographicPosition MoveBy(this GeographicPosition position, Angle direction, Length distance)
{
return(GreatCircle.CalcCoords(position, direction, distance));
}
