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