/// <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);
}
