/// <summary>
/// Wall constructor which accepts the wall line as well as heading adjustments.
/// </summary>
/// <param name="wallLine">The 2D line segment representing the start and end point of the wall.</param>
/// <param name="leftApproachAdjustmentCoefficient">
/// The direction in which to adjust the heading given an approach from the
/// west.
/// </param>
/// <param name="rightApproachAdjustmentCoefficient">
/// The direction in which to adjust the heading given an approach from the
/// east.
/// </param>
/// <param name="headingAdjustmentMagnitude">The "amount" (magnitude) in degrees by which the heading should be adjusted.</param>
public Wall(DoubleLine wallLine, int leftApproachAdjustmentCoefficient, int rightApproachAdjustmentCoefficient,
int headingAdjustmentMagnitude)
{
WallLine = wallLine;
_leftApproachAdjustment = leftApproachAdjustmentCoefficient*headingAdjustmentMagnitude;
_rightApproachAdjustment = rightApproachAdjustmentCoefficient*headingAdjustmentMagnitude;
}
/// <summary>
/// Determines whether the newly proposed location will result in a collision.
/// </summary>
/// <param name="newLocation">The location to which the navigator is prepped to move.</param>
/// <param name="walls">The list of walls in the environment.</param>
/// <param name="collidingWall">Output parameter recording the wall at which the collision would occur.</param>
/// <returns>Whether or not the proposed move will result in a collision.</returns>
private static bool IsCollision(DoublePoint newLocation, IList <Wall> walls, out Wall collidingWall)
{
var doesCollide = false;
collidingWall = null;
// Iterate through all of the walls, determining if the traversal to the
// newly proposed location will result in a collision
foreach (var wall in walls)
{
// If the distance between the wall and the new location is less than
// the radius of the navigator itself, then a collision will occur
if (!(DoubleLine.CalculateEuclideanDistanceFromLineToPoint(wall.WallLine, newLocation) < Radius))
{
continue;
}
// If we made it here, there is a collision, so set the flag, record the colliding wall, and break
doesCollide = true;
collidingWall = wall;
break;
}
return(doesCollide);
}
/// <summary>
/// Wall constructor which accepts the wall line as well as heading adjustments.
/// </summary>
/// <param name="wallLine">The 2D line segment representing the start and end point of the wall.</param>
/// <param name="leftApproachAdjustmentCoefficient">
/// The direction in which to adjust the heading given an approach from the
/// west.
/// </param>
/// <param name="rightApproachAdjustmentCoefficient">
/// The direction in which to adjust the heading given an approach from the
/// east.
/// </param>
/// <param name="headingAdjustmentMagnitude">The "amount" (magnitude) in degrees by which the heading should be adjusted.</param>
public Wall(DoubleLine wallLine, int leftApproachAdjustmentCoefficient, int rightApproachAdjustmentCoefficient,
int headingAdjustmentMagnitude)
{
WallLine = wallLine;
_leftApproachAdjustment = leftApproachAdjustmentCoefficient * headingAdjustmentMagnitude;
_rightApproachAdjustment = rightApproachAdjustmentCoefficient * headingAdjustmentMagnitude;
}
/// <summary>
/// Calculates the adjusted heading based on the current position in relation to the colliding wall.
/// </summary>
/// <param name="currentHeading">The current heading of the navigator.</param>
/// <param name="currentPosition">The current position of the navigator.</param>
/// <returns>The updated navigator heading.</returns>
public double CalculateAdjustedHeading(double currentHeading, DoublePoint currentPosition)
{
double adjustedHeading = 0;
// Calculate the closest point on the colliding wall from the current position
DoublePoint closestPointOnWall = DoubleLine.CalculateLineSegmentClosestPoint(WallLine, currentPosition);
// Adjust the heading based on whether the navigator is approaching from the left or the right
adjustedHeading = currentPosition.X < closestPointOnWall.X
? currentHeading + _leftApproachAdjustment
: currentHeading + _rightApproachAdjustment;
// If the navigator's resulting heading is greater than 360 degrees,
// it has performed more than a complete rotation, so subtract 360
// degrees to have a valid heading
if (adjustedHeading > 360)
{
adjustedHeading -= 360;
}
// On the other hand, if the heading is negative, the same has happened
// in the other direction. So add 360 degrees
else if (adjustedHeading < 0)
{
adjustedHeading += 360;
}
return(adjustedHeading);
}
/// <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>
/// Wall constructor which accepts online the line definition.
/// </summary>
/// <param name="wallLine">The line representing a wall in the maze.</param>
public Wall(DoubleLine wallLine)
: this(wallLine, 0, 0, 0)
{
}
/// <summary>
/// Wall constructor which accepts online the line definition.
/// </summary>
/// <param name="wallLine">The line representing a wall in the maze.</param>
public Wall(DoubleLine wallLine) : this(wallLine, 0, 0, 0)
{
}
