public PieSliceSensorArray(Robot o)
{
signalsSensors = new List<SignalSensor>(4);
radarAngles1 = new List<float>();
radarAngles2 = new List<float>();
//radar = new List<float>();
//TODO make the number of slices adjustable
//define the radar sensors
radarAngles1.Add(315.0f);
radarAngles2.Add(405.0f);
radarAngles1.Add(45.0f);
radarAngles2.Add(135.0f);
radarAngles1.Add(135.0f);
radarAngles2.Add(225.0f);
radarAngles1.Add(225.0f);
radarAngles2.Add(315.0f);
for (int i = 0; i < 4; i++)
{
SignalSensor s = new SignalSensor(o);
signalsSensors.Add(s);
o.sensors.Add(signalsSensors[i]);
}
owner = o;
}
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 GameScreen(ContentManager Content, Texture2D Background, Player player, Robot robot, bool OnLeftSide)
{
this.Content.Unload();
this.Content.Dispose();
this.Content = Content;
//Do some stuff to Left and Right before adding them
if (OnLeftSide)
{
player.Position = new Vector2(60, 120);
robot.Position = new Vector2(730, 120);
}
else
{
player.Position = new Vector2(730, 120);
robot.Position = new Vector2(60, 120);
}
FlyingDisc flyingDisc = new FlyingDisc(Content.Load<Texture2D>("General\\FlyingDisc"));
flyingDisc.Position = new Vector2(70, 110);
Entities = new List<Entity>(new List<Entity> { player, robot, flyingDisc });
this.Background = Background;
}
public PreySensor(double angle, Robot owner, double maxRange, double noise)
{
this.angle = angle;
this.owner = owner;
this.maxRange = maxRange;
this.noise = noise/100.0;
this.seePrey = false;
this.distance = maxRange;
}
public RangeFinder(double a,Robot o,double _max_range)
{
owner =o;
angle = a;
max_range = _max_range;///EPUCK 15.0; 30 works //epuck radius = 3.8
distance = (-1);
noise = 0.0;
}
public Radar(double sA, double eA,Robot o)
{
owner = o;
startAngle = sA;
endAngle = eA;
max_range = 100.0;
distance = (-1);
noise = 0.0;
}
public RangeFinder(double a,Robot o,double _max_range,double _noise)
{
seeRobot=false;
owner =o;
angle = a;
max_range = _max_range;///EPUCK 15.0; 30 works //epuck radius = 3.8
distance = (-1);
noise = _noise / 100.0;
}
public EnemyRobot(Robot bot, Boolean flee)
: base()
{
this.flee = flee;
name = "EnemyRobot";
default_speed = 25.0f;
default_turn_speed = 9.0f;
actualRange = 40.0f;
collisionAvoidance = false;
autopilot = false;
addtimer = false;
evolved = bot;
// This is set by EngineUtilities when a collision occurs
collisionWithEvolved = false;
}
double nearest(instance_pack ip, Robot robot,Environment env) {
double nd=1000000.0;
bool found =false;
Point2D p = robot.location;
foreach(Robot r in ip.robots) {
if(r != robot && env.AOIRectangle.Contains((int)r.location.x,(int)r.location.y)) {
double d=r.location.distance(p);
if(d<nd) nd=d;
found=true;
//nd+=d;
}
}
if(found)
return nd;
else
return 0.0;
}
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 override bool RobotCollide (Robot robot)
{
foreach (Wall wall in env.walls)
{
if (EngineUtilities.collide(robot, wall))
{
return true;
}
}
if(!agentCollide) return false;
foreach (Robot robot2 in rbts)
{
if (robot == robot2)
continue;
if (EngineUtilities.collide(robot, robot2))
{
return true;
}
}
return false;
}
public override bool RobotCollide(Robot robot) {
List<collision_grid_square> squares =
grid.circle(robot.location.x,robot.location.y,robot.radius);
List<Wall> checked_walls = new List<Wall>();
List<Robot> checked_robots = new List<Robot>();
foreach(collision_grid_square square in squares)
{
//TODO
if(this.agentCollide)
foreach(SimulatorObject o in square.dynamic_objs)
{
Robot r = (Robot)o;
if (r == robot)
continue;
if (!checked_robots.Contains(r))
{
if (EngineUtilities.collide(r,robot))
return true;
checked_robots.Add(r);
}
}
foreach(SimulatorObject o in square.static_objs)
{
Wall w = (Wall)o;
if(!checked_walls.Contains(w))
{
if(EngineUtilities.collide(robot,w))
return true;
checked_walls.Add(w);
}
}
}
return false;
}
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.circle.collide(b.circle);
}
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.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 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));
}
public static bool collide(Robot a, Wall b)
{
return(EngineUtilities.collide(b, a));
}
public abstract bool RobotCollide(Robot r);
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);
}
public abstract double Raycast(double angle, double max_range, Point2D point, Robot owner, out SimulatorObject hit);
public EnemyPieSliceSensorArray(Robot o) : base(o)
{
// Just call parent constructor
}
public override void Update(GameTime gameTime)
{
if (Updates)
{
#region Rectangles
Vector2 BBottomRight = new Vector2(Backgrounds[Bindex].Value.Width, Backgrounds[Bindex].Value.Height);
BRectangle = new Rectangle(Graphics.Width / 2 - (int)BBottomRight.X / 4, (int)BBottomRight.Y / 4, (int)BBottomRight.X / 2, (int)BBottomRight.Y / 2);
Vector2 PleftBottomRight = new Vector2(Characters[Pleftindex].Value.Width, Characters[Pleftindex].Value.Height);
Vector2 PrightBottomRight = new Vector2(Characters[Prightindex].Value.Width, Characters[Prightindex].Value.Height);
PleftRectangle = new Rectangle(Graphics.Width / 4 - (int)PleftBottomRight.X / 2 - 60, Graphics.Height / 4 * 3 - (int)PleftBottomRight.Y / 2, (int)PleftBottomRight.X, (int)PleftBottomRight.Y);
PrightRectangle = new Rectangle(Graphics.Width / 4 * 3 - (int)PrightBottomRight.X / 2 + 60, Graphics.Height / 4 * 3 - (int)PrightBottomRight.Y / 2, (int)PrightBottomRight.X, (int)PrightBottomRight.Y);
StartTripRectangle = new Rectangle(Graphics.Width / 2 - StartTripTex.Width / 2, Graphics.Height / 4 * 3 - StartTripTex.Height, StartTripTex.Width, StartTripTex.Height);
StartTripRectangle.Inflate(80, 40);
#endregion
foreach (TouchLocation tl in Input.Touches)
{
#region Selector
if (tl.State == TouchLocationState.Pressed)
{
if (Graphics.Device.Viewport.Bounds.Contains((int)tl.Position.X, (int)tl.Position.Y))
{
if (Utilities.PadRectangle(PleftRectangle, 1.2f, 1.03f).Contains((int)tl.Position.X, (int)tl.Position.Y))
{
if (OnLeftSide)
{
if (Pleftindex >= Characters.Count - 1) Pleftindex = 0; else Pleftindex++;
}
else
{
OnLeftSide = true;
}
}
else if (Utilities.PadRectangle(PrightRectangle, 1.25f, 1.04f).Contains((int)tl.Position.X, (int)tl.Position.Y))
{
//This code is for the right player selection
//if (Prightindex >= Characters.Count - 1) Prightindex = 0; else Prightindex++;
if (!OnLeftSide)
{
if (Pleftindex >= Characters.Count - 1) Pleftindex = 0; else Pleftindex++;
}
else
{
OnLeftSide = false;
}
}
else if (StartTripRectangle.Contains((int)tl.Position.X, (int)tl.Position.Y))
{
StartPressed = true;
startId = tl.Id;
}
else
{
if (Bindex >= Backgrounds.Count - 1) Bindex = 0; else Bindex++;
}
}
}
if (tl.State == TouchLocationState.Released)
{
if (!StartTripRectangle.Contains((int)tl.Position.X, (int)tl.Position.Y))
{
if (startId == tl.Id)
{
StartPressed = false;
}
}
if (StartTripRectangle.Contains((int)tl.Position.X, (int)tl.Position.Y))
{
if (startId == tl.Id)
{
StartPressed = false;
Prightindex = Rand.Next(Characters.Count - 1);
Texture2D leftTex = Characters[Pleftindex].Value;
Texture2D rightTex = Characters[Prightindex].Value; //Just make it random
Texture2D Background = Backgrounds[Bindex].Value;
Backgrounds.RemoveAt(Bindex);
//logic is wierd here since upon removing indices change
//Consider revisiting seperate content managers per Screen
if (Pleftindex == Prightindex)
{
Characters.RemoveAt(Pleftindex);
}
else if (Pleftindex < Prightindex)
{
Prightindex--;
Characters.RemoveAt(Pleftindex);
Characters.RemoveAt(Prightindex);
}
else if (Pleftindex > Prightindex)
{
Characters.RemoveAt(Pleftindex);
Characters.RemoveAt(Prightindex);
}
//Manually dispose of unneeded content
foreach (var pair in Backgrounds) pair.Value.Dispose();
foreach (var pair in Characters) pair.Value.Dispose();
StartTripPressedTex.Dispose();
StartTripTex.Dispose();
Player player = new Player(leftTex);
Robot robot = new Robot(rightTex, player);
//Exceptions can throw if you try to draw disposed content
base.Updates = false;
base.Draws = false;
GameState.AddScreen(new LoadingScreen(new GameScreen(Content, Background, player, robot, OnLeftSide)));
GameState.RemoveScreen(this);
}
}
}
#endregion
}
}
}
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 abstract double Raycast(double angle, double max_range, Point2D point, Robot owner,out SimulatorObject hit);
public abstract bool RobotCollide(Robot r);
public override double Raycast(double angle, double max_range, Point2D point, Robot owner, out SimulatorObject hit) {
RaycastGrid(angle+3.14,max_range/2.0,point,owner,true,out hit);
RaycastGrid(angle,max_range/2.0,point,owner,true,out hit);
double dist= RaycastGrid(angle,max_range,point,owner,false,out hit);
return dist;
}
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();
}
}
public static bool collide(Robot a, Robot b)
{
// Schrum: The "undo" that happens on collisions makes it hard for
// all robots to be aware of collisions, and to be aware of with whom
// they collided. Therefore, I changed this code so that the evolved
// bot will tell an enemy when it collides with it.
bool result = a.circle.collide(b.circle);
if (result && b is EnemyRobot && !(a is EnemyRobot))
{
((EnemyRobot)b).collisionWithEvolved = true;
}
return result;
}
//Robots need to be registered before they can receive ANN results
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");
}
}
private void functionComboBox_SelectedIndexChanged(object sender, EventArgs e)
{
robot = RobotModelFactory.getRobotModel(robotModelComboBox.SelectedItem.ToString());
}
public static bool collide(Robot a, Robot b)
{
return(a.circle.collide(b.circle));
}
public SignalSensor(Robot o)
{
owner=o;
val=0.0;
}
public TimeSensor(Robot owner, double noise, double timestep)
{
this.owner = owner;
this.noise = noise;
this.timestep = timestep;
}
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)));
}