public void Difference()
{
Angle a1 = Angle.FromDegrees(0);
Angle a2 = Angle.FromDegrees(2);
Assert.Equal(-2, AngleExtensions.Difference(a1, a2).Degrees, 5);
a1 = Angle.FromDegrees(0);
a2 = Angle.FromDegrees(-2);
Assert.Equal(2, AngleExtensions.Difference(a1, a2).Degrees, 5);
a1 = Angle.FromDegrees(359);
a2 = Angle.FromDegrees(2);
Assert.Equal(-3, AngleExtensions.Difference(a1, a2).Degrees, 5);
a1 = Angle.FromDegrees(719);
a2 = Angle.FromDegrees(2);
Assert.Equal(-3, AngleExtensions.Difference(a1, a2).Degrees, 5);
a1 = Angle.FromDegrees(719);
a2 = Angle.FromDegrees(-1);
Assert.Equal(0, AngleExtensions.Difference(a1, a2).Degrees, 5);
}
private bool HasPassedWaypoint(GeographicPosition position, Angle courseOverGround, ref RoutePoint?nextWaypoint, List <RoutePoint> currentRoute)
{
RoutePoint?previousWayPoint = null;
RoutePoint?wayPointAfterNext = null;
int idx = 0;
if (nextWaypoint != null)
{
idx = currentRoute.IndexOf(nextWaypoint);
}
else
{
// Can't have passed a null waypoint
return(false);
}
if (idx < 0)
{
// This is weird
return(false);
}
if (idx == 0)
{
previousWayPoint = _currentOrigin;
}
else
{
previousWayPoint = currentRoute[idx - 1];
}
if (idx < currentRoute.Count - 2)
{
wayPointAfterNext = currentRoute[idx + 1];
}
GreatCircle.DistAndDir(position, nextWaypoint.Position, out var distanceToNext, out var angleToNext);
if (distanceToNext < WaypointSwitchDistance)
{
_logger.LogInformation($"Reached waypoint {nextWaypoint.WaypointName}");
nextWaypoint = wayPointAfterNext;
return(true);
}
if (previousWayPoint != null && wayPointAfterNext != null)
{
GreatCircle.CrossTrackError(previousWayPoint.Position, nextWaypoint.Position, position, out var crossTrackCurrentLeg,
out _);
GreatCircle.CrossTrackError(nextWaypoint.Position, wayPointAfterNext.Position, position, out var crossTrackNextLeg,
out var distanceToAfterNext);
Angle delta = AngleExtensions.Difference(courseOverGround, angleToNext);
// We switch to the next leg if the cross track error to it is smaller than to the current leg.
// This condition is obvious for the side of the route with the smaller angle (that's the one in which the
// route bends at nextWaypoint), for the other side we need the additional condition that that waypoint
// is no longer ahead of us. This is the case if the direction to it and our current track are pointing in
// opposite directions
if (crossTrackCurrentLeg > crossTrackNextLeg && Math.Abs(delta.Normalize(false).Degrees) > 90)
{
_logger.LogInformation($"Reached waypoint {nextWaypoint.WaypointName}");
nextWaypoint = wayPointAfterNext;
return(true);
}
}
return(false);
}
