public RangeFinder(double a, RobotAgent o, double _max_range, double _noise)
{
Owner = o;
Angle = a;
MaxRange = _max_range;
DistanceToClosestObject = (-1);
}
/// <summary>
/// Default constructor.
/// </summary>
public Radar(double startAngle, double endAngle, RobotAgent owner, string type = "goal", double maxRange = 150)
{
Owner = owner;
StartAngle = startAngle;
EndAngle = endAngle;
MaxRange = maxRange;
Distance = (-1);
Noise = 0.0;
Type = type;
}
/// <summary>
/// Casts a ray from the robot's center point according to the specified Angle and returns the Distance to the closest object.
/// </summary>
/// <param name="Angle">Angle of sensor, in radians. Also see the "absolute" parameter.</param>
/// <param name="maxRange">Max Distance that the robot can see in front of itself.</param>
/// <param name="point">2D location of the robot's center.</param>
/// <param name="Owner">The currently active robot.</param>
/// <param name="hit">** Output variable ** - true if another object intersects the cast ray, false otherwise.</param>
/// <param name="absolute">If false, the Angle parameter is relative to the agent's Heading. Otherwise it follows the standard unit AreaOfImpact.</param>
/// <returns></returns>
public static double raycast(this IAgentSensor sensor, HardMaze maze, double angle, double maxRange, Point2D point, RobotAgent owner, out Wall hit, bool absolute = false)
{
hit = null;
Point2D casted = new Point2D(point);
double distance = maxRange;
// Cast point casted out from the robot's center point along the sensor direction
if (!absolute)
{
casted.X += Math.Cos(angle + owner.Heading) * distance;
casted.Y += Math.Sin(angle + owner.Heading) * distance;
}
else
{
casted.X += Math.Cos(angle) * distance;
casted.Y += Math.Sin(angle) * distance;
}
// Create line segment from robot's center to casted point
Line2D cast = new Line2D(point, casted);
// Now do naive detection of collision of casted rays with objects
// First for all walls
foreach (Wall wall in maze.walls)
{
if (!wall.Visible)
{
continue;
}
bool found = false;
Point2D intersection = wall.Line.intersection(cast, out found);
if (found)
{
double new_distance = intersection.distance(point);
if (new_distance < distance)
{
distance = new_distance;
hit = wall;
}
}
}
return(distance);
}