Exemplo n.º 1
0
		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;
		}
Exemplo n.º 2
0
        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.");
            }
        }
Exemplo n.º 3
0
        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);
        }
Exemplo n.º 4
0
        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);
        }
Exemplo n.º 5
0
 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);
Exemplo n.º 7
0
        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.");
            }
        }
Exemplo n.º 8
0
		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;
		}
Exemplo n.º 9
0
		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;
		}
Exemplo n.º 10
0
        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);
        }