void IFitnessFunction.update(SimulatorExperiment engine, Environment environment) { if (!(engine.timeSteps % 25 ==0)) //(int)(0.5 / engine.timestep) == 0)) { //grid.decay_viewed(0); return; } double[] gl=new double[3]; double[] el=new double[3]; if(!first) { for(int i=0;i<engine.robots.Count;i++) gl[i]=0.0; } else { for(int i=0;i<engine.robots.Count;i++) el[i]=0.0; } int r_ind=0; if(!first && (engine.timeSteps * engine.timestep )> 6.0) { foreach(Robot r in engine.robots) { SignalSensor s = (SignalSensor)r.sensors[r.sensors.Count-1]; s.setSignal(1.0); origDist[r_ind]=r.location.distance(environment.goal_point); r_ind++; } r_ind=0; first=true; } foreach (Robot r in engine.robots) { int p_ind=0; double d2 = r.location.distance(environment.goal_point); if(first) { //if(point.distance(environment.goal_point) <25.0) { // endList[i]=1.5; //} //else { el[r_ind]=Math.Max(0.0,(origDist[r_ind]-d2)/200.0); //} } /* else { System.Drawing.Point p=environment.POIPosition[r_ind]; Point2D point= new Point2D((double)p.X,(double)p.Y); double d1=point.distance(r.location); gl[r_ind]=Math.Max(0.0,(200.0-d1)/200.0); } */ foreach (System.Drawing.Point p in environment.POIPosition) { Point2D point= new Point2D((double)p.X,(double)p.Y); int i =p_ind; double d1 = point.distance(r.location); if(!first) { gl[i]= Math.Max(gl[i],(200.0-d1)/200.0*5.0); } p_ind+=1; } r_ind+=1; } for(int i=0;i<3;i++) { gotoList[i]+=gl[i]; endList[i]+=el[i]; } return; }
//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); }
public double RaycastGrid(double angle, double max_range, Point2D point, Robot owner, bool view, out SimulatorObject hitobj) { hitobj = null; Point2D casted = new Point2D(point); double distance = max_range; bool hit = false; bool hitRobot = false; //cast point casted out from the robot's center point along the sensor direction double sum_angle = angle + owner.heading; double cosval = Math.Cos(sum_angle); double sinval = Math.Sin(sum_angle); double add_valx = cosval * distance; double add_valy = sinval * distance; casted.x += add_valx; casted.y += add_valy; // if(Double.IsNaN(casted.x)) // Console.WriteLine("casted x nan " + cosval + " " + add_valx + " " + Math.Cos(sum_angle)); // if(Double.IsNaN(casted.y)) // Console.WriteLine("casted y nan " + sinval + " " + add_valy + " " + Math.Sin(sum_angle)); //create line segment from robot's center to casted point Line2D cast = new Line2D(point, casted); List <collision_grid_square> squares = grid.cast_ray(point.x, point.y, casted.x, casted.y); foreach (collision_grid_square square in squares) { //if we had at least one collision, then quit! if (hit) { break; } if (view && !owner.disabled) { if (owner.stopped) { square.viewed2 = 1.0; } square.viewed = 1.0; } //if(view && square.viewed < health) // square.viewed=health; //now do naive detection of collision of casted rays with objects in square //first for all walls foreach (SimulatorObject obj in square.static_objs) { Wall wall = (Wall)obj; 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; hitobj = wall; } hit = true; } } //then for all robots if (agentVisible) { foreach (SimulatorObject obj in square.dynamic_objs) { Robot r = (Robot)obj; bool found = false; if (r == owner) { continue; } double new_distance = cast.nearest_intersection(r.circle, out found); if (found) { if (new_distance < distance) { distance = new_distance; hitRobot = true; hitobj = r; } hit = 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; }
void IFitnessFunction.update(SimulatorExperiment Experiment, Environment environment, instance_pack ip) { if (reachedGoal) return; double rangeMin = 2.5; double rangeMax = 3.5; double sumx = 0; double sumy = 0; int sum = 0; inFormation = 0; double maxY = 0.0; double minY = 2000.0; foreach (Robot r in ip.robots) { if (!r.autopilot) { double radius = r.radius; Point2D loc = r.location; sumx += loc.x; sumy += loc.y; sum++; foreach (Robot r2 in ip.robots) { double dist = loc.distance(r2.location); if ((dist >= radius * rangeMin) && (dist <= radius * rangeMax)) inFormation++; } if (loc.y > maxY) maxY = loc.y; if (loc.y < minY) minY = loc.y; } } formHeight = maxY - minY; inFormation /= 2; double avgX = sumx / (double)sum; double avgY = sumy / (double)sum; avgLoc = new Point2D(avgX, avgY); if (avgLoc.distance(environment.goal_point) < 20.0f) reachedGoal = true; for (int i = 0; i < environment.POIPosition.Count; i++) { if (avgLoc.distance(new Point2D((int)environment.POIPosition[i].X, (int)environment.POIPosition[i].Y)) < 20.0f) reachedPOI[i] = true; } }
void IBehaviorCharacterization.update(SimulatorExperiment engine) { Environment environment = engine.environment; if (!(engine.timeSteps % 25 == 0)) //(int)(0.5 / engine.timestep) == 0)) { //grid.decay_viewed(0); return; } double[] gl = new double[3]; double[] el = new double[3]; for (int i = 0; i < engine.robots.Count; i++) { gl[i] = 0.0; } for (int i = 0; i < engine.robots.Count; i++) { el[i] = 0.0; } int r_ind = 0; if (!last && (engine.timeSteps * engine.timestep) > 14.0) { foreach (Robot r in engine.robots) { bc.Add(r.location.x); bc.Add(r.location.y); last = true; } } if (!first && (engine.timeSteps * engine.timestep) > 6.0) { foreach (Robot r in engine.robots) { SignalSensor s = (SignalSensor)r.sensors[r.sensors.Count - 1]; s.setSignal(1.0); bc.Add(r.location.x); bc.Add(r.location.y); origDist[r_ind] = r.location.distance(environment.goal_point); r_ind++; } first = true; r_ind = 0; } foreach (Robot r in engine.robots) { double d2 = r.location.distance(environment.goal_point); if (first) { //if(point.distance(environment.goal_point) <25.0) { // endList[i]=1.5; //} //else { el[r_ind] = Math.Max(el[r_ind], (origDist[r_ind] - d2) / 200.0); //} } /* * else { * System.Drawing.Point p=environment.POIPosition[r_ind]; * Point2D point= new Point2D((double)p.X,(double)p.Y); * double d1=point.distance(r.location); * gl[r_ind]=Math.Max(0.0,(200.0-d1)/200.0); * } */ int p_ind = 0; foreach (System.Drawing.Point p in environment.POIPosition) { Point2D point = new Point2D((double)p.X, (double)p.Y); int i = p_ind; double d1 = point.distance(r.location); if (!first) { gl[i] = Math.Max(gl[i], (200.0 - d1) / 200.0); } p_ind += 1; } r_ind += 1; } for (int i = 0; i < 3; i++) { gotoList[i] += gl[i]; endList[i] += el[i]; } return; }
//what is the length of this line segment public double length() { return(p1.distance(p2)); }
public override double Raycast(double angle, double max_range, Point2D point, Robot owner, out SimulatorObject hit) { hit = null; bool hitRobot = false; Point2D casted = new Point2D(point); double distance = max_range; //cast point casted out from the robot's center point along the sensor direction casted.x += Math.Cos(angle + owner.heading) * distance; casted.y += Math.Sin(angle + owner.heading) * distance; //create line segment from robot's center to casted point Line2D cast = new Line2D(point, casted); //TODO remove //now do naive detection of collision of casted rays with objects //first for all walls foreach (Wall wall in env.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 hitRobot = false; if (!agentVisible) { return(distance); } //if (agentsVisible) foreach (Robot robot2 in rbts) { bool found = false; if (robot2 == owner) { continue; } double new_distance = cast.nearest_intersection(robot2.circle, out found); if (found) { if (new_distance < distance) { distance = new_distance; hitRobot = true; hit = robot2; } } } return(distance); }