/// <summary>
/// Checks the conic area defined by a radar sensor for detectable objects.
/// </summary>
/// <returns>Distance to the closest object.</returns>
public static double scanCone(Radar radar, List<SimulatorObject> objList)
{
double distance = radar.MaxRange;
double new_distance;
double heading=radar.Owner.Heading;
Point2D point=new Point2D(radar.Owner.Location.X,radar.Owner.Location.Y);
double startAngle = radar.StartAngle + heading;
double endAngle = radar.EndAngle + heading;
double twoPi = 2 * Math.PI;
if (startAngle < 0)
{
startAngle += twoPi;
}
else if (startAngle > twoPi)
{
startAngle -= twoPi;
}
if (endAngle < 0)
{
endAngle += twoPi;
}
else if (endAngle > twoPi)
{
endAngle -= twoPi;
}
foreach (SimulatorObject obj in objList)
{
bool found = false;
if (obj == radar.Owner)
continue;
new_distance = point.distance(obj.Location);
double angle = Math.Atan2(obj.Location.Y - point.Y, obj.Location.X - point.X);
if (angle < 0)
{
angle += Utilities.twoPi;
}
if (endAngle < startAngle)
{
if ((angle >= startAngle && angle <= Math.PI * 2) || (angle >= 0 && angle <= endAngle))
{
found = true;
}
}
else if ((angle >= startAngle && angle <= endAngle))
found = true;
if (found)
{
if (new_distance < distance)
{
distance = new_distance;
}
}
}
return distance;
}
/// <summary>
/// Checks the conic area defined by a radar sensor for detectable objects.
/// </summary>
/// <returns>Distance to the closest object.</returns>
public static double scanCone(Radar radar, List <SimulatorObject> objList)
{
double distance = radar.MaxRange;
double new_distance;
double heading = radar.Owner.Heading;
Point2D point = new Point2D(radar.Owner.Location.X, radar.Owner.Location.Y);
double startAngle = radar.StartAngle + heading;
double endAngle = radar.EndAngle + heading;
double twoPi = 2 * Math.PI;
if (startAngle < 0)
{
startAngle += twoPi;
}
else if (startAngle > twoPi)
{
startAngle -= twoPi;
}
if (endAngle < 0)
{
endAngle += twoPi;
}
else if (endAngle > twoPi)
{
endAngle -= twoPi;
}
foreach (SimulatorObject obj in objList)
{
bool found = false;
if (obj == radar.Owner)
{
continue;
}
new_distance = point.distance(obj.Location);
double angle = Math.Atan2(obj.Location.Y - point.Y, obj.Location.X - point.X);
if (angle < 0)
{
angle += Utilities.twoPi;
}
if (endAngle < startAngle)
{
if ((angle >= startAngle && angle <= Math.PI * 2) || (angle >= 0 && angle <= endAngle))
{
found = true;
}
}
else if ((angle >= startAngle && angle <= endAngle))
{
found = true;
}
if (found)
{
if (new_distance < distance)
{
distance = new_distance;
}
}
}
return(distance);
}
/// <summary>
/// Calculates the length of this line segment
/// </summary>
public double length()
{
return(Endpoint1.distance(Endpoint2));
}
/// <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 override double raycast(double angle, double maxRange, Point2D point, Robot owner, out SimulatorObject 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 Domain.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;
}
}
}
// Then for all robots
if (!AgentVisible)
{
return(distance);
}
foreach (Robot robot2 in Robots)
{
bool found = false;
if (robot2 == owner)
{
continue;
}
double new_distance = cast.nearestIntersection(robot2.AreaOfImpact, out found);
if (found)
{
if (new_distance < distance)
{
distance = new_distance;
hit = robot2;
}
}
}
return(distance);
}
