/* * CaptureSim.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.util.Polygon2;import EDU.cmu.cs.coral.util.Circle2;/** * CaptureSim implements Capture for simulation. * Also includes code implementing communication, gripper and * vision. * <P> * <A HREF="../COPYRIGHT.html">Copyright</A> * (c)1998 Tucker Balch * * @see Capture * @author Tucker Balch * @version $Revision: 1.5 $ */public class CaptureSim extends Simple	implements Capture, SimpleN150, SimulatedObject	{	private	CircularBuffer	trail;	// robot's trail	private KinSensorSim	kin_sensor;	// senses our kin	private TransceiverSim	transceiver;	// comm to other robots	private Vec2	position;	private Vec2	steer;	private	double	turret;	private	double	speed;	private	Color	foreground, background;	private long	time;	private double	timed;	private double	left, right, top, bottom;	private	SimulatedObject[] all_objects = new SimulatedObject[0];	private	int	visionclass;	public	static final boolean DEBUG = false;	/**	 * Instantiate a <B>CaptureSim</B> object.  Be sure	 * to also call init with proper values.	 * @see CaptureSim#init	 */        public CaptureSim()		{		/*--- 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("CaptureSim: instantiated.");		/*--- set default bounds ---*/		top = 1000;		bottom = -1000;		left = -1000;		right = 1000;				}	        /**         * Initialize a <B>CaptureSim</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;		if (DEBUG) System.out.println("CaptureSim: initialized"			+" at "+xp+","+yp);		}	/**	 * Take a simulated step;	 */	public void takeStep(long time_increment, SimulatedObject[] all_objs)		{		if (DEBUG) System.out.println("CaptureSim.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 * desired_speed);		else			velocity.setr(base_speed * desired_speed);		// 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;		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;					}				}			}		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);		}        /**	 * 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 centre 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 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);                }        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;		}	  public Color getForegroundColor() { return foreground; }	  public Color getBackgroundColor() { return background; }        /**         * 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 Vectors ---*/		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 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);                }	/**	 * 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)(RADIUS / meterspp);		int turretd = (int)Units.RadToDeg(turret);		int visionr = (int)(Capture.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);		g.fillOval(xpix - radius, ypix - radius,			radius + radius, radius + radius);		//g.drawOval(xpix - radius, ypix - radius,		//	radius + radius, radius + radius);		/*--- draw the turret ---*/		g.setColor(background);		int dirx = xpix + (int)((double)radius * Math.cos(turret));		int diry = ypix + -(int)((double)radius * Math.sin(turret));		g.drawLine(xpix, ypix, dirx, diry);		// don't draw the vision arc for capture agents		//g.drawArc(xpix - visionr, ypix - visionr,		//		visionr + visionr, visionr + visionr,		//		turretd - (MultiForageN150.VISION_FOV_DEG/2), 		//			MultiForageN150.VISION_FOV_DEG);		/*--- draw steer      ---*/		dirx = xpix + (int)((double)radius * Math.cos(steer.t)*0.5);		diry = ypix + -(int)((double)radius * Math.sin(steer.t)*0.5);		g.drawLine(xpix, ypix, dirx, diry);		/*--- draw what we are carrying ---*/		if (in_gripper != null)			{			Vec2 gpos = new Vec2(RADIUS,0);			gpos.sett(turret);			gpos.add(position);			in_gripper.draw(gpos,g,w,h,t,b,l,r);			}		}