コード例 #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;
		}
コード例 #2
0
        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;
                    }
                }
            }
        }
コード例 #3
0
ファイル: Robot.cs プロジェクト: jal278/agent_multimodal
          public void init(int id, double nx, double ny, double dir, 
            AgentBrain agentBrain, Environment environment,
            float sensorNoise, float effectorNoise, float headingNoise, float timeStep)
        {
            steps = 0;

            this.id = id;
    	    radius = defaultRobotSize();
            location = new Point2D(nx, ny);
            circle = new Circle2D(location, radius);
            old_location = new Point2D(location);
           
            heading = dir;
            velocity = 0.0;
            collide_last = false;
            this.timeStep = timeStep;
            this.environment = environment;
            this.agentBrain = agentBrain;
            this.sensorNoise = sensorNoise;
            this.effectorNoise = effectorNoise;
            this.headingNoise = headingNoise;
            populateSensors();
			if(environment.seed != -1) 
            	rng=environment.rng; //new SharpNeatLib.Maths.FastRandom(environment.seed); //Utilities.random;
        	else
				rng=environment.rng; //new SharpNeatLib.Maths.FastRandom();
		}
コード例 #4
0
 public FlockingFitness()
 {
     //Initialize variables
     avgLoc = null;
     reachedGoal = false;
     inFormation = 0;
     formHeight = 0.0;
 }
コード例 #5
0
		double nearest(instance_pack ip, double x, double y) {
			double nd=10000000.0;
			Point2D p = new Point2D(x,y);
			foreach(Robot r in ip.robots) {
				double d=r.location.distance(p);
				if(d<nd) nd=d;
			}
			return nd;
		}
コード例 #6
0
ファイル: Prey.cs プロジェクト: jtglaze/IndependentWork2013
        public Prey(double x, double y, double rad, double range, double speed, bool vis, string n)
        {
            this.name = n;
            this.colored = false;
            this.dynamic = true;
            this.caught = false;
            this.visible = vis;
            this.speed = speed;
            this.range = range;

            Point2D p = new Point2D(x, y);
            this.circle = new Circle2D(p, rad);
            this.location = circle.p;
        }
コード例 #7
0
ファイル: Wall.cs プロジェクト: jal278/agent_multimodal
        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;
        }
コード例 #8
0
		//rotate this point about another point
		public void rotate(double angle, Point2D point)
		{
			x-=point.x;
			y-=point.y;
			
			double ox = x;
			double oy = y;

			x=Math.Cos(angle)*ox - Math.Sin(angle)*oy;
			y=Math.Sin(angle)*ox + Math.Cos(angle)*oy;
			
			x+=point.x;
			y+=point.y;
		}
コード例 #9
0
        //Different from PieSliceSensorArray in that EnemyRobots are used as targets instead
        // of goal points. 
        public void update(Engine.Environment env, List<Robot> robots, CollisionManager cm)
        {
            List<float> translatedEnemyAngles = new List<float>();
            // Start at 1: Assume all robots after first are enemies
            for (int i = 1; i < robots.Count; i++)
            {
                EnemyRobot er = (EnemyRobot) robots[i];
                if (!er.stopped) // Only sense prey robots that are active
                {
                    Point2D temp = new Point2D((int)er.location.x, (int)er.location.y);

                    temp.rotate((float)-(owner.heading), owner.circle.p);  //((float)-(owner.heading * 180.0 / 3.14), owner.circle.p);

                    //translate with respect to location of navigator
                    temp.x -= (float)owner.circle.p.x;
                    temp.y -= (float)owner.circle.p.y;

                    //what angle is the vector between target & navigator
                    float angle = angleValue((float)temp.x, (float)temp.y);// (float)temp.angle();

                    translatedEnemyAngles.Add(angle);
                }
            }

            //fire the appropriate radar sensor
            for (int i = 0; i < radarAngles1.Count; i++)
            {
                signalsSensors[i].setSignal(0.0);

                for (int a = 0; a < translatedEnemyAngles.Count; a++)
                {
                    float angle = translatedEnemyAngles[a];

                    if (angle >= radarAngles1[i] && angle < radarAngles2[i])
                    {
                        signalsSensors[i].setSignal(1.0);
                    }

                    if (angle + 360.0 >= radarAngles1[i] && angle + 360.0 < radarAngles2[i])
                    {
                        signalsSensors[i].setSignal(1.0);
                    }
                }
            }

        }
コード例 #10
0
        //per default we use the goal point of the environment as our target point. 
        public void update(Engine.Environment env, List<Robot> robots, CollisionManager cm)
        {
            Point2D temp = new Point2D((int)env.goal_point.x, (int)env.goal_point.y);

            temp.rotate((float)-(owner.heading), owner.circle.p);  //((float)-(owner.heading * 180.0 / 3.14), owner.circle.p);

            //translate with respect to location of navigator
            temp.x -= (float)owner.circle.p.x;
            temp.y -= (float)owner.circle.p.y;

            //what angle is the vector between target & navigator
            float angle = angleValue((float)temp.x, (float) temp.y);// (float)temp.angle();

        //!    angle *= 57.297f;//convert to degrees

            //fire the appropriate radar sensor
            for (int i = 0; i < radarAngles1.Count; i++)
            {
                signalsSensors[i].setSignal(0.0);
                //radar[i] = 0.0f;

                if (angle >= radarAngles1[i] && angle < radarAngles2[i])
                {
                    signalsSensors[i].setSignal(1.0);
                 //   Console.WriteLine(i);
                }
                //radar[i] = 1.0f;

                if (angle + 360.0 >= radarAngles1[i] && angle + 360.0 < radarAngles2[i])
                {
                    signalsSensors[i].setSignal(1.0);
                   // Console.WriteLine(i);
                }
//                    radar[i] = 1.0f;

                
               // inputs[sim_engine.robots[0].rangefinders.Count + i] = sim_engine.radar[i];

            }

        }
コード例 #11
0
        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();
            }
        }
コード例 #12
0
 public abstract double Raycast(double angle, double max_range, Point2D point, Robot owner,out SimulatorObject hit);
コード例 #13
0
        //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;
        }
コード例 #14
0
 public static double euclideanDistance(Point2D p1, Point2D p2)
 {
     return Math.Sqrt(Math.Pow(p1.x - p2.x, 2) + Math.Pow(p1.y - p2.y, 2));
 }
コード例 #15
0
		public Circle2D(Circle2D other)
		{
			p=other.p;
			radius=other.radius;
		}
コード例 #16
0
ファイル: poi.cs プロジェクト: jal278/agent_multimodal
		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;		
		}
コード例 #17
0
ファイル: Prey.cs プロジェクト: jtglaze/IndependentWork2013
        public void update(List<Robot> rbts, double timestep)
        {
            if (start == null) start = new Circle2D(circle);

            Robot closest = null;
            double closestDist = range;
            foreach (Robot r in rbts)
            {
                double dist = circle.p.distance(r.location);
                //Console.WriteLine("dist: " + dist);
                if (dist <= closestDist)
                {
                    closestDist = dist;
                    closest = r;
                }
            }

            if (closestDist <= (circle.radius + rbts[0].radius))
            {
                caught = true;
                dynamic = false;
                return;
            }

            if (closest != null)
            {
                double dA = circle.p.x - closest.location.x;
                double dB = circle.p.y - closest.location.y;
                double c = closest.location.distance(circle.p);

                double z = speed * timestep;
                double dX = (dA * z) / c;
                double dY = (dB * z) / c;

                Point2D newP = new Point2D(dX + circle.p.x, dY + circle.p.y);
                circle.p = newP;
            }
        }
コード例 #18
0
		public Line2D(Point2D a, Point2D b)
		{
			p1=a;
			p2=b;
		}
コード例 #19
0
		//what is the distance from this line to a point
		public double distance(Point2D n)
		{
			return Math.Sqrt(distance_sq(n));
		}
コード例 #20
0
		//what is the squared distance from this line to a point 
		public double distance_sq(Point2D n)
		{
			double utop = (n.x-p1.x)*(p2.x-p1.x)+(n.y-p1.y)*(p2.y-p1.y);
			double ubot = p1.distance_sq(p2);
			double u = utop/ubot;
			
			if(u<0 || u> 1)
			{
				double d1 = p1.distance_sq(n);
				double d2 = p2.distance_sq(n);
				if(d1<d2) return d1;
				return d2;
			}
			Point2D p=new Point2D(0.0,0.0);
			p.x=p1.x+u*(p2.x-p1.x);
			p.y=p1.y+u*(p2.y-p1.y);
			return p.distance_sq(n);
		}
コード例 #21
0
		//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;
			}
		}
コード例 #22
0
		public Circle2D(Point2D a, double rad)
		{
			radius=rad;
			p=a;
		}
コード例 #23
0
        public void reset()
        {
            seed = 0;
            view_x = 0.0f;
            view_y = 0.0f;
            view_scale = 5.0f;
            AOIRectangle = new Rectangle(30, 60, 640, 500);

            group_orientation = 0;
            robot_spacing = 30;
            robot_heading = 0;
            rng = new SharpNeatLib.Maths.FastRandom();
            walls = new List<Wall>();
            preys = new List<Prey>();

            POIPosition = new List<Point>();

            start_point = new Point2D(0, 0);
            goal_point = new Point2D(100, 100);
        }
コード例 #24
0
		//what is the squared dist to another point
		public double distance_sq(Point2D point)
		{
			double dx=point.x-x;
			double dy=point.y-y;
			return dx*dx+dy*dy;
		}
コード例 #25
0
 // Schrum2: Vector subtraction of one point from another
 public Point2D subtract(Point2D other) {
     return new Point2D(x - other.x, y - other.y);
 }
コード例 #26
0
 public Point2D to_display(Point2D p)
 {
     float ox,oy;
         to_display((float)p.x,(float)p.y,out ox, out oy);
         return new Point2D((double)ox,(double)oy);
 }
コード例 #27
0
		//what is the distance to another point
		public double distance(Point2D point)
		{
			return Math.Sqrt(distance_sq(point));
		}
コード例 #28
0
        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;
        }
コード例 #29
0
		public Point2D(Point2D another)
		{
			x=another.x;
			y=another.y;
		}
コード例 #30
0
		public Line2D(Line2D other)
		{
			p1=other.p1;
			p2=other.p2;
		}