//TODO: port to radar sensor class
public static double scanCone(Radar rf, List<SimulatorObject> objList)
{
double distance = rf.max_range;
//hitRobot = false;
double new_distance;
double heading=rf.owner.heading;
Point2D point=new Point2D(rf.owner.location.x+rf.offsetx,rf.owner.location.y+rf.offsety);
double startAngle = rf.startAngle + heading;
double endAngle = rf.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;
}
// if (agentsVisible)
foreach (SimulatorObject obj in objList)
{
bool found = false;
if (obj == rf.owner)
continue;
new_distance = point.distance(obj.location);
// if (new_distance - obj.radius <= rf.max_range) //TODO do we need this
//{
//TODO before: double angle = Math.Atan2(robot2.circle.p.y - point.y, robot2.circle.p.x - point.x);
double angle = Math.Atan2(obj.location.y - point.y, obj.location.x - point.x);
if (angle < 0)
{
angle += Utilities.twoPi;
}
if (endAngle < startAngle)//sensor spans the 0 line
{
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;
// hitRobot = true;
}
}
}
return distance;
}
//TODO: port to radar sensor class
public static double scanCone(Radar rf, List <SimulatorObject> objList)
{
double distance = rf.max_range;
//hitRobot = false;
double new_distance;
double heading = rf.owner.heading;
Point2D point = new Point2D(rf.owner.location.x + rf.offsetx, rf.owner.location.y + rf.offsety);
double startAngle = rf.startAngle + heading;
double endAngle = rf.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;
}
// if (agentsVisible)
foreach (SimulatorObject obj in objList)
{
bool found = false;
if (obj == rf.owner)
{
continue;
}
new_distance = point.distance(obj.location);
// if (new_distance - obj.radius <= rf.max_range) //TODO do we need this
//{
//TODO before: double angle = Math.Atan2(robot2.circle.p.y - point.y, robot2.circle.p.x - point.x);
double angle = Math.Atan2(obj.location.y - point.y, obj.location.x - point.x);
if (angle < 0)
{
angle += Utilities.twoPi;
}
if (endAngle < startAngle) //sensor spans the 0 line
{
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;
// hitRobot = true;
}
}
}
return(distance);
}
