public void RecordsDestination() { // Arrange var orig = _landsEnd; var dest = _JohnoGroats; // Act var route = new GreatCircle(orig, dest); // Assert Assert.That( dest.Latitude, Is.EqualTo(_JohnoGroats.Latitude).Within(0.0001), "Recorded destination's latitude did not match supplied values"); Assert.That( dest.Longitude, Is.EqualTo(_JohnoGroats.Longitude).Within(0.0001), "Recorded destination's longitude did not match supplied values"); Assert.That( dest.Altitude, Is.EqualTo(_JohnoGroats.Altitude).Within(0.0001), "Recorded destination's altitude did not match supplied values"); Assert.That( dest.WorldRadius, Is.EqualTo(_JohnoGroats.WorldRadius).Within(0.0001), "Recorded destination's radius did not match supplied values"); }
public void AviaticToRadiansTest() { Assert.Equal(0, GreatCircle.AviaticToRadians(90)); Assert.Equal(Math.PI / 2.0, GreatCircle.AviaticToRadians(0)); Assert.Equal(Math.PI, GreatCircle.AviaticToRadians(270)); Assert.Equal(Math.PI, GreatCircle.AviaticToRadians(-90)); Assert.Equal(Math.PI / 2.0 * 3, GreatCircle.AviaticToRadians(180)); }
public void RadiansToAviaticTest() { Assert.Equal(90, GreatCircle.RadiansToAviatic(0)); Assert.Equal(0, GreatCircle.RadiansToAviatic(Math.PI / 2.0)); Assert.Equal(270, GreatCircle.RadiansToAviatic(Math.PI)); Assert.Equal(270, GreatCircle.RadiansToAviatic(-Math.PI)); Assert.Equal(180, GreatCircle.RadiansToAviatic(-Math.PI / 2.0)); }
public void StructureArgumentGetsSameResultCalcCoords() { GreatCircle.CalcCoords(21.0, -120.12, 88.0, 1002, out double resLat, out double resLon); var newPos = GreatCircle.CalcCoords(new GeographicPosition(21, -120.12, 250), Angle.FromDegrees(88.0), Length.FromMeters(1002)); Assert.Equal(resLat, newPos.Latitude); Assert.Equal(resLon, newPos.Longitude); }
public void CrossTrackError1() { GeographicPosition start = new GeographicPosition(0, 0, 0); GeographicPosition end = new GeographicPosition(1, 0, 0); GreatCircle.CrossTrackError(start, end, start, out var crossTrackError, out Length distance); Assert.Equal(59.7053933897411, distance.NauticalMiles, 2); // 1 degree latitude = ~60 nautical miles Assert.Equal(0, crossTrackError.Meters); // start on track -> deviation is 0 }
public void StructureArgumentGetsSameResultDistAndDir() { GreatCircle.DistAndDir(-35.84, 94, 35.87, -85, out var dist1, out var dir1); GreatCircle.DistAndDir(new GeographicPosition(-35.84, 94, 100), new GeographicPosition(35.87, -85, 200), out var dist2, out var dir2); Assert.Equal(dist1, dist2.Meters); Assert.Equal(dir1 + 360.0, dir2.Degrees); }
public void CrossTrackError2() { GeographicPosition start = new GeographicPosition(1, 0, 0); GeographicPosition end = new GeographicPosition(2, 0, 0); GeographicPosition current = new GeographicPosition(1.75, 0, 0); GreatCircle.CrossTrackError(start, end, current, out var crossTrackError, out Length distance); Assert.Equal(14.9264938243846, distance.NauticalMiles, 4); // 1 degree latitude = 60 nautical miles. A quarter of it is remaining Assert.Equal(0, crossTrackError.Meters); // On track -> deviation is 0 }
public void CrossTrackError4() { GeographicPosition start = new GeographicPosition(1, 0, 0); GeographicPosition end = new GeographicPosition(2, 0, 0); GeographicPosition current = new GeographicPosition(1.75, -1.0 / 60.0, 0); GreatCircle.CrossTrackError(start, end, current, out var crossTrackError, out Length distance); Assert.Equal(14.9264938243846, distance.NauticalMiles, 4); // 1 degree latitude = 60 nautical miles. A third of it is remaining Assert.Equal(-1.00, crossTrackError.NauticalMiles, 2); // One nautical mile off, to the left }
private static void InternalDistDir(GeographicPosition startPosition, GeographicPosition endPosition, ref double distance, ref double direction) { double deltaLat = endPosition.Latitude - startPosition.Latitude; double deltaLon = endPosition.Longitude - startPosition.Longitude; double deltaX = deltaLon * GreatCircle.MetersPerDegreeLongitude * Math.Abs(Math.Cos(startPosition.Latitude * Math.PI / 180.0)); double deltaY = deltaLat * GreatCircle.MetersPerDegreeeLatitude; distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY); direction = GreatCircle.RadiansToAviatic(Math.Atan2(deltaY, deltaX)); }
private void MainSimulator() { while (!_terminate) { var newData = _activeData.Clone(); GeographicPosition newPosition = GreatCircle.CalcCoords(newData.Position, _activeData.Course, _activeData.Speed * UpdateRate); newData.Position = newPosition; _activeData = newData; SendNewData(); Thread.Sleep(UpdateRate); } }
public void CalculatesNewCoordinates_NoDeltaAlt() { // Arrange var orig = _landsEnd; var route = new GreatCircle(_landsEnd, _JohnoGroats); // Act var dest = orig.Travel(route); // Assert Assert.That(dest.Latitude, Is.EqualTo(_JohnoGroats.Latitude).Within(0.0001), "Miscalculation of final latitude"); Assert.That(dest.Longitude, Is.EqualTo(_JohnoGroats.Longitude).Within(0.0001), "Miscalculation of final longitude"); Assert.That(dest.Altitude, Is.EqualTo(_JohnoGroats.Altitude).Within(0.0001), "Miscalculation of final altitude"); }
public void CalculateVelocityTowardsTarget1() { GeographicPosition end = new GeographicPosition(2, 0, 0); GeographicPosition current = new GeographicPosition(1, 0, 0); Speed result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.Zero); Assert.Equal(Speed.FromMetersPerSecond(10), result); // directly towards target result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(180)); Assert.Equal(-Speed.FromMetersPerSecond(10), result); // directly away from target result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(270)); Assert.Equal(Speed.Zero.MetersPerSecond, result.MetersPerSecond, 5); // perpendicular to target }
public void RouteCalculationForward() { GeographicPosition start = new GeographicPosition(0, 1, 0); var points = GreatCircle.CalculateRoute(start, 180, 1000, 100); Assert.Equal(11, points.Count); double previousLat = 1.1; foreach (var pt in points) { Assert.True(previousLat > pt.Latitude); previousLat = pt.Latitude; Assert.Equal(1, pt.Longitude); } }
public void CalculateVelocityTowardsTarget2() { GeographicPosition end = new GeographicPosition(2, 0, 0); GeographicPosition current = new GeographicPosition(1.75, 0.25, 0); Speed result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.Zero); // Will miss the target like this Assert.True(Math.Abs((Speed.FromMetersPerSecond(10) * Math.Cos(45.0 / 180 * Math.PI)).MetersPerSecond - result.MetersPerSecond) < 0.05, result.ToString()); result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(45)); Assert.True(Math.Abs(result.MetersPerSecond) <= 0.04, result.ToString()); // about perpendicular to target result = GreatCircle.CalculateVelocityTowardsTarget(end, current, Speed.FromMetersPerSecond(10), Angle.FromDegrees(280)); Assert.True(Math.Abs(8.21 - result.MetersPerSecond) < 0.01, result.ToString()); // perpendicular to target }
public void RouteCalculationInverse() { GeographicPosition start = new GeographicPosition(0, 1, 0); GeographicPosition end = new GeographicPosition(0, 2, 0); var points = GreatCircle.CalculateRoute(start, end, 1000); Assert.Equal(112, points.Count); double previousLon = 0.9; foreach (var pt in points) { Assert.True(previousLon < pt.Longitude); previousLon = pt.Longitude; Assert.Equal(0, pt.Latitude); } }
/// <summary> /// Calculates distances and directions between the waypoints /// </summary> private void CalculateMetaData() { for (var index = 0; index < _routePoints.Count; index++) { var pt = _routePoints[index]; // First, make sure the total length and other properties are copied correctly everywhere pt.RouteName = Name; pt.IndexInRoute = index; pt.TotalPointsInRoute = _routePoints.Count; if (index != _routePoints.Count - 1) { GreatCircle.DistAndDir(pt.Position, _routePoints[index + 1].Position, out var distance, out var direction); pt.DistanceToNextWaypoint = distance; pt.BearingToNextWaypoint = direction; } } }
public void CalculatesGreatCircle_Kerbin() { // Arrange var orig = _KscFlagpole; var dest = _VAB; var expectedDistanceAtDestinationAltitude = 357.3329782527984d; var expectedDistanceAtOriginAltitude = 357.33109503718202d; var expectedDistanceAtSeaLeval = 357.29275513850934d; var expectedDistanceWithAltitudeChange = 357.33203664499024d; var expectedIntialBearing = 94.383387027330514d; var expectedAltitudeChange = 3.1624748992035023d; // Act var route = new GreatCircle(orig, dest); // Assert Assert.That( route.DeltaASL, Is.EqualTo(expectedAltitudeChange).Within(0.0001), "Miscalculated altitude difference"); Assert.That( route.DistanceAtDestAlt, Is.EqualTo(expectedDistanceAtDestinationAltitude).Within(0.0001), "Miscalculated distance at destination's altitude"); Assert.That( route.DistanceAtOrigAlt, Is.EqualTo(expectedDistanceAtOriginAltitude).Within(0.0001), "Miscalculated distance at origin's altitude"); Assert.That( route.DistanceAtSeaLevel, Is.EqualTo(expectedDistanceAtSeaLeval).Within(0.0001), "Miscalculated distance at sea level"); Assert.That( route.DistanceWithAltChange, Is.EqualTo(expectedDistanceWithAltitudeChange).Within(0.0001), "Miscalculated distance including altitude difference"); Assert.That( route.ForwardAzimuth, Is.EqualTo(expectedIntialBearing).Within(0.0001), "Miscalulated initial bearing"); }
public void WGS84Test_Statics() { Assert.True(TestDistAndDir(0, 180, 0, -180)); Assert.True(TestDistAndDir(0, -179, 0, 179)); Assert.True(TestDistAndDir(45, 10, -55, -170)); Assert.True(TestDistAndDir(-35.84, 94, 35.87, -85)); double dist = 0; double dir = 0; GreatCircle.DistAndDir(47, 9, 47, 9, out dist, out dir); Assert.Equal(0, dist); double dist2 = 0; double dir2 = 0; GreatCircle.DistAndDir(-35.84, 94, 35.87, -85, out dist, out dir); GreatCircle.DistAndDir(-35.840000001, 94, 35.87, -85, out dist2, out dir2); Assert.True(dist2 > dist); GreatCircle.DistAndDir(-35.84, 94.000001, 35.87, -85, out dist2, out dir2); Assert.True(dist2 > dist); }
public void CalculatesGreatCircle_NoDeltaAlt() { // Arrange var orig = _landsEnd; var dest = _JohnoGroats; var expectedDistance = 968853.54671313858d; var expectedIntialBearing = 9.1198181045040769d; // Act var route = new GreatCircle(orig, dest); // Assert Assert.That( route.DeltaASL, Is.EqualTo(0).Within(0.0001), "Miscalculated altitude difference"); Assert.That( route.DistanceAtDestAlt, Is.EqualTo(expectedDistance).Within(0.0001), "Miscalculated distance at destination's altitude"); Assert.That( route.DistanceAtOrigAlt, Is.EqualTo(expectedDistance).Within(0.0001), "Miscalculated distance at origin's altitude"); Assert.That( route.DistanceAtSeaLevel, Is.EqualTo(expectedDistance).Within(0.0001), "Miscalculated distance at sea level"); Assert.That( route.DistanceWithAltChange, Is.EqualTo(expectedDistance).Within(0.0001), "Miscalculated distance including altitude difference"); Assert.That( route.ForwardAzimuth, Is.EqualTo(expectedIntialBearing).Within(0.0001), "Miscalulated initial bearing"); }
private void textBox_TextChanged(object sender, TextChangedEventArgs e) { var latitude = Regex.Match(textBox.Text, @"(?<=Latitude\s=\s)[-\.\d]+").Value; var longitude = Regex.Match(textBox.Text, @"(?<=Longitude\s=\s)[-\.\d]+").Value; var altitude = Regex.Match(textBox.Text, @"(?<=Altitude\s=\s)[-\.\d]+").Value; if ((latitude != string.Empty) && (longitude != string.Empty) && (altitude != string.Empty)) { var coordinates = new WorldCoordinates( double.Parse(latitude), double.Parse(longitude), double.Parse(altitude), 600000); var gc = new GreatCircle(_flagPole, coordinates); var output = $" ForwardAzimuth = {gc.ForwardAzimuth}\n Distance = {gc.DistanceAtOrigAlt} \n DeltaAltitude = {gc.DeltaASL}"; Clipboard.SetText(output); outputBlock1.Text = $"copied to clipboard:\n{output}"; } else { outputBlock1.Text = "Could not extract Latitude, Longitude, and Altitude"; } }
public void GreatCircle_Travel_to_WorldCoordinate() { // Arrange var orig = _KscFlagpole; var dest = _VAB; // Act var route = new GreatCircle(orig, dest); var endpoint = orig.Travel(route); // Assert Assert.That( endpoint.Latitude, Is.EqualTo(dest.Latitude).Within(0.0001), "Unacceptable variance in calculated endpoint latitude"); Assert.That( endpoint.Longitude, Is.EqualTo(dest.Longitude).Within(0.0001), "Unacceptable variance in calculated endpoint longitude"); Assert.That( endpoint.Altitude, Is.EqualTo(dest.Altitude).Within(0.0001), "Unacceptable variance in calculated endpoint altitude"); }
public void GreatCircleDeltas() { GeographicPosition p1 = new GeographicPosition(0, 1, 0); GeographicPosition p2 = new GeographicPosition(0, 2, 0); GeographicPosition p3 = new GeographicPosition(1, 1, 0); Length distance = p1.DistanceTo(p2); Angle direction; Assert.Equal(111319.49079327357, distance.Meters, 10); direction = p1.DirectionTo(p2); Assert.Equal(90, direction.Degrees, 10); distance = p1.DistanceTo(p3); Assert.Equal(110574.38855780043, distance.Meters, 10); p1 = new GeographicPosition(89.9999999, 0, 0); p2 = new GeographicPosition(-89.999, 0, 0); GreatCircle.DistAndDir(p1, p2, out distance, out direction); Assert.Equal(20003.8197534766, distance.Kilometers, 10); Assert.Equal(180, direction.Degrees, 5); p1 = new GeographicPosition(0.00002, 90.99, 0); p2 = new GeographicPosition(0.00001, -89.99, 0); GreatCircle.DistAndDir(p1, p2, out distance, out direction); Assert.Equal(19928.415241680563, distance.Kilometers, 10); Assert.Equal(89.9954651234, direction.Degrees, 10); p1 = new GeographicPosition(45, 9, 0); p2 = new GeographicPosition(45, 10, 90); GreatCircle.DistAndDir(p1, p2, out distance, out direction); Assert.Equal(78.84633470979473, distance.Kilometers, 10); Assert.Equal(89.6464421068, direction.Degrees, 10); }
/// <summary> /// Get the current position from the latest message containing any of the relevant data parts. /// If <paramref name="extrapolate"></paramref> is true, the speed and direction are used to extrapolate the position (many older /// GNSS receivers only deliver the position at 1Hz or less) /// </summary> /// <param name="position">Current position</param> /// <param name="extrapolate">True to extrapolate the current position using speed and track</param> /// <param name="track">Track (course over ground)</param> /// <param name="sog">Speed over ground</param> /// <param name="heading">Vessel Heading</param> /// <returns>True if a valid position is returned</returns> public bool TryGetCurrentPosition(out GeographicPosition?position, bool extrapolate, out Angle track, out Speed sog, out Angle?heading) { // Try to get any of the position messages var gll = (PositionFastUpdate?)GetLastSentence(PositionFastUpdate.Id); var gga = (GlobalPositioningSystemFixData?)GetLastSentence(GlobalPositioningSystemFixData.Id); var rmc = (RecommendedMinimumNavigationInformation?)GetLastSentence(RecommendedMinimumNavigationInformation.Id); var vtg = (TrackMadeGood?)GetLastSentence(TrackMadeGood.Id); var hdt = (HeadingTrue?)GetLastSentence(HeadingTrue.Id); TimeSpan age; List <(GeographicPosition, TimeSpan)> orderablePositions = new List <(GeographicPosition, TimeSpan)>(); if (gll != null && gll.Position != null) { orderablePositions.Add((gll.Position, gll.Age)); } if (gga != null && gga.Valid) { orderablePositions.Add((gga.Position, gga.Age)); } if (rmc != null && rmc.Valid) { orderablePositions.Add((rmc.Position, rmc.Age)); } if (orderablePositions.Count == 0) { // No valid positions received position = null; track = Angle.Zero; sog = Speed.Zero; heading = null; return(false); } (position, age) = orderablePositions.OrderBy(x => x.Item2).Select(x => (x.Item1, x.Item2)).First(); if (gga != null && gga.EllipsoidAltitude.HasValue) { // If we had seen a gga message, use its height, regardless of which other message provided the position position = new GeographicPosition(position.Latitude, position.Longitude, gga.EllipsoidAltitude.Value); } if (rmc != null) { sog = rmc.SpeedOverGround; track = rmc.TrackMadeGoodInDegreesTrue; } else if (vtg != null) { sog = vtg.Speed; track = vtg.CourseOverGroundTrue; } else { sog = Speed.Zero; track = Angle.Zero; heading = null; return(false); } if (hdt != null) { heading = hdt.Angle; } else { heading = null; } if (extrapolate) { position = GreatCircle.CalcCoords(position, track, sog * age); } return(true); }
/// <summary> /// Navigation loop. /// </summary> internal void CalculateNewStatus(int loops, DateTimeOffset now) { bool passedWp = false; RecommendedMinimumNavToDestination?currentLeg = null; if (_cache.TryGetLastSentence(RecommendedMinimumNavToDestination.Id, out RecommendedMinimumNavToDestination currentLeg1) && currentLeg1.Valid) { passedWp = currentLeg1.Arrived; if (_selfNavMode) { // Reset navigation _manualNextWaypoint = null; _selfNavMode = false; } OperationState = AutopilotErrorState.OperatingAsSlave; currentLeg = currentLeg1; } else { // So we have to test only one condition currentLeg = null; } if (_activeDeviation == null || loops % 100 == 0) { if (!_cache.TryGetLastSentence(HeadingAndDeclination.Id, out HeadingAndDeclination deviation) || !deviation.Declination.HasValue) { if (!_cache.TryGetLastSentence(RecommendedMinimumNavigationInformation.Id, out RecommendedMinimumNavigationInformation rmc) || !rmc.MagneticVariationInDegrees.HasValue) { if (loops % LogSkip == 0) { _logger.LogWarning("Autopilot: No magnetic variance"); } return; } deviation = new HeadingAndDeclination(Angle.Zero, Angle.Zero, rmc.MagneticVariationInDegrees); } _activeDeviation = deviation; } if (_cache.TryGetCurrentPosition(out var position, out Angle track, out Speed sog, out Angle? heading) && position != null) { string previousWayPoint = string.Empty; string nextWayPoint = string.Empty; if (currentLeg != null) { previousWayPoint = currentLeg.PreviousWayPointName; nextWayPoint = currentLeg.NextWayPointName; } List <RoutePoint>?currentRoute = null; if (_activeRoute != null) { currentRoute = _activeRoute.Points; } RoutePoint?next; // This returns RoutePresent if at least one valid waypoint is in the list if (currentRoute == null && _cache.TryGetCurrentRoute(out currentRoute) != AutopilotErrorState.RoutePresent) { // No route. But if we have an RMB message, there could still be a current target (typically one that was // directly selected with "Goto") if (currentLeg == null) { OperationState = AutopilotErrorState.NoRoute; return; } OperationState = AutopilotErrorState.DirectGoto; next = new RoutePoint("Goto", 0, 1, currentLeg.NextWayPointName, currentLeg.NextWayPoint, null, null); } else if (currentLeg != null) { // Better to compare by position rather than name, because the names (unless using identifiers) may // not be unique. next = currentRoute.FirstOrDefault(x => x.Position.EqualPosition(currentLeg.NextWayPoint)); } else { if (_manualNextWaypoint == null) { next = currentRoute.First(); _manualNextWaypoint = next; } else if (!HasPassedWaypoint(position, track, ref _manualNextWaypoint, currentRoute)) { next = _manualNextWaypoint; } else { passedWp = true; next = _manualNextWaypoint; if (next == null) // reached end of route { currentRoute = null; } } OperationState = AutopilotErrorState.OperatingAsMaster; } if (next != null && next.Position != null && (_knownNextWaypoint == null || next.Position.EqualPosition(_knownNextWaypoint.Position) == false)) { // the next waypoint changed. Set the new origin (if previous is undefined) // This means that either the user has selected a new route or we moved to the next leg. _knownNextWaypoint = next; _currentOrigin = null; } RoutePoint?previous = null; if (currentRoute != null) { previous = currentRoute.Find(x => x.WaypointName == previousWayPoint); } if (previous == null && next != null) { if (_currentOrigin != null) { previous = _currentOrigin; } else { // Assume the current position is the origin GreatCircle.DistAndDir(position, next.Position !, out Length distance, out Angle direction); _currentOrigin = new RoutePoint("Goto", 1, 1, "Origin", position, direction, distance); previous = _currentOrigin; } } else { // We don't need that any more. Reinit when previous is null again _currentOrigin = null; } if (next == null) { // No position for next waypoint OperationState = AutopilotErrorState.InvalidNextWaypoint; NextWaypoint = null; // Note: Possibly reached destination return; } Length distanceToNext = Length.Zero; Length distanceOnTrackToNext = Length.Zero; Length crossTrackError = Length.Zero; Length distancePreviousToNext = Length.Zero; Angle bearingCurrentToDestination = Angle.Zero; Angle bearingOriginToDestination = Angle.Zero; GeographicPosition nextPosition = new GeographicPosition(); Speed approachSpeedToWayPoint = Speed.Zero; if (next.Position != null) { nextPosition = next.Position; GreatCircle.DistAndDir(position, next.Position, out distanceToNext, out bearingCurrentToDestination); approachSpeedToWayPoint = GreatCircle.CalculateVelocityTowardsTarget(next.Position, position, sog, track); // Either the last waypoint or "origin" if (previous != null && previous.Position != null) { GreatCircle.DistAndDir(previous.Position, next.Position, out distancePreviousToNext, out bearingOriginToDestination); GreatCircle.CrossTrackError(previous.Position, next.Position, position, out crossTrackError, out distanceOnTrackToNext); } } NextWaypoint = next; List <NmeaSentence> sentencesToSend = new List <NmeaSentence>(); RecommendedMinimumNavToDestination rmb = new RecommendedMinimumNavToDestination(now, crossTrackError, previousWayPoint, nextWayPoint, nextPosition, distanceToNext, bearingCurrentToDestination, approachSpeedToWayPoint, passedWp); CrossTrackError xte = new CrossTrackError(crossTrackError); Angle variation = _activeDeviation.Declination.GetValueOrDefault(Angle.Zero); TrackMadeGood vtg = new TrackMadeGood(track, AngleExtensions.TrueToMagnetic(track, variation), sog); BearingAndDistanceToWayPoint bwc = new BearingAndDistanceToWayPoint(now, nextWayPoint, nextPosition, distanceToNext, bearingCurrentToDestination, AngleExtensions.TrueToMagnetic(bearingCurrentToDestination, variation)); BearingOriginToDestination bod = new BearingOriginToDestination(bearingOriginToDestination, AngleExtensions.TrueToMagnetic( bearingOriginToDestination, variation), previousWayPoint, nextWayPoint); sentencesToSend.AddRange(new NmeaSentence[] { rmb, xte, vtg, bwc, bod }); if (loops % 2 == 0) { // Only send these once a second IEnumerable <RoutePart> rte; IEnumerable <Waypoint> wpt; if (currentRoute == null || currentRoute.Count == 0) { currentRoute = new List <RoutePoint>(); if (_currentOrigin != null) { currentRoute.Add(_currentOrigin); } if (next.Position != null) { currentRoute.Add(next); } } // This should actually always contain at least two points now (origin and current target) if (currentRoute.Count > 0) { CreateRouteMessages(currentRoute, out rte, out wpt); sentencesToSend.AddRange(wpt); sentencesToSend.AddRange(rte); } } _output.SendSentences(sentencesToSend); } }
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); }
/// <summary> /// Creates a new instance of the <see cref="VoronoiCell"/> class with the given center and sides. /// </summary> /// <param name="center">The point used as center for Voronoi calculations. Assumed to be inside.</param> /// <param name="firstPart">The <see cref="GreatCircle"/> that makes the first part of the Polygon.</param> public VoronoiCell(SphereCoordinate center, GreatCircle firstPart) { Center = center; FirstPart = firstPart; }
private static bool TestDistAndDir(double startlat, double startlon, double endlat, double endlon) { double dist = 0; double dir = 0; GreatCircle.DistAndDir(startlat, startlon, endlat, endlon, out dist, out dir); Angle dirNormalized = Angle.FromDegrees(dir).Normalize(true); if (endlon >= 180) { endlon -= 360; } if (endlon < -180) { endlon += 360; } Assert.True(dist < 30000000, "Distance more than 1/2 times around the earth"); Assert.True(dirNormalized.Degrees >= 0 && dirNormalized.Degrees < 360, "Angle out of bounds"); double reslat = 0; double reslon = 0; GreatCircle.CalcCoords(startlat, startlon, dir, dist, out reslat, out reslon); Angle lon = Angle.FromDegrees(reslon); lon = lon.Normalize(false); Assert.True(reslat >= -90 && reslat <= 90, "Invalid latitude"); Assert.True(lon.Degrees > -180, $"Invalid longitude A {reslon}"); Assert.True(lon.Degrees <= 180, $"Invalid longitude B {reslon}"); double accuracy = 1E-7; if (dist > 100000) { accuracy = 1E-6; } if (dist > 1000000) { accuracy = 1E-6; } if (dist > 10000000) { accuracy = 1E-5; } if (Math.Abs(endlat - reslat) > accuracy) { Console.WriteLine("Error testing: " + startlat + "/" + startlon + " to " + endlat + "/" + endlon + " Distance: " + dist + " Difference in lat: " + (endlat - reslat)); return(false); } if (GreatCircle.AngleDifferenceSignedDegrees(endlon, lon.Degrees) > accuracy) { Console.WriteLine("Error testing: " + startlat + "/" + startlon + " to " + endlat + "/" + endlon + " Distance: " + dist + " Difference in lon: " + (endlon - reslon)); return(false); } return(true); }
public void TestCartesianToGC() { // start position of test double latStart = 47.0; double lonStart = 11.0; // 1 arcsec is about 30m => move in 0.3m steps along all 8 axis away from the start position double deltaInc = 0.01 / 3600.0; GeographicPosition pStart = new GeographicPosition(latStart, lonStart, 0); double distance = 0; double direction = 0; Length gcDist = Length.Zero; Angle gcDir; // iterate over the 8 axis (45° increments) for (int signIdx = 0; signIdx < 8; signIdx++) { double dblXSign = 1.0; if (signIdx >= 4 && signIdx <= 6) { dblXSign = -1.0; } else if (signIdx == 3 || signIdx == 7) { dblXSign = 0.0; } double dblYSign = 1.0; if (signIdx >= 2 && signIdx <= 4) { dblYSign = -1.0; } else if (signIdx == 1 || signIdx == 5) { dblYSign = 0.0; } // iterate in 0.3m steps (up to about 150m distance) for (int idx = 1; idx <= 400; idx++) { double delta = idx * deltaInc; GeographicPosition pEnd = new GeographicPosition(latStart + dblYSign * delta, lonStart + dblXSign * delta, 0); InternalDistDir(pStart, pEnd, ref distance, ref direction); GreatCircle.DistAndDir(pStart, pEnd, out gcDist, out gcDir); // compare the two calculation methods Assert.True(Math.Abs(distance - gcDist.Meters) < 1.0, "position accuracy less than 1m"); Assert.True(GreatCircle.AngleDifferenceSignedDegrees(direction, gcDir.Degrees) < 1.0, "direction accuracy less than 1 deg"); // calculate the endpoint with the previously calculated offsets using great circle double dblEndLat = 0; double dblEndLon = 0; GreatCircle.CalcCoords(pStart.Latitude, pStart.Longitude, gcDir.Degrees, gcDist.Meters, out dblEndLat, out dblEndLon); Assert.True(Math.Abs(dblEndLat - pEnd.Latitude) < 1.0, "GC latitude accuracy less than 1m"); Assert.True(GreatCircle.AngleDifferenceSignedDegrees(dblEndLon, pEnd.Longitude) < 1.0, "GC longitude accuracy less than 1m"); // calculate the endpoint with the previously calculated offsets using the cartesic routine GeographicPosition pCalcEnd = InternalExtrapolatePosition(pStart, distance, direction); double dblDeltaX = Math.Abs(pCalcEnd.Longitude - pEnd.Longitude) * GreatCircle.MetersPerDegreeLongitude; double dblDeltaY = Math.Abs(pCalcEnd.Latitude - pEnd.Latitude) * GreatCircle.MetersPerDegreeeLatitude; Assert.True(dblDeltaY < 1.0, "XY latitude accuracy less than 1m"); Assert.True(dblDeltaX < 1.0, "XY longitude accuracy less than 1m"); } } }