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