simplecyesim.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

			bodyy[j] = body[j].y + ypix;
			}
		Vector bodyVector = new Vector();
		for(j=0;j<4;j++)
			{
			vertex = new Vec2(bodyx[j], bodyy[j]);
			bodyVector.addElement(vertex);
			}
		cyeBody = new Polygon2(bodyVector);

		double txpix = trailer_pp.x;
		double typix = trailer_pp.y;


		Vec2[] tbody = new Vec2[5];     // outline of trailer
		double[] tbodyx = new double[5];
		double[] tbodyy = new double[5];
		tbody[0] = new Vec2(TRAILER_FRONT, 0); 
		tbody[4] = new Vec2(-1*TRAILER_FRONT, TRAILER_WIDTH/2);
		tbody[3] = new Vec2(-1*TRAILER_LENGTH, TRAILER_WIDTH/2);
		tbody[2] = new Vec2(-1*TRAILER_LENGTH, -1*TRAILER_WIDTH/2);
		tbody[1] = new Vec2(-1*TRAILER_FRONT,  -1*TRAILER_WIDTH/2);
		for(k = 0; k<5; k++) // scale and rotate
			{
			newAng = tbody[k].t + trailer_steer.t;
			if (newAng >= 2*Math.PI)
				{
				newAng-= 2*Math.PI;
				}
			tbody[k].sett(newAng);
			tbodyx[k] = tbody[k].x + txpix;
			tbodyy[k] = tbody[k].y + typix;
			}
		Vector trailerVector = new Vector();
		for(k=0;k<5;k++)
			{
			vertex = new Vec2(tbodyx[k], tbodyy[k]);
			trailerVector.addElement(vertex);
			}
		trailer = new Polygon2(trailerVector);
		}


	/*--- From SimulatedObject ---*/

	double dsignum(double a)
		{
		if (a < 0.0)
			{
			return -1.0;
			}
		else
			{
			return 1.0;
			}
		}

	double sgn(double a)
           {
	     if (a < 0.0)
	       return -1.0;
	     if (a > 0.0)
	       return 1.0;
	     else
	       return 0.0;
            } 

	public boolean isObstacle()
		{
		return(true);
		}
	
	public boolean isPushable()
		{
		return(false);
		}
	
	public boolean isPickupable()
		{
		return(false);
		}
	
	public Vec2 getClosestPoint(Vec2 from)
		{
		int numberEdges = cyeBody.vertices.size(); // n edges if n vertices (as vertex n+1 wraps round to vertex 0)
		double scale;
		int i;
		Vector closestPts = new Vector(); 
		Vec2 closest, vertex1, vertex2, vec1, vector2;
		for (i=0;i<numberEdges;i++)
			{
			vertex1 = (Vec2)cyeBody.vertices.elementAt(i);
			vertex2 = (Vec2)cyeBody.vertices.elementAt((i+1)%numberEdges);
			vec1 = new Vec2(vertex2.x-vertex1.x, vertex2.y-vertex1.y);
			vector2 = new Vec2(from.x-vertex1.x, from.y-vertex1.y);
			scale = ((vec1.x*vector2.x)+(vec1.y*vector2.y))/((vec1.x*vec1.x)+(vec1.y*vec1.y));
			// if closest pt lies within vertices add, if scale is negative, set closest point to vertex 1, if it is > 1.0 then point not in line, set equal to vertex2
			if (scale <= 0.0) {
				closest = new Vec2(0,0); // set to vertex1
			} else if (scale >= 1.0) {
				closest = new Vec2(vec1); // set to vertex2 relative to vertex1
			} else {
				closest = new Vec2(scale*vec1.x, scale*vec1.y);
			}
			closest.add(vertex1); // absolute position of closest point
			closestPts.addElement(closest);
		}
		numberEdges = trailer.vertices.size(); // n edges if n vertices (as vertex n+1 wraps round to vertex 0)
		for (i=0;i<numberEdges;i++)
			{
			vertex1 = (Vec2)trailer.vertices.elementAt(i);
			vertex2 = (Vec2)trailer.vertices.elementAt((i+1)%numberEdges);
			vec1 = new Vec2(vertex2.x-vertex1.x, vertex2.y-vertex1.y);
			vector2 = new Vec2(from.x-vertex1.x, from.y-vertex1.y);
			scale = ((vec1.x*vector2.x)+(vec1.y*vector2.y))/((vec1.x*vec1.x)+(vec1.y*vec1.y));
			// if closest pt lies within vertices add, if scale is negative, set closest point to vertex 1, if it is > 1.0 then point not in line, set equal to vertex2
			if (scale <= 0.0) {
				closest = new Vec2(0,0); // set to vertex1
			} else if (scale >= 1.0) {
				closest = new Vec2(vec1); // set to vertex2 relative to vertex1
			} else {
				closest = new Vec2(scale*vec1.x, scale*vec1.y);
			}
			closest.add(vertex1); // absolute position of closest point
			closestPts.addElement(closest);
			}
		Vec2 tempDistance = new Vec2(position);
		tempDistance.sub(from);
		double distance = tempDistance.r;
		closest = new Vec2(position);
		closest.sub(from);
		Vec2 temp;
		for(i=0;i<closestPts.size();i++)
			{
			temp = (Vec2)closestPts.elementAt(i);
			temp.sub(from);
			if(temp.r <= distance) 
				{
				distance = temp.r;
				closest = new Vec2(temp); 
				}
			}
		return closest;
	
	}	
        /**
	 * determine if the object is intersecting with a specified circle.
	 * This is useful for obstacle avoidance and so on.
	 * @param c the circle which may be intersecting the current object.
	 * @return true if collision detected.
         */
	public boolean checkCollision(Circle2 c)
	    {
		// set up p to be correct vertices
		Vec2 vertex1, vertex2, vec1, vector2, closestPt;
		int numberEdges = cyeBody.vertices.size(); // n edges if n vertices (as vertex n+1 wraps round to vertex 0)
		double scale;
		int i;

		for (i=0;i<numberEdges;i++)
			{
			vertex1 = (Vec2)cyeBody.vertices.elementAt(i);
			vertex2 = (Vec2)cyeBody.vertices.elementAt((i+1)%numberEdges);
			vertex1.sub(c.centre);
			vertex2.sub(c.centre);
			// if either vertex is within the circles radius you are colliding
			if ((vertex1.r < c.radius) || (vertex2.r < c.radius))
				{
				return true;
				} 
			vertex1.add(c.centre);
			vertex2.add(c.centre);
			vec1 = new Vec2(vertex2);
			vec1.sub(vertex1);
			vector2 = new Vec2(c.centre);
			vector2.sub(vertex1);
			scale = ((vec1.x*vector2.x)+(vec1.y*vector2.y))/((vec1.x*vec1.x)+(vec1.y*vec1.y));
			closestPt = new Vec2(scale*vec1.x, scale*vec1.y);
			closestPt.add(vertex1); // absolute position of closest point
			closestPt.sub(c.centre); // position of closest point relative to centre of current object
			if (closestPt.r < c.radius)
				{
				// now need to check if closestPt lies between vertex1 and vertex2
				// i.e. it could lie on vector between them but outside of them
				if ( (scale > 0.0) && (scale < 1.0) )
					{
					return true;
					}
				}
			}

		numberEdges = trailer.vertices.size(); // n edges if n vertices (as vertex n+1 wraps round to vertex 0)
		for (i=0;i<numberEdges;i++)
			{
			vertex1 = (Vec2)trailer.vertices.elementAt(i);
			vertex2 = (Vec2)trailer.vertices.elementAt((i+1)%numberEdges);
			vec1 = new Vec2(vertex2);
			vec1.sub(vertex1);
			vector2 = new Vec2(c.centre);
			vector2.sub(vertex1);
			scale = ((vec1.x*vector2.x)+(vec1.y*vector2.y))/((vec1.x*vec1.x)+(vec1.y*vec1.y));
			closestPt = new Vec2(scale*vec1.x, scale*vec1.y);
			closestPt.add(vertex1); // absolute position of closest point
			closestPt.sub(c.centre); // position of closest point relative to centre of current object
			if (closestPt.r <= c.radius)
				{
				// now need to check if closestPt lies between vertex1 and vertex2
				// i.e. it could lie on vector between them but outside of them
				closestPt.add(c.centre);
				vertex1.sub(c.centre);
				vertex2.sub(c.centre);
				if ( (vertex1.r <= c.radius) || (vertex2.r <= c.radius) || (trailer.pointWithinPolygon(closestPt)))
					{
					return true;
					}
				}
			}

		return false; // closest point to object on each edge of polygon not within object			
	    }

        /**
	 * determine if the object is intersecting with a specified polygon.
	 * This is useful for obstacle avoidance and so on.
	 * @param p the polygon which may be intersecting the current object.
	 * @return true if collision detected.
         */
	public boolean checkCollision(Polygon2 p)
		{
		int i = 0;
		Vec2 vertex1, vertex2;
		while(i<p.vertices.size())
			{
			vertex1 = (Vec2)p.vertices.elementAt(i);
			vertex2 = (Vec2)p.vertices.elementAt((i+1)%(p.vertices.size()));
			if(cyeBody.lineIntersectsWithPolygon(vertex1, vertex2))
				{
				return true;
				}
			if(trailer.lineIntersectsWithPolygon(vertex1, vertex2))
				{
				return true;
				}
			i++;
			}
		return false;
		}

	public Vec2 getCenter(Vec2 from)
		{
		Vec2 tmp = new Vec2(position.x, position.y);
		tmp.sub(from);
		return(tmp);
		}

	public void push(Vec2 d, Vec2 v)
		{
		// sorry no pushee robots!
		}

	public void pickUp(SimulatedObject o)
		{
		// sorry no pickupee robots!
		}

	public void putDown(Vec2 p)
		{
		// sorry no put downee robots!
		}

	public void setVisionClass(int v)
		{
		visionclass = v;
		}

	public int getVisionClass()
		{
		return(visionclass);
		}


	/**
         * Draw the robot's ID.
         */
        public void drawID(Graphics g, int w, int h,
                double t, double b, double l, double r)
                {
                top =t; bottom =b; left =l; right =r;
                if (DEBUG) System.out.println("draw "+
                        w + " " +
                        h + " " +
                        t + " " +
                        b + " " +
                        l + " " +
                        r + " ");
                double meterspp = (r - l) / (double)w;
                int radius = (int)(RADIUS / meterspp);
                int xpix = (int)((position.x - l) / meterspp);
                int ypix = (int)((double)h - ((position.y - b) / meterspp));

                /*--- draw ID ---*/
                g.setColor(background);
                g.drawString(String.valueOf(getPlayerNumber(0))
			,xpix-radius,ypix-radius);
                }



        /**
         * Draw the object as an icon.
         * Default is just to do a regular draw.
         */
        public void drawIcon(Graphics g, int w, int h,
                double t, double b, double l, double r)
                {
                draw(g, w, h, t, b, l, r);
                }



        private Point last = new Point(0,0);
        /**
         * Draw the robot's Trail.
         */
        public void drawTrail(Graphics g, int w, int h,
                double t, double b, double l, double r)
		{
                top =t; bottom =b; left =l; right =r;
                if (DEBUG) System.out.println("draw "+
                        w + " " +
                        h + " " +
                        t + " " +
                        b + " " +
                        l + " " +
                        r + " ");
                double meterspp = (r - l) / (double)w;
                int xpix = (int)((position.x - l) / meterspp);
                int ypix = (int)((double)h - ((position.y - b) / meterspp));

                /*--- record the point ---*/
                Point p = new Point(xpix,ypix);
                if ((last.x!=xpix)||(last.y!=ypix))
                        trail.put(p);
                last = p;

                /*--- get the list of all points ---*/
                Enumeration point_list = trail.elements();

                /*--- draw the trail ---*/
                g.setColor(background);
                Point from = (Point)point_list.nextElement();
                while (point_list.hasMoreElements())
                        {
                        Point next = (Point)point_list.nextElement();
                        g.drawLine(from.x,from.y,next.x,next.y);
                        from = next;
                        }
		}



	private String display_string = "blank";
	/**
         * Set the String that is printed on the robot's display.
         * For simulated robots, this appears printed below the agent
         * when view "Robot State" is selected.
         * @param s String, the text to display.
         */
        public void setDisplayString(String s)
		{
		display_string = s;
		}


        /**
         * Draw the robot's state.
         */
        public void drawState(Graphics g, int w, int h,
                double t, double b, double l, double r)
                {
                top =t; bottom =b; left =l; right =r;
                if (DEBUG) System.out.println("draw "+
                        w + " " +
                        h + " " +
                        t + " " +
                        b + " " +
                        l + " " +
                        r + " ");
                double meterspp = (r - l) / (double)w;
                int radius = (int)(RADIUS / meterspp);
                int xpix = (int)((position.x - l) / meterspp);
                int ypix = (int)((double)h - ((position.y - b) / meterspp));

                /*--- draw State ---*/
                g.setColor(background);
                g.drawString(display_string,xpix+radius+3,ypix-radius);
                
		/*--- draw the vectors if any ---*/
		displayVectors.draw(g, w, h, t, b, l, r);
		}


	/**
         * Set the length of the trail (in movement steps).
         * @param l int, the length of the trail.
         */
        public void setTrailLength(int l)
		{
		trail = new CircularBuffer(l);
		}


        /**
         * Clear the trail.
         */
        public void clearTrail()
		{
		trail.clear();
		}


        /**
         * Draw the robot in a specific spot.
         */
        public void draw(Vec2 pos, Graphics g, int w, int h,
                double t, double b, double l, double r)
                {
                Vec2 old_pos = position;
                position = pos;
                draw(g,w,h,t,b,l,r);
                position = old_pos;
                }


	/**
	 * Draw the robot.
	 */
	public void draw(Graphics g, int w, int h,
		double t, double b, double l, double r)
	{
		top =t; bottom =b; left =l; right =r;
		if (DEBUG) System.out.println("draw "+
			w + " " +
			h + " " +
			t + " " +
			b + " " +
			l + " " +
			r + " ");
		double meterspp = (r - l) / (double)w;
		if (DEBUG) System.out.println("meterspp "+meterspp);
		int radius = (int)(0.203 / meterspp);
		int visionr = (int)(SimpleCye.VISION_RANGE/meterspp);
		int xpix = (int)((position.x - l) / meterspp);
		int ypix = (int)((double)h - ((position.y - b) / meterspp));
		if (DEBUG) System.out.println("robot at"+
			" at "+xpix+","+ypix);

		/*--- draw the main body ---*/
		g.setColor(foreground);
		Vec2[] body = new Vec2[4];     // outline of body
		int[] bodyx = new int[4];
		int[] bodyy = new int[4];
		body[0] = new Vec2(WIDTH/2, LENGTH/2); 
		body[1] = new Vec2(-1*WIDTH/2, LENGTH/2);
		body[2] = new Vec2(-1*WIDTH/2, -1*LENGTH/2);
		body[3] = new Vec2(WIDTH/2,  -1*LENGTH/2);
		for(int j = 0; j<4; j++) // scale and rotate