/// <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) { }