/// <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>
/// 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(List<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)
{
// Initialize the intersection indicator to false
var intersectionFound = false;
// Get the intersection point between wall and projected trajectory
// (if one exists)
var wallIntersectionPoint = DoubleLine.CalculateLineIntersection(wall.WallLine, projectedLine,
out intersectionFound);
// If trajectory intersects with a wall, adjust the range to the point
// of intersection (as the range finder cannot penetrate walls)
if (intersectionFound)
{
// 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>
/// Updates each radar in the array based on the given navigator heading and the goal location.
/// </summary>
/// <param name="heading">The heading of the navigator to which the radar array is attached.</param>
/// <param name="location">The location of the goal.</param>
internal void UpdateRadarArray(double heading, DoublePoint location)
{
var target = new DoublePoint(location.X, location.Y);
// Rotate the target with respect to the heading of the navigator
target.RotatePoint(-heading, location);
// Offset by the navigator's current location
target.X -= location.X;
target.Y -= location.Y;
// Get the angle between the navigator and the target
var navigatorTargetAngle = DoublePoint.CalculateAngleFromOrigin(target);
// Update every radar in the array based on target alignment
foreach (var radar in Radars)
{
radar.updateRadar(navigatorTargetAngle);
}
}
