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