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