public void ShouldHaveConsistentDistance(
[Random(-1000, 1000.0, 5)] double lat,
[Random(-1000.0, 1000.0, 5)] double lon,
[Random(-360.0, 360.0, 10)] double bearing,
[Random(0.0, 1000.0, 10)] double distance)
{
var sourceGeo = new Geo(lat, lon);
var destGeo = sourceGeo.Offset(Geo.KilometersToRadians(distance), Geo.DegreesToRadians(bearing));
Assert.That(sourceGeo.DistanceKilometers(destGeo), Is.EqualTo(distance).Within(0.0003));
}
/// <summary>
/// Bounce a platform around inside a given perimeter
/// </summary>
/// <param name="perimeter">A GeoArray of points in the perimeter, which must satisfy certain conditions (no segments may cross and the polygon
/// must be closed)</param>
/// <param name="startPoint">A Geo containing the starting point. If this is null, a point inside the perimeter is chosen randomly</param>
/// <param name="initialCourse">The initial course, in radians from true north. If startPoint is null, or if initialCourse is NaN, this is
/// chosen randomly</param>
/// <param name="minimumCourseLength">The minimum length of the returned course, in meters</param>
/// <returns>A GeoArray containing the start location and each point where the course bounces off the perimeter</returns>
public static GeoArray PerimeterBounce(this GeoArray perimeter, Geo startPoint, double initialCourse, double minimumCourseLength)
{
if (!perimeter.IsClosed) throw new InvalidPerimeterException("Perimeter is not closed");
if (perimeter.HasCrossingSegments) throw new InvalidPerimeterException("Perimeter is not a simple polygon (segments cross each other)");
var points = new List<Geo>();
while ((startPoint == null) || (!startPoint.IsInside(perimeter)))
{
var distance = Random.NextDouble() * perimeter.BoundingCircle.Radius;
var azimuth = Random.NextDouble() * MoreMath.TwoPi;
startPoint = perimeter.Center.Offset(distance, azimuth);
}
points.Add(startPoint);
if (double.IsNaN(initialCourse)) initialCourse = Random.NextDouble()*MoreMath.TwoPi;
var courseSegment = new GeoSegment(startPoint, startPoint.Offset(Geo.KilometersToRadians(0.001), initialCourse)).Scale(1000 * Geo.RadiansToKilometers(MoreMath.PiOverTwo));
var bounceCount = 1;
while (minimumCourseLength > 0)
{
var minIntersectionRange = double.MaxValue;
GeoSegment firstIntersectingSegment = null;
Geo firstIntersection = null;
foreach (var segment in perimeter.Segments)
{
var intersection = courseSegment.Intersection(segment);
if (intersection == null) continue;
var curIntersectionRange = startPoint.DistanceRadians(intersection);
if (curIntersectionRange < Geo.KilometersToRadians(0.001) || curIntersectionRange >= minIntersectionRange) continue;
minIntersectionRange = curIntersectionRange;
firstIntersectingSegment = segment;
firstIntersection = intersection;
}
if (firstIntersection == null) throw new PerimeterBounceException(string.Format("Course segment failed to intersect the perimeter on bounce {0}", bounceCount));
var actualCourseSegment = new GeoSegment(points.Last(), firstIntersection);
minimumCourseLength -= Geo.RadiansToKilometers(actualCourseSegment.LengthRadians) * 1000;
points.Add(firstIntersection);
var reflectedSegment = courseSegment.Reflect(firstIntersectingSegment);
var reflectionAzimuth = reflectedSegment[0].Azimuth(reflectedSegment[1]);
courseSegment = new GeoSegment(reflectedSegment[1], 1000 * Geo.RadiansToKilometers(MoreMath.PiOverTwo), Geo.RadiansToDegrees(reflectionAzimuth));
bounceCount++;
}
return new GeoArray(points);
}
public void Inflations()
{
var rect = new GeoRect(44, 41, -69, -72);
var nw = new Geo(rect.North, rect.West,true);
var se = new Geo(rect.South, rect.East,true);
var kmToInflate = 10.0f;
nw = nw.Offset(Geo.KilometersToRadians(Math.Sqrt(2) * kmToInflate), Geo.DegreesToRadians(315));
se = se.Offset(Geo.KilometersToRadians(Math.Sqrt(2) * kmToInflate), Geo.DegreesToRadians(135));
var rect1 = new GeoRect(nw.Latitude,se.Latitude,se.Longitude,nw.Longitude);
var inflatedRect = GeoRect.Inflate(rect, kmToInflate);
Assert.IsTrue(rect1.Equals(inflatedRect));
}
/// <summary>
/// Takes a start location and a proposed end location, and returns a reflected end location
/// If the proposed end location is outside of the current figure, the end location is reflected
/// by the normal to the intersecting segment, and returned to the caller. If the proposed end
/// location is inside the current figure, then it is simply returned to the caller.
/// </summary>
/// <param name="startLocation">Start location for the current proposed move</param>
/// <param name="proposedEndLocation">End location for the current proposed move</param>
/// <param name="proposedCourse"> </param>
/// <returns>Actual end location that remains inside the figure, which may be reflected from the
/// proposed end location provided if the proposed end location lies outside the figure</returns>
public override Geo Reflect(Geo startLocation, Geo proposedEndLocation, out Course proposedCourse)
{
//Geo start = new Geo(StartLocation);
//Geo end = new Geo(ProposedEndLocation);
#if MATLAB_DEBUG_OUTPUT
MatlabDumpVertices();
#endif
proposedCourse = new Course(startLocation, proposedEndLocation);
if (Contains(proposedEndLocation))
return proposedEndLocation;
//start.Move(ProposedCourse.ReciprocalDegrees, 1000);
//end.Move(ProposedCourse.Degrees, 1000);
#if MATLAB_DEBUG_OUTPUT
Console.WriteLine("Course=zeros(2,2);\nCourse(1,:)=[{0} {1}];\nCourse(2,:)=[{2} {3}];",
startLocation.Longitude, startLocation.Latitude,
proposedEndLocation.Longitude, proposedEndLocation.Latitude);
#endif
var proposedCourseSegment = new OverlayLineSegment(startLocation, proposedEndLocation);
for (var i = 0; i < Segments.Count(); i++)
{
var intersect = proposedCourseSegment.IntersectionPoint(Segments[i]);
#if MATLAB_DEBUG_OUTPUT
Console.WriteLine("Intersects({0},:)=[{1} {2}];", i + 1, intersect.Longitude, intersect.Latitude);
#endif
//if (intersect == null) continue;
if (!proposedCourseSegment.Contains(intersect) || !Segments[i].Contains(intersect)) continue;
proposedCourse = proposedCourse.Reflect(Normals[i]);
var distance = startLocation.DistanceKilometers(proposedEndLocation);
var result = startLocation.Offset(Geo.KilometersToRadians(distance), proposedCourse.Radians);
return result;
}
#if MATLAB_DEBUG_OUTPUT
Console.WriteLine("figure;");
Console.WriteLine("plot(Vertices(:, 1), Vertices(:, 2), 'g-*');");
Console.WriteLine("hold on;");
Console.WriteLine("plot(Course(:, 1), Course(:, 2), 'r-o');");
Console.WriteLine("plot(Intersects(:, 1), Intersects(:, 2), 'bx');");
Console.WriteLine("legend('Area Boundary', 'Course Segment under review', 'Calculated intersection points');");
#endif
throw new GeometricException("The proposed course didn't intersect with any of the edges of the figure");
}
void MovementModel()
{
var timestepCount = (int) NemoPlatform.NemoScenario.Duration.DivideBy(NemoBase.SimulationStepTime);
var currentTime = NemoPlatform.NemoScenario.StartTime;
PlatformStates = new PlatformStates();
NemoTrackdef curTrackdef = null;
var curLocation = new Geo();
Course course = null;
//double curCourseDegrees = 0;
double curSpeedMetersPerSecond = 0;
var overlayPoints = new List<Geo>();
OverlayShape curTrackBoundingRegion = null;
CourseChangePoints = new List<CourseChangeDatum>();
overlayPoints.Add(new Geo(NemoPlatform.Trackdefs[0].InitialLocation));
// Put trackdefs in ascending start-time order, if they weren't already
NemoPlatform.Trackdefs.Sort();
for (var i = 0; i < timestepCount; i++, currentTime += NemoBase.SimulationStepTime)
{
// if we have a current trackdef we're processing, AND that current trackdef DOES NOT CONTAIN the current time
// THEN we no longer have a current trackdef
if ((curTrackdef != null) && (!curTrackdef.Contains(currentTime))) curTrackdef = null;
// if we don't have a current trackdef
if (curTrackdef == null)
{
// look through all of our trackdefs
foreach (var trackdef in NemoPlatform.Trackdefs) // If we find one that contains the current time
if (trackdef.Contains(currentTime))
{
// make this trackdef the current one
curTrackdef = trackdef;
curLocation = curTrackdef.InitialLocation;
course = new Course(curTrackdef.InitialCourse);
curSpeedMetersPerSecond = curTrackdef.InitialSpeed * 0.514444444f;
CourseChangePoints.Add(new CourseChangeDatum
{
IsStart = true,
Location = curLocation,
NewCourse = course.Degrees,
});
// Conversion factor for knots to meters per second
if (curTrackdef.OverlayFile != null)
{
if (curTrackdef.OverlayFile.Shapes.Count() != 1) throw new PlatformMovementException(string.Format("Specified overlay file {0} is unsuitable for use as a bounding region.\nReason(s): Overlay file contains multiple shapes, therefore the bounding shape is undefined", curTrackdef.OverlayFile.FileName));
curTrackBoundingRegion = curTrackdef.OverlayFile.Shapes[0];
#if true
if (!curTrackBoundingRegion.IsUsableAsPerimeter)
{
var reasons = new StringBuilder();
if (!curTrackBoundingRegion.IsClosed) reasons.Append("Bounding region is not closed, ");
if (curTrackBoundingRegion.HasCrossingSegments) reasons.Append("Bounding region is not a simple polygon (segments cross each other), ");
if (reasons.Length != 0) reasons.Remove(reasons.Length - 2, 2); // Remove the trailing ", "
throw new PlatformMovementException(string.Format("Specified overlay file {0} is unsuitable for use as a bounding region.\nReason(s): {1}", curTrackdef.OverlayFile.FileName, reasons));
}
#endif
if (!curTrackBoundingRegion.Contains(curLocation)) throw new PlatformMovementException(string.Format("Specified start location ({0:0.####}, {1:0.####}) is not contained within the trackdef bounding region", curLocation.Latitude, curLocation.Longitude));
}
else
{
// Else, the current trackdef's overlay file IS null, and if the type is perimeter_bounce, that's a no-no
if (curTrackdef.TrackType.ToLower() == "perimeter_bounce") throw new PlatformMovementException("PERIMETER_BOUNCE trackdefs require a bounding region, none was supplied.");
}
break;
}
}
// If we have a current trackdef, use it, otherwise we don't update the location or course
if (curTrackdef != null)
{
switch (curTrackdef.TrackType.ToLower())
{
default:
case "stationary":
curSpeedMetersPerSecond = 0;
break;
case "straight_line":
// straight line navigation code
curLocation = curLocation.Offset(Geo.KilometersToRadians((curSpeedMetersPerSecond * NemoBase.SimulationStepTime.TotalSeconds) / 1000), course.Radians);
break;
case "perimeter_bounce":
// perimeter bounce navigation code here
var proposedLocation = curLocation.Offset(Geo.KilometersToRadians((curSpeedMetersPerSecond * NemoBase.SimulationStepTime.TotalSeconds) / 1000), course.Radians);
//proposedLocation = new EarthCoordinate3D(curLocation);
//proposedLocation.Move(curCourseDegrees, curSpeedMetersPerSecond*NemoBase.SimulationStepTime.TotalSeconds);
if (curTrackBoundingRegion == null) throw new PlatformBehaviorException("Platform behavior is specified as Perimeter Bounce, but no bounding shape was specified");
if (curTrackBoundingRegion.Contains(proposedLocation)) curLocation = proposedLocation;
else
{
//curLocation.Compare(proposedLocation);
Course proposedCourse;
proposedLocation = new Geo(curTrackBoundingRegion.Reflect(curLocation, proposedLocation, out proposedCourse));
if (!curTrackBoundingRegion.Contains(proposedLocation))
{
proposedLocation = new Geo(curTrackBoundingRegion.Reflect(curLocation, proposedLocation, out proposedCourse));
if (!curTrackBoundingRegion.Contains(proposedLocation)) throw new PlatformMovementException("Two reflections failed to keep the platform inside the bounding region. Please check the bounding region closely for small pockets or other irregularities");
}
var newCourse = new Course(curLocation, proposedLocation);
CourseChangePoints.Add(new CourseChangeDatum
{
Location = curLocation,
OldCourse = course.Degrees,
NewCourse = newCourse.Degrees,
});
//curLocation.Compare(proposedLocation);
course = newCourse;
curLocation = new Geo(proposedLocation);
if (!curTrackBoundingRegion.Contains(curLocation)) throw new PlatformMovementException("Reflected position is outside the bounding region");
overlayPoints.Add(curLocation);
}
break;
}
}
else
// If we don't have a current trackdef, that means our speed is zero
curSpeedMetersPerSecond = 0;
// Put the current location, course, speed and time into the PlatformStates list
PlatformStates.Add(new PlatformLocation
{
Location = curLocation,
Course = (float) course.Degrees,
Speed = (float) curSpeedMetersPerSecond,
//SimulationTime = currentTime
});
}
overlayPoints.Add(new Geo(curLocation));
CourseOverlay = new OverlayLineSegments(overlayPoints.ToArray(), Colors.Orange, 1, LineStyle.Dot);
CourseEnd = new OverlayPoint(curLocation, Colors.Red, 2);
CourseChangePoints.Add(new CourseChangeDatum
{
IsEnd = true,
Location = curLocation,
OldCourse = course.Degrees,
});
CourseStart = new OverlayPoint(NemoPlatform.Trackdefs[0].InitialLocation, Colors.Green, 2);
}
