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;
}
public void update(Environment env, List<Robot> robots, CollisionManager cm)
{
distance = maxRange;
seePrey = false;
Point2D casted = new Point2D(owner.location);
casted.x += Math.Cos(angle + owner.heading) * maxRange;
casted.y += Math.Sin(angle + owner.heading) * maxRange;
Line2D cast = new Line2D(owner.location, casted);
foreach (Prey prey in env.preys)
{
bool found = false;
double newDistance = cast.nearest_intersection(prey.circle, out found);
if (found)
{
if (newDistance < distance)
{
distance = newDistance;
seePrey = true;
}
}
}
}
public void init(int id, double nx, double ny, double dir,
AgentBrain agentBrain, Environment environment,
float sensorNoise, float effectorNoise, float headingNoise, float timeStep)
{
steps = 0;
this.id = id;
radius = defaultRobotSize();
location = new Point2D(nx, ny);
circle = new Circle2D(location, radius);
old_location = new Point2D(location);
heading = dir;
velocity = 0.0;
collide_last = false;
this.timeStep = timeStep;
this.environment = environment;
this.agentBrain = agentBrain;
this.sensorNoise = sensorNoise;
this.effectorNoise = effectorNoise;
this.headingNoise = headingNoise;
populateSensors();
if(environment.seed != -1)
rng=environment.rng; //new SharpNeatLib.Maths.FastRandom(environment.seed); //Utilities.random;
else
rng=environment.rng; //new SharpNeatLib.Maths.FastRandom();
}
public FlockingFitness()
{
//Initialize variables
avgLoc = null;
reachedGoal = false;
inFormation = 0;
formHeight = 0.0;
}
double nearest(instance_pack ip, double x, double y) {
double nd=10000000.0;
Point2D p = new Point2D(x,y);
foreach(Robot r in ip.robots) {
double d=r.location.distance(p);
if(d<nd) nd=d;
}
return nd;
}
public Prey(double x, double y, double rad, double range, double speed, bool vis, string n)
{
this.name = n;
this.colored = false;
this.dynamic = true;
this.caught = false;
this.visible = vis;
this.speed = speed;
this.range = range;
Point2D p = new Point2D(x, y);
this.circle = new Circle2D(p, rad);
this.location = circle.p;
}
public Wall(double nx1, double ny1, double nx2, double ny2, bool vis, string n)
{
name = n;
colored = false;
Point2D p1 = new Point2D(nx1, ny1);
Point2D p2 = new Point2D(nx2, ny2);
line = new Line2D(p1, p2);
//set center point
location = line.midpoint();
dynamic = false;
visible = vis;
}
//rotate this point about another point
public void rotate(double angle, Point2D point)
{
x-=point.x;
y-=point.y;
double ox = x;
double oy = y;
x=Math.Cos(angle)*ox - Math.Sin(angle)*oy;
y=Math.Sin(angle)*ox + Math.Cos(angle)*oy;
x+=point.x;
y+=point.y;
}
//Different from PieSliceSensorArray in that EnemyRobots are used as targets instead
// of goal points.
public void update(Engine.Environment env, List<Robot> robots, CollisionManager cm)
{
List<float> translatedEnemyAngles = new List<float>();
// Start at 1: Assume all robots after first are enemies
for (int i = 1; i < robots.Count; i++)
{
EnemyRobot er = (EnemyRobot) robots[i];
if (!er.stopped) // Only sense prey robots that are active
{
Point2D temp = new Point2D((int)er.location.x, (int)er.location.y);
temp.rotate((float)-(owner.heading), owner.circle.p); //((float)-(owner.heading * 180.0 / 3.14), owner.circle.p);
//translate with respect to location of navigator
temp.x -= (float)owner.circle.p.x;
temp.y -= (float)owner.circle.p.y;
//what angle is the vector between target & navigator
float angle = angleValue((float)temp.x, (float)temp.y);// (float)temp.angle();
translatedEnemyAngles.Add(angle);
}
}
//fire the appropriate radar sensor
for (int i = 0; i < radarAngles1.Count; i++)
{
signalsSensors[i].setSignal(0.0);
for (int a = 0; a < translatedEnemyAngles.Count; a++)
{
float angle = translatedEnemyAngles[a];
if (angle >= radarAngles1[i] && angle < radarAngles2[i])
{
signalsSensors[i].setSignal(1.0);
}
if (angle + 360.0 >= radarAngles1[i] && angle + 360.0 < radarAngles2[i])
{
signalsSensors[i].setSignal(1.0);
}
}
}
}
//per default we use the goal point of the environment as our target point.
public void update(Engine.Environment env, List<Robot> robots, CollisionManager cm)
{
Point2D temp = new Point2D((int)env.goal_point.x, (int)env.goal_point.y);
temp.rotate((float)-(owner.heading), owner.circle.p); //((float)-(owner.heading * 180.0 / 3.14), owner.circle.p);
//translate with respect to location of navigator
temp.x -= (float)owner.circle.p.x;
temp.y -= (float)owner.circle.p.y;
//what angle is the vector between target & navigator
float angle = angleValue((float)temp.x, (float) temp.y);// (float)temp.angle();
//! angle *= 57.297f;//convert to degrees
//fire the appropriate radar sensor
for (int i = 0; i < radarAngles1.Count; i++)
{
signalsSensors[i].setSignal(0.0);
//radar[i] = 0.0f;
if (angle >= radarAngles1[i] && angle < radarAngles2[i])
{
signalsSensors[i].setSignal(1.0);
// Console.WriteLine(i);
}
//radar[i] = 1.0f;
if (angle + 360.0 >= radarAngles1[i] && angle + 360.0 < radarAngles2[i])
{
signalsSensors[i].setSignal(1.0);
// Console.WriteLine(i);
}
// radar[i] = 1.0f;
// inputs[sim_engine.robots[0].rangefinders.Count + i] = sim_engine.radar[i];
}
}
public Circle2D(Circle2D other)
{
p = other.p;
radius = other.radius;
}
public void reset()
{
seed = 0;
view_x = 0.0f;
view_y = 0.0f;
view_scale = 5.0f;
AOIRectangle = new Rectangle(30, 60, 640, 500);
group_orientation = 0;
robot_spacing = 30;
robot_heading = 0;
rng = new SharpNeatLib.Maths.FastRandom();
walls = new List<Wall>();
preys = new List<Prey>();
POIPosition = new List<Point>();
start_point = new Point2D(0, 0);
goal_point = new Point2D(100, 100);
}
public abstract double Raycast(double angle, double max_range, Point2D point, Robot owner,out SimulatorObject hit);
public static double euclideanDistance(Point2D p1, Point2D p2)
{
return Math.Sqrt(Math.Pow(p1.x - p2.x, 2) + Math.Pow(p1.y - p2.y, 2));
}
public Point2D to_display(Point2D p)
{
float ox,oy;
to_display((float)p.x,(float)p.y,out ox, out oy);
return new Point2D((double)ox,(double)oy);
}
//what is the distance from this line to a point
public double distance(Point2D n)
{
return Math.Sqrt(distance_sq(n));
}
public Line2D(Point2D a, Point2D b)
{
p1 = a;
p2 = b;
}
public Point2D(Point2D another)
{
x = another.x;
y = another.y;
}
//what is the distance from this line to a point
public double distance(Point2D n)
{
return(Math.Sqrt(distance_sq(n)));
}
public Circle2D(Point2D a, double rad)
{
radius = rad;
p = a;
}
//calculate the point of intersection between two line segments
public Point2D intersection(Line2D L,out bool found)
{
Point2D pt = new Point2D(0.0,0.0);
Point2D A = p1;
Point2D B = p2;
Point2D C = L.p1;
Point2D D = L.p2;
double rTop = (A.y-C.y)*(D.x-C.x)-(A.x-C.x)*(D.y-C.y);
double rBot = (B.x-A.x)*(D.y-C.y)-(B.y-A.y)*(D.x-C.x);
double sTop = (A.y-C.y)*(B.x-A.x)-(A.x-C.x)*(B.y-A.y);
double sBot = (B.x-A.x)*(D.y-C.y)-(B.y-A.y)*(D.x-C.x);
if ((rBot == 0 || sBot == 0))
{
found = false;
return pt;
}
double r = rTop/rBot;
double s = sTop/sBot;
if( (r>0) && (r<1) && (s>0) && (s<1))
{
pt.x = A.x + r * (B.x-A.x);
pt.y = A.y + r * (B.y - A.y);
found=true;
return pt;
}
else
{
found = false;
return pt;
}
}
// Schrum2: Vector subtraction of one point from another
public Point2D subtract(Point2D other)
{
return(new Point2D(x - other.x, y - other.y));
}
//what is the squared distance from this line to a point
public double distance_sq(Point2D n)
{
double utop = (n.x-p1.x)*(p2.x-p1.x)+(n.y-p1.y)*(p2.y-p1.y);
double ubot = p1.distance_sq(p2);
double u = utop/ubot;
if(u<0 || u> 1)
{
double d1 = p1.distance_sq(n);
double d2 = p2.distance_sq(n);
if(d1<d2) return d1;
return d2;
}
Point2D p=new Point2D(0.0,0.0);
p.x=p1.x+u*(p2.x-p1.x);
p.y=p1.y+u*(p2.y-p1.y);
return p.distance_sq(n);
}
//what is the distance to another point
public double distance(Point2D point)
{
return Math.Sqrt(distance_sq(point));
}
public Point2D(Point2D another)
{
x=another.x;
y=another.y;
}
// Schrum2: Vector subtraction of one point from another
public Point2D subtract(Point2D other) {
return new Point2D(x - other.x, y - other.y);
}
//what is the squared dist to another point
public double distance_sq(Point2D point)
{
double dx=point.x-x;
double dy=point.y-y;
return dx*dx+dy*dy;
}
public Line2D(Point2D a, Point2D b)
{
p1=a;
p2=b;
}
public static bool collide(Wall wall, Robot robot)
{
Point2D a1 = new Point2D(wall.line.p1);
Point2D a2 = new Point2D(wall.line.p2);
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.length_sq();
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.distance_sq(b) < rad_sq)
return true;
else
return false;
}
double d1 = b.distance_sq(a1);
double d2 = b.distance_sq(a2);
if (d1 < rad_sq || d2 < rad_sq)
return true;
else
return false;
}
public void update(List<Robot> rbts, double timestep)
{
if (start == null) start = new Circle2D(circle);
Robot closest = null;
double closestDist = range;
foreach (Robot r in rbts)
{
double dist = circle.p.distance(r.location);
//Console.WriteLine("dist: " + dist);
if (dist <= closestDist)
{
closestDist = dist;
closest = r;
}
}
if (closestDist <= (circle.radius + rbts[0].radius))
{
caught = true;
dynamic = false;
return;
}
if (closest != null)
{
double dA = circle.p.x - closest.location.x;
double dB = circle.p.y - closest.location.y;
double c = closest.location.distance(circle.p);
double z = speed * timestep;
double dX = (dA * z) / c;
double dY = (dB * z) / c;
Point2D newP = new Point2D(dX + circle.p.x, dY + circle.p.y);
circle.p = newP;
}
}
//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 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;
}
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.offset_from_display((float)dx,(float)dy,out odx, out ody);
frame.cx-=odx;
frame.cy-=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.offset_from_display((float)dx,(float)dy,out odx, out ody);
if (selectMode == selectModes.dragMode)
{
//update wall's position
selected_wall.line.p1.x += odx;
selected_wall.line.p1.y += ody;
selected_wall.line.p2.x += odx;
selected_wall.line.p2.y += ody;
}
if (selectMode == selectModes.rotateMode)
{
Point2D midpoint = selected_wall.line.midpoint();
selected_wall.line.p1.rotate(dy / 180.0 * 3.14, midpoint);
selected_wall.line.p2.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.offset_from_display((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.circle.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.to_display(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.to_display(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;
selected_POI = -1;
if (experiment.environment.walls != null)
foreach (Point p in experiment.environment.POIPosition)
{
Point2D screen_location = frame.to_display(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.to_display(wall.line.p1);
Point2D screen_location2 = frame.to_display(wall.line.p2);
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.from_display(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();
}
}
//what is the distance to another point
public double distance(Point2D point)
{
return(Math.Sqrt(distance_sq(point)));
}
public Line2D(Line2D other)
{
p1=other.p1;
p2=other.p2;
}
public Line2D(Line2D other)
{
p1 = other.p1;
p2 = other.p2;
}
void IFitnessFunction.update(SimulatorExperiment Experiment, Environment environment, instance_pack ip)
{
// Schrum: brain-switching policy
// Schrum: Restricting numBrains == 2 assures that this switch won't occur in the FourTasks experiments with 5 brains
if (Experiment.multibrain && !Experiment.preferenceNeurons && Experiment.numBrains == 2)
{
// Schrum: These magic numbers directly correspond to the Two Rooms environment.
// This range of y values is the portion of the environment that is taken
// up by the hallway.
if (ip.agentBrain.getBrainCounter() == 1 && // Don't "switch" if already using right brain
ip.robots[0].location.y < 716 && ip.robots[0].location.y > 465) //hallway
{
//Console.WriteLine("Switch to 0");
ip.agentBrain.switchBrains(0); // use brain 0 for hallway
}
else if (ip.agentBrain.getBrainCounter() == 0) //room
{
//Console.WriteLine("Switch to 1");
ip.agentBrain.switchBrains(1); // use brain 1 when in the open
}
}
// Schrum: Debug: For comparing non-visual eval with visual
// Prints out locations visited by all robots
/*
for (int i = 0; i < ip.robots.Count; i++)
{
Console.Write(ip.robots[i].location.x + "\t" + ip.robots[i].location.y + "\t");
if (ip.robots[i] is EnemyRobot)
{
Console.Write(((EnemyRobot)ip.robots[i]).wallResponse + "\t" + ((EnemyRobot)ip.robots[i]).chaseResponse + "\t" + ip.robots[i].heading + "\t" + ((EnemyRobot)ip.robots[i]).angle + "\t");
}
}
Console.WriteLine();
*/
// End debug
//For food gathering
if (ip.timeSteps == 1)
{
environment.goal_point.x = environment.POIPosition[0].X;
environment.goal_point.y = environment.POIPosition[0].Y;
collectedFood = 0;
POINr = 0;
collided = false;
finished = false;
portionAlive = 0;
}
Point2D goalPoint = new Point2D (environment.POIPosition [POINr].X, environment.POIPosition [POINr].Y);
float d = (float)ip.robots[0].location.distance(goalPoint);
bool guidance = true;
if (d < 20.0f && !finished)
{
collectedFood++;
POINr++;
if (POINr >= environment.POIPosition.Count) {
POINr = 0;
finished = true;
}
if (POINr > 4 && POINr < 11) {
guidance = false;
}
if (guidance) {
environment.goal_point.x = environment.POIPosition [POINr].X;
environment.goal_point.y = environment.POIPosition [POINr].Y;
}
}
// Schrum2: Added to detect robot collisions and end the evaluation when they happen
if (ip.robots[0].collisions > 0)
{ // Disabling prevents further action
collided = true;
portionAlive = (ip.elapsed * 1.0) / Experiment.evaluationTime;
//Console.WriteLine("Fitness before:" + ip.elapsed + "/" + Experiment.evaluationTime + ":" + ip.timeSteps);
ip.elapsed = Experiment.evaluationTime; // force end time: only affects non-visual evaluation
Experiment.running = false; // Ends visual evaluation
//Console.WriteLine("Collision");
//Console.WriteLine("Fitness after:" + ip.elapsed + "/" + Experiment.evaluationTime + ":" + ip.timeSteps);
}
}
public Circle2D(Point2D a, double rad)
{
radius=rad;
p=a;
}
public Circle2D(Circle2D other)
{
p=other.p;
radius=other.radius;
}
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;
}