/// <summary>
/// Updates each of the range finders based on obstacles along their trajectory. This amounts to setting the output of
/// the range finder to either the distance to the nearest obstacle or, if there are no obstacles in its path, to the
/// maximum distance of the range finder.
/// </summary>
/// <param name="walls">The list of walls in the environment.</param>
/// <param name="heading">The heading of the navigator (in degrees).</param>
/// <param name="location">The location of the navigator in the environment.</param>
internal void Update(IList <Wall> walls, double heading, DoublePoint location)
{
// Convert rangefinder angle to radians
var radianAngle = MathUtils.ToRadians(_angle);
// Project a point from the navigator location outward
var projectedPoint = new DoublePoint(location.X + Math.Cos(radianAngle) * Range,
location.Y + Math.Sin(radianAngle) * Range);
// Rotate the point based on the navigator's heading
projectedPoint.RotatePoint(heading, location);
// Create a line segment from the navigator's current location to the
// projected point
var projectedLine = new DoubleLine(location, projectedPoint);
// Initialize the range to the maximum range of the range finder sensor
var adjustedRange = Range;
foreach (var wall in walls)
{
// Get the intersection point between wall and projected trajectory
// (if one exists)
var wallIntersectionPoint = DoubleLine.CalculateLineIntersection(wall.WallLine, projectedLine,
out var intersectionFound);
// Skip to next wall if there's no intersection for current one
if (!intersectionFound)
{
continue;
}
// Otherwise, if trajectory intersects with a wall, adjust the range to the point
// of intersection (as the range finder cannot penetrate walls)
// Get the distance from the wall
var wallRange = DoublePoint.CalculateEuclideanDistance(wallIntersectionPoint, location);
// If the current wall range is shorter than the current adjusted range,
// update the adjusted range to the shorter value
if (wallRange < adjustedRange)
{
adjustedRange = wallRange;
}
}
// Update the range finder range to be the adjusted range
Output = adjustedRange;
}
/// <summary>
/// Calculates the distance between the navigator's location and the goal location.
/// </summary>
/// <returns>The distance between the navigator and the goal.</returns>
public double GetDistanceToTarget()
{
// Get the distance to the target based on the navigator's current location
return(DoublePoint.CalculateEuclideanDistance(_navigator.Location, _goalLocation));
}
