/// <summary>
/// Default constructor.
/// </summary>
public Radar(double startAngle, double endAngle, Robot owner, string type = "goal", double maxRange = 150)
{
Owner = owner;
StartAngle = startAngle;
EndAngle = endAngle;
MaxRange = maxRange;
Distance = (-1);
Noise = 0.0;
Type = type;
}
public void setupRobots(string robotModel)
{
foreach (Type t in this.GetType().Assembly.GetTypes())
{
if (t.IsSubclassOf(typeof(Robot)))
{
robotModelComboBox.Items.Add(t.Name);
}
}
robot = RobotModelFactory.getRobotModel(robotModel);
if (robot != null)
{
for (int j = 0; j < robotModelComboBox.Items.Count; j++)
if (robotModelComboBox.Items[j].ToString().Equals(robot.Name))
{
robotModelComboBox.SelectedIndex = j;
break;
}
}
}
public NetworkVisualizerForm(Robot _selectedRobot, ModularNetwork _net)
{
_net.UpdateNetworkEvent += networkUpdated;
InitializeComponent();
net = _net;
selectedRobot = _selectedRobot;
this.Text = "Network Visualizer [z="+ selectedRobot.ZStack+"]";
SetBounds(1, 1, 320, 320);
brush = new SolidBrush(Color.Red);
penConnection = new Pen(Color.Black);
penConnection.StartCap = System.Drawing.Drawing2D.LineCap.Flat;
penConnection.EndCap = System.Drawing.Drawing2D.LineCap.ArrowAnchor;
startX = 1.1f * dtx;
startY = 1.1f * dty;
//set up double buffering
this.SetStyle(
ControlStyles.AllPaintingInWmPaint |
ControlStyles.UserPaint |
ControlStyles.DoubleBuffer, true);
}
/// <summary>
/// Tests collision between the specified robot and all walls and other robots in the CurrentEnvironment.
/// </summary>
public override bool robotCollide (Robot robot)
{
foreach (Wall wall in Domain.walls)
{
if (EngineUtilities.collide(robot, wall))
{
return true;
}
}
if(!AgentCollide) return false;
foreach (Robot otherRobot in Robots)
{
if (robot == otherRobot)
continue;
if (EngineUtilities.collide(robot, otherRobot))
{
return true;
}
}
return false;
}
private void functionComboBox_SelectedIndexChanged(object sender, EventArgs e)
{
robot = RobotModelFactory.getRobotModel(robotModelComboBox.SelectedItem.ToString());
}
/// <summary>
/// Calculates the squared Distance between two Robot objects.
/// </summary>
public static double squaredDistance(Robot r1, Robot r2)
{
return Math.Pow(r1.Location.X - r2.Location.X, 2) + Math.Pow(r1.Location.Y - r2.Location.Y, 2);
}
/// <summary>
/// Calculates the Euclidean Distance between two Robot objects.
/// </summary>
public static double euclideanDistance(Robot r1, Robot r2)
{
return Math.Sqrt(Math.Pow(r1.Location.X - r2.Location.X, 2) + Math.Pow(r1.Location.Y - r2.Location.Y, 2));
}
public static bool collide(Robot a, Robot b)
{
return a.AreaOfImpact.collide(b.AreaOfImpact);
}
public static bool collide(Robot a, Wall b)
{
return EngineUtilities.collide(b, a);
}
public static bool collide(Wall wall, Robot robot)
{
Point2D a1 = new Point2D(wall.Line.Endpoint1);
Point2D a2 = new Point2D(wall.Line.Endpoint2);
Point2D b = new Point2D(robot.Location.X, robot.Location.Y);
if (!wall.Visible)
return false;
double rad = robot.Radius;
double r = ((b.X - a1.X) * (a2.X - a1.X) + (b.Y - a1.Y) * (a2.Y - a1.Y)) / wall.Line.squaredLength();
double px = a1.X + r * (a2.X - a1.X);
double py = a1.Y + r * (a2.Y - a1.Y);
Point2D np = new Point2D(px, py);
double rad_sq = rad * rad;
if (r >= 0.0f && r <= 1.0f)
{
if (np.squaredDistance(b) < rad_sq)
return true;
else
return false;
}
double d1 = b.squaredDistance(a1);
double d2 = b.squaredDistance(a2);
if (d1 < rad_sq || d2 < rad_sq)
return true;
else
return false;
}
public abstract double raycast(double angle, double maxRange, Point2D point, Robot owner, out SimulatorObject hit, bool absolute = false);
public abstract bool robotCollide(Robot robot);
private void MouseMoveHandler(object sender, MouseEventArgs e)
{
if (drawMode == drawModes.selectMode)
{
if (bDragDisplay)
{
//how far has the wall been dragged?
int dx = e.X - x1;
int dy = e.Y - y1;
x1 = e.X;
y1 = e.Y;
float odx,ody;
frame.convertFromDisplayOffset((float)dx,(float)dy,out odx, out ody);
frame.X-=odx;
frame.Y-=ody;
}
if (bSelected_wall)
{
//wall-dragging code
//how far has the wall been dragged?
int dx = e.X - x1;
int dy = e.Y - y1;
x1 = e.X;
y1 = e.Y;
float odx,ody;
frame.convertFromDisplayOffset((float)dx,(float)dy,out odx, out ody);
if (selectMode == selectModes.dragMode)
{
//update wall's position
selected_wall.Line.Endpoint1.X += odx;
selected_wall.Line.Endpoint1.Y += ody;
selected_wall.Line.Endpoint2.X += odx;
selected_wall.Line.Endpoint2.Y += ody;
}
if (selectMode == selectModes.rotateMode)
{
Point2D midpoint = selected_wall.Line.midpoint();
selected_wall.Line.Endpoint1.rotate(dy / 180.0 * 3.14, midpoint);
selected_wall.Line.Endpoint2.rotate(dy / 180.0 * 3.14, midpoint);
}
if (selectMode == selectModes.scaleMode)
{
selected_wall.Line.scale(1.0 + 0.05 * dy);
}
Invalidate();
}
if (bSelected_robot)
{
//robot-dragging code
//how far has the robot been dragged?
int dx = e.X - x1;
int dy = e.Y - y1;
x1 = e.X;
y1 = e.Y;
float odx,ody;
frame.convertFromDisplayOffset((float)dx,(float)dy,out odx, out ody);
//update robot's position
if (selectMode == selectModes.dragMode)
{
selected_robot.Location.X += odx;
selected_robot.Location.Y += ody;
}
if (selectMode == selectModes.rotateMode)
{
selected_robot.Heading += (dy / 10.0);
}
if (selectMode == selectModes.scaleMode)
{
selected_robot.Radius += (dy / 2.0);
if (selected_robot.Radius < 1.0)
selected_robot.Radius = 1.0;
selected_robot.AreaOfImpact.Radius = selected_robot.Radius;
}
Invalidate();
}
}
if (drawMode == drawModes.selectMode && !bSelected_wall && !bSelected_robot)
{
selected_wall = null;
selected_robot = null;
display_snap = false;
//find snap
Point2D newpoint = new Point2D((double)e.X, (double)e.Y);
if (experiment != null)
{
if(experiment.robots!=null)
foreach (Robot robot in experiment.robots)
{
Point2D screen_location = frame.convertToDisplay(robot.Location);
if (screen_location.distance(newpoint) < 20.0)
{
display_snap = true;
snapx = (int)screen_location.X;
snapy = (int)screen_location.Y;
selected_robot = robot;
}
}
if(experiment.environment.walls!=null)
foreach (Wall wall in experiment.environment.walls)
{
Point2D screen_location = frame.convertToDisplay(wall.Location);
if (screen_location.distance(newpoint) < 20.0)
{
display_snap = true;
snapx = (int)screen_location.X;
snapy = (int)screen_location.Y;
selected_wall = wall;
}
}
int index = 0;
if (experiment.environment.walls != null)
foreach (Point p in experiment.environment.POIPosition)
{
Point2D screen_location = frame.convertToDisplay(new Point2D(p.X, p.Y));
if (screen_location.distance(newpoint) < 20.0)
{
display_snap = true;
snapx = (int)screen_location.X;
snapy = (int)screen_location.Y;
selected_POI = index;
}
index++;
}
}
Invalidate();
}
if (drawMode == drawModes.wallMode)
{
display_snap = false;
//find snap
Point2D newpoint = new Point2D((double)e.X, (double)e.Y);
foreach (Wall wall in experiment.environment.walls)
{
Point2D screen_location1 = frame.convertToDisplay(wall.Line.Endpoint1);
Point2D screen_location2 = frame.convertToDisplay(wall.Line.Endpoint2);
if (screen_location1.distance(newpoint) < 10.0)
{
display_snap = true;
snapx = (int)screen_location1.X;
snapy = (int)screen_location1.Y;
}
if (screen_location2.distance(newpoint) < 10.0)
{
display_snap = true;
snapx = (int)screen_location2.X;
snapy = (int)screen_location2.Y;
}
}
x2 = e.X;
y2 = e.Y;
Invalidate();
}
if (drawMode == drawModes.AOIMode && displayAOIRectangle)
{
float ox,oy;
frame.convertFromDisplay(e.X,e.Y,out ox,out oy);
experiment.environment.AOIRectangle = new Rectangle(experiment.environment.AOIRectangle.X,
experiment.environment.AOIRectangle.Y,(int) ox - experiment.environment.AOIRectangle.X, (int) oy - experiment.environment.AOIRectangle.Y);
Invalidate();
}
}
/// <summary>
/// Adds the specified robot to the robot list. Robots need to be registered before they can receive ANN results.
/// </summary>
public void registerRobot(Robot robot)
{
if (RobotListeners.Contains(robot))
{
Console.WriteLine("Robot " + robot.ID + " already registered");
return;
}
RobotListeners.Add(robot);
if (RobotListeners.Count > NumRobots)
{
Console.WriteLine("Number of registered agents [" + RobotListeners.Count + "] and number of agents [" + NumRobots + "] does not match");
}
}
public RangeFinder(double a, Robot o, double _max_range, double _noise)
{
Owner =o;
Angle = a;
MaxRange = _max_range;
DistanceToClosestObject = (-1);
}
/// <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;
}
