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 insert_into_grid(SimulatorObject obj)
{
bool dynamic = obj.dynamic;
if (obj is Wall)
{
Wall w = (Wall)obj;
foreach (collision_grid_square square in cast_ray(w.line.p1.x, w.line.p1.y, w.line.p2.x, w.line.p2.y))
{
if (dynamic)
{
square.dynamic_objs.Add(obj);
}
else
{
square.static_objs.Add(obj);
}
}
}
else if (obj is Robot)
{
Robot r = (Robot)obj;
foreach (collision_grid_square square in circle(r.location.x, r.location.y, r.radius))
{
if (dynamic)
{
square.dynamic_objs.Add(obj);
}
else
{
square.static_objs.Add(obj);
}
}
}
else
{
Console.WriteLine("Object not supported by collision grid.");
}
}
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);
}
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 double Raycast(double angle, double max_range, Point2D point, Robot owner,out SimulatorObject hit);
public void insert_into_grid(SimulatorObject obj)
{
bool dynamic = obj.dynamic;
if (obj is Wall)
{
Wall w = (Wall)obj;
foreach (collision_grid_square square in cast_ray(w.line.p1.x, w.line.p1.y, w.line.p2.x, w.line.p2.y))
{
if (dynamic)
square.dynamic_objs.Add(obj);
else
square.static_objs.Add(obj);
}
}
else if (obj is Robot)
{
Robot r = (Robot)obj;
foreach (collision_grid_square square in circle(r.location.x, r.location.y, r.radius))
{
if (dynamic)
square.dynamic_objs.Add(obj);
else
square.static_objs.Add(obj);
}
}
else
{
Console.WriteLine("Object not supported by collision grid.");
}
}
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 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;
}
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);
}
