simplen150sim.java

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

/*
 * SimpleN150Sim.java
 */

package EDU.gatech.cc.is.abstractrobot;

import java.awt.*;
import java.util.Enumeration;

import EDU.gatech.cc.is.util.*;
import EDU.gatech.cc.is.simulation.*;
import EDU.gatech.cc.is.communication.*;

import EDU.cmu.cs.coral.simulation.*;
import EDU.cmu.cs.coral.abstractrobot.*;
import EDU.cmu.cs.coral.util.Polygon2;
import EDU.cmu.cs.coral.util.Circle2;


/**
 * SimpleN150Sim implements SimpleN150 for simulation.
 * Also includes code implementing communication, gripper and
 * vision.
 * <P>
 * <A HREF="../COPYRIGHT.html">Copyright</A>
 * (c)1998 Tucker Balch
 *
 * @see SimpleN150
 * @author Tucker Balch
 * @version $Revision: 1.6 $
 */

public class SimpleN150Sim extends Simple
	implements SimpleN150, SimulatedObject
	{
	private	CircularBuffer	trail;	// robot's trail
	private KinSensorSim	kin_sensor;	// senses our kin
	private TransceiverSim	transceiver;	// comm to other robots
	protected Vec2	position;
	protected Vec2	steer;
	private	double	turret;
	private	double	speed;
	protected Color	foreground, background;
	private long	time;
	private double	timed;
	protected double left, right, top, bottom;
	private	SimulatedObject[] all_objects = new SimulatedObject[0];
	private	int	visionclass;
	
	private	boolean	isWaiting;
	
	public	static final boolean DEBUG = false;
	 
	/**
	 * Instantiate a <B>SimpleN150Sim</B> object.  Be sure
	 * to also call init with proper values.
	 * @see SimpleN150Sim#init
	 */
        public SimpleN150Sim()
		{
		/*--- set parameters ---*/
                super(1);

		position = new Vec2(0,0);
		steer = new Vec2(1,0);
		turret = 0;
		foreground = Color.black;
		background = Color.black;
		if (DEBUG) System.out.println("SimpleN150Sim: instantiated.");

		/*--- set default bounds ---*/
		top = 1000;
		bottom = -1000;
		left = -1000;
		right = 1000;
		}	


    /**
     * Initialize a <B>SimpleN150Sim</B> object.
     */
	public void init(double xp, double yp, double tp, double ignore,
		Color f, Color b, int v, int i, long s)
		{
		trail = new CircularBuffer(1000);
		setID(i);
        kin_sensor = new KinSensorSim(this);
		transceiver = new TransceiverSim(this, this);
		position = new Vec2(xp,yp);
		steer = new Vec2(1,0);
		steer.sett(tp);
		turret = tp;
		foreground = f;
		background = b;
		time = 0;
		timed = 0;
		visionclass = v;
		
		isWaiting = false;
		
		if (DEBUG) System.out.println("SimpleN150Sim: initialized"
			+" at "+xp+","+yp);	
		}


	/**
	 * Take a simulated step;
	 */
	private double last_traversability = 1.0;
	public void takeStep(long time_increment, SimulatedObject[] all_objs)
		{
		if (DEBUG) System.out.println("SimpleN150Sim.TakeStep()");

		/*--- keep pointer to the other objects ---*/
		all_objects = all_objs;

		/*--- update the time ---*/
		time += time_increment;
		double time_incd = ((double)time_increment)/1000;
		timed += time_incd;

		/*--- update the steering ---*/
		double sturn = Units.BestTurnRad(steer.t, desired_heading);
		if (Math.abs(sturn) > (MAX_STEER*time_incd))
			{
			if (sturn<0)
				sturn = -MAX_STEER*time_incd;
			else sturn = MAX_STEER*time_incd;
			}
		steer.sett(steer.t + sturn);

		/*--- update the turret ---*/
		double tturn = Units.BestTurnRad(turret, desired_turret_heading);
		if (Math.abs(tturn) > (MAX_TURRET*time_incd))
			{
			if (tturn<0)
				tturn = -MAX_TURRET*time_incd;
			else tturn = MAX_TURRET*time_incd;
			}
		turret = Units.ClipRad(turret + tturn);

		/*--- compute velocity ---*/
		Vec2 velocity = new Vec2(steer.x, steer.y);
		if (in_reverse)
			velocity.setr(-base_speed * last_traversability	* desired_speed);
		else
			velocity.setr(base_speed * last_traversability * desired_speed);
		//System.out.println(velocity.r);
		// don't drive unless close turret aligned.
		//if (Math.abs(Units.BestTurnRad(turret,
                        //desired_turret_heading))>Math.PI/2.0)
			//velocity.setr(0.00001);

		/*--- compute a movement step ---*/
		Vec2 mvstep = new Vec2(velocity.x, velocity.y);
		mvstep.setr(mvstep.r * time_incd);

		/*--- test the new position to see if in bounds ---*/
		Vec2 pp = new Vec2(position.x, position.y);
		pp.add(mvstep);
		if (pp.x+RADIUS > right)
			{
			position.setx(right-RADIUS);
			velocity.setx(0);
			mvstep.setx(0);
			}
		else if (pp.x-RADIUS < left)
			{
			position.setx(left+RADIUS);
			velocity.setx(0);
			mvstep.setx(0);
			}

		if (pp.y+RADIUS > top)
			{
			position.sety(top-RADIUS);
			velocity.sety(0);
			mvstep.sety(0);
			}
		else if (pp.y-RADIUS < bottom)
			{
			position.sety(bottom+RADIUS);
			velocity.sety(0);
			mvstep.sety(0);
			}

		/*--- test the new position to see if on top of obstacle ---*/
		pp = new Vec2(position.x, position.y);
		boolean moveok = true;
		last_traversability = 1.0;
		pp.add(mvstep);
		for (int i=0; i<all_objects.length; i++)
			{
			if (all_objects[i].isObstacle() && 
				(all_objects[i].getID() != unique_id))
				{
				Vec2 tmp = all_objects[i].getClosestPoint(pp);
				if (tmp.r < RADIUS)
					{
					moveok = false;
					break;
					}
				}
			else if (all_objects[i] instanceof 
				SimulatedTerrainObject)
				{
				Vec2 tmp = all_objects[i].getClosestPoint(pp);
				if (tmp.r == 0) // on/in object
					last_traversability =
				((SimulatedTerrainObject)all_objects[i]).getTraversability();
				}
			}
		if (moveok) position.add(mvstep);

		/*--- test the new position to see if on top of pushable ---*/
		for (int i=0; i<all_objects.length; i++)
			{
			if (all_objects[i].isPushable() && 
				(all_objects[i].getID() != unique_id))
				{
				Vec2 tmp = all_objects[i].getClosestPoint(pp);
				if (tmp.r < RADIUS)
					{
					tmp.setr(RADIUS - tmp.r);
					all_objects[i].push(tmp, velocity);
					}
				}
			}

		/*--- check for object in gripper ---*/
		if (trigger_mode)
			setGripperFingers(-1,-1);
		}


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

	public boolean isObstacle()
		{
		return(true);
		}
	
	public boolean isPushable()
		{
		return(false);
		}
	
	public boolean isPickupable()
		{
		return(false);
		}
	
	public Vec2 getClosestPoint(Vec2 from)
		{
		Vec2 tmp = new Vec2(position.x, position.y);
		tmp.sub(from);
		if (tmp.r < RADIUS)
			tmp.setr(0);
		else
			tmp.setr(tmp.r-RADIUS);
		return(tmp);
		}

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

    /**
	 * 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)
	    {
	    Vec2 closest = getClosestPoint(c.centre); // closest is a vector with origin at center that leads to closest point on current object
	    if (closest.r <= c.radius) // closest point is within c.radius of c.centre
			{
			return true;
			}
	    else 
			{
			return false;
			}
	    }

    /**
	 * 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)
		{
		Vec2 vertex1, vertex2, vec1, vector2, closestPt;
		int numberEdges = p.vertices.size(); // n edges if n vertices (as vertex n+1 wraps round to vertex 0)
		double scale;

		for (int i=0;i<numberEdges;i++)
			{
			vertex1 = (Vec2)p.vertices.elementAt(i);
			vertex2 = (Vec2)p.vertices.elementAt((i+1)%numberEdges);
			vertex1.sub(position);
			vertex2.sub(position);
			// if either vertex is within the circles radius you are colliding
			if ((vertex1.r < RADIUS) || (vertex2.r < RADIUS))
				{
				return true;
				} 
			vertex1.add(position);
			vertex2.add(position);
			vec1 = new Vec2(vertex2);
			vec1.sub(vertex1);
			vector2 = new Vec2(position);
			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(position); // position of closest point relative to centre of current object
			if (closestPt.r < 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;
					}
				}
			}
		return false; // closest point to object on each edge of polygon not within object			
		}


	public void push(Vec2 d, Vec2 v)
		{
		// sorry no pushee robots! This means the robot cannot be pushed by others.
		}

	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();