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 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;
}
public Wall(Wall k)
{
name = k.name;
//Console.WriteLine(name);
colored = false;
line = new Line2D(k.line);
//set center point
location = line.midpoint();
dynamic = false;
visible = k.visible;
}
public override void doAction()
{
if (!this.stopped)
{
//Schrum: debug
//Console.WriteLine("-------------------------------------------------------");
//Console.WriteLine("Start EnemyRobot.doAction(): heading = " + heading);
//Console.WriteLine(evolved.location.x + "\t" + evolved.location.y + "\t" + location.x + "\t" + location.y);
//Console.WriteLine("Enemy doAction");
// Schrum2: Simple behavior
// Speed calculation taken from Khepera3RobotModelContinuous, but made faster
float speed = 11.0f * (1.0f + (effectorNoise / 100.0f * (float)(2.0 * (rng.NextDouble() - 0.5))));
velocity = speed;
double turn = 0;
// Schrum: A collision overrides all other behaviors
if (this.collisions > lastCollisions)
{
velocity = -velocity; // back up
}
else // Not currently colliding
{
const double WALL_TURN_AMOUNT = Math.PI / 50.0;
const double CHASE_TURN_AMOUNT = Math.PI / 40.0;
const double TOO_CLOSE_WALL_THRESHOLD = 17;
const double TOO_CLOSE_EVOLVED_THRESHOLD = 50;
const double EXTREME_CLOSE_EVOLVED_THRESHOLD = 25;
Line2D toEvolved = new Line2D(location, getEvolved().location);
double angleDifference = toEvolved.signedAngleFromSourceHeadingToTarget(heading);
double distance = toEvolved.length();
Boolean evolvedClose = distance < TOO_CLOSE_EVOLVED_THRESHOLD;
Boolean extremeEvolvedClose = distance < EXTREME_CLOSE_EVOLVED_THRESHOLD;
Boolean wallTooClose = false;
double closestWall = Double.MaxValue;
// schrum2: check sensors for walls.
// sensor values of 1 mean there is no wall contact.
// Lesser values means wall is closer along that sensor
double left = 0;
int half = sensors.Count / 2;
for (int j = 0; j < half; j++)
{
if (!(sensors[j] is SignalSensor))
{
double raw = sensors[j].get_value_raw();
closestWall = Math.Min(raw, closestWall);
wallTooClose = wallTooClose || TOO_CLOSE_WALL_THRESHOLD > raw;
left += transformSensor(sensors[j].get_value_raw());
}
}
double right = 0;
for (int j = half + 1; j < sensors.Count; j++)
{
if (!(sensors[j] is SignalSensor))
{
double raw = sensors[j].get_value_raw();
closestWall = Math.Min(raw, closestWall);
wallTooClose = wallTooClose || TOO_CLOSE_WALL_THRESHOLD > raw;
right += transformSensor(sensors[j].get_value_raw());
}
}
if (wallTooClose &&
(flee || // fleeing bots are always concerned about close walls
(closestWall < distance && !extremeEvolvedClose))) // Chasing bots sometimes prioritize chasing over collision avoidance
{
velocity /= 4; // Go slower when collision imminent
}
// Schrum: For debugging
//wallResponse = 0;
//chaseResponse = 0;
//angle = angleDifference;
if ((flee || // fleeing bots always avoid walls
(!extremeEvolvedClose && (!evolvedClose || wallTooClose))) // chasing bots sometimes prioritize chasing
&& right < left)
{ // right sensors are closer to wall
// turn left
turn = -WALL_TURN_AMOUNT;
}
else if ((flee ||
(!extremeEvolvedClose && (!evolvedClose || wallTooClose))) // complicated special cases for chasing bots
&& left < right)
{ // left sensors are closer to wall
// turn right
turn = WALL_TURN_AMOUNT;
}
else
{
// Base turn purely on evolved bot location if there are no wall problems
if (angleDifference < 0)
{ // turn towards evolved bot to chase, or away if fleeing
turn = (flee ? 1 : -1) * CHASE_TURN_AMOUNT;
}
else
{
turn = (flee ? -1 : 1) * CHASE_TURN_AMOUNT;
}
}
heading += turn;
}
//Console.WriteLine(flee +":" + turn);
lastCollisions = this.collisions; // Keep up to date
// Schrum2: Need to do this manually.
updatePosition(); // Moves robot based on velocity and heading
/*
if (heading > Math.PI * 2)
{
Console.WriteLine("Heading out of range (doAction): " + heading);
System.Windows.Forms.Application.Exit();
System.Environment.Exit(1);
}
*/
}
}
//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;
}
}
public Line2D(Line2D other)
{
p1=other.p1;
p2=other.p2;
}
/// <summary>
/// Transform line coordinates
/// </summary>
/// <param name="line">Line</param>
/// <param name="transform">Transformation</param>
/// <returns>Returns new line</returns>
public static Line2D Transform(Line2D line, Matrix transform)
{
return(new Line2D(
Vector2.TransformCoordinate(line.Point1, transform),
Vector2.TransformCoordinate(line.Point2, transform)));
}
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;
}
public Line2D(Line2D other)
{
p1 = other.p1;
p2 = other.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);
}
public override void doAction()
{
if (!this.stopped)
{
//Schrum: debug
//Console.WriteLine("-------------------------------------------------------");
//Console.WriteLine("Start EnemyRobot.doAction(): heading = " + heading);
//Console.WriteLine(evolved.location.x + "\t" + evolved.location.y + "\t" + location.x + "\t" + location.y);
//Console.WriteLine("Enemy doAction");
// Schrum2: Simple behavior
// Speed calculation taken from Khepera3RobotModelContinuous, but made faster
float speed = 11.0f * (1.0f + (effectorNoise / 100.0f * (float)(2.0 * (rng.NextDouble() - 0.5))));
velocity = speed;
double turn = 0;
// Schrum: A collision overrides all other behaviors
if (this.collisions > lastCollisions)
{
velocity = -velocity; // back up
}
else // Not currently colliding
{
const double WALL_TURN_AMOUNT = Math.PI / 50.0;
const double CHASE_TURN_AMOUNT = Math.PI / 40.0;
const double TOO_CLOSE_WALL_THRESHOLD = 17;
const double TOO_CLOSE_EVOLVED_THRESHOLD = 50;
const double EXTREME_CLOSE_EVOLVED_THRESHOLD = 25;
Line2D toEvolved = new Line2D(location, getEvolved().location);
double angleDifference = toEvolved.signedAngleFromSourceHeadingToTarget(heading);
double distance = toEvolved.length();
Boolean evolvedClose = distance < TOO_CLOSE_EVOLVED_THRESHOLD;
Boolean extremeEvolvedClose = distance < EXTREME_CLOSE_EVOLVED_THRESHOLD;
Boolean wallTooClose = false;
double closestWall = Double.MaxValue;
// schrum2: check sensors for walls.
// sensor values of 1 mean there is no wall contact.
// Lesser values means wall is closer along that sensor
double left = 0;
int half = sensors.Count / 2;
for (int j = 0; j < half; j++)
{
if (!(sensors[j] is SignalSensor))
{
double raw = sensors[j].get_value_raw();
closestWall = Math.Min(raw, closestWall);
wallTooClose = wallTooClose || TOO_CLOSE_WALL_THRESHOLD > raw;
left += transformSensor(sensors[j].get_value_raw());
}
}
double right = 0;
for (int j = half + 1; j < sensors.Count; j++)
{
if (!(sensors[j] is SignalSensor))
{
double raw = sensors[j].get_value_raw();
closestWall = Math.Min(raw, closestWall);
wallTooClose = wallTooClose || TOO_CLOSE_WALL_THRESHOLD > raw;
right += transformSensor(sensors[j].get_value_raw());
}
}
if (wallTooClose &&
(flee || // fleeing bots are always concerned about close walls
(closestWall < distance && !extremeEvolvedClose))) // Chasing bots sometimes prioritize chasing over collision avoidance
{
velocity /= 4; // Go slower when collision imminent
}
// Schrum: For debugging
//wallResponse = 0;
//chaseResponse = 0;
//angle = angleDifference;
if ((flee || // fleeing bots always avoid walls
(!extremeEvolvedClose && (!evolvedClose || wallTooClose))) && // chasing bots sometimes prioritize chasing
right < left)
{ // right sensors are closer to wall
// turn left
turn = -WALL_TURN_AMOUNT;
}
else if ((flee ||
(!extremeEvolvedClose && (!evolvedClose || wallTooClose))) && // complicated special cases for chasing bots
left < right)
{ // left sensors are closer to wall
// turn right
turn = WALL_TURN_AMOUNT;
}
else
{
// Base turn purely on evolved bot location if there are no wall problems
if (angleDifference < 0)
{ // turn towards evolved bot to chase, or away if fleeing
turn = (flee ? 1 : -1) * CHASE_TURN_AMOUNT;
}
else
{
turn = (flee ? -1 : 1) * CHASE_TURN_AMOUNT;
}
}
heading += turn;
}
//Console.WriteLine(flee +":" + turn);
lastCollisions = this.collisions; // Keep up to date
// Schrum2: Need to do this manually.
updatePosition(); // Moves robot based on velocity and heading
/*
* if (heading > Math.PI * 2)
* {
* Console.WriteLine("Heading out of range (doAction): " + heading);
* System.Windows.Forms.Application.Exit();
* System.Environment.Exit(1);
* }
*/
}
}