Robot.java

import java.awt.*;
import java.awt.geom.*;


public class Robot
{
    double px, py, theta;
    double vl, vr;
    double vx, vy;
    double mass;
    int radius;
    Color color;
    Robot()
    {
	color = new Color(255,0,0);
	px = 50;
	py = 50;
	vr = 7;
	vl = -7;
	set_radius(14);
    }
    Robot(int px, int py, int vr, int vl, Color color)
    {
	setPose(px,py,0);
	setVelocity(vr,vl);
	this.color = color;
	set_radius(14);
    }
    
    void set_radius(int r)
    {
	radius = r;
	mass = r*r;
    }

    public String toString()
    {
	String str = "("+px+", "+py+") " + color;
	return str;
    }
    
    public void move()
    {
	px += vl;
	py += vr;
    }

    public void setPose(double px, double py, double theta)
    {
	this.px = px;
	this.py = py;
	this.theta = theta;
    }

    public void updateVelocity()
    {
	double v = 0.5*(vr+vl);
	double rtheta = Math.toRadians(theta);
	vx = v*Math.cos(rtheta);
	vy = v*Math.sin(rtheta);
    }


    public void setVelocity(double vr, double vl)
    {
        this.vr = vr;
	this.vl = vl;
    }

    public String showVelocity()
    {
	double ell = 12.0;
	if (vr==vl) {
		if (Math.abs(vr)>0) return String.format("linear motion, v = %.1f",vr);
		else return "Robot stopped";
	}
	double RADIUS = Math.abs(ell/2*(vr+vl)/(vr-vl));
	double PERIOD = 2*ell/(vr-vl);
	String dir = (PERIOD>0? "counterclockwise": "clockwise");
	if (RADIUS==0.0) return String.format("vr %.1f vl %.1f    spin, %s",vr,vl,dir);
	else return String.format("vr %.1f vl %.1f    radius %.1f, %s",vr,vl,RADIUS,dir);
    }

    public void move(double delta)
    {
	double radg = Math.atan(1.0)/45.0;
	double wp = 12.0*radg;
	double  dv, dtheta, rtheta;

	dtheta = (vl-vr)*delta/wp;
	dv = 0.5*(vr+vl)*delta;
	rtheta = radg*theta;
	px += dv*Math.cos(rtheta);
	py += dv*Math.sin(rtheta);
	theta += dtheta;
    }
    
    public void draw(Graphics g) 
    {
	Graphics2D g2 = (Graphics2D) g;

	Color saved = g2.getColor();
	AffineTransform saveXform = g2.getTransform();
	AffineTransform at = new AffineTransform();
	at.translate(px,py);
	//at.rotate(30*Math.PI/180);
	at.rotate(Math.toRadians(theta));
	g2.transform(at);
	Path2D body = new Path2D.Double();
	body.moveTo(-8,10);
	body.lineTo(12,10);
	body.lineTo(16,0);
	body.lineTo(12,-10);
	body.lineTo(-8,-10);
	body.lineTo(-8,10);
	g2.setColor(color);
	g2.fill(body);
	g2.setColor(Color.BLACK);
	g2.draw(body);
	Path2D.Double wheel = new Path2D.Double();
	wheel.moveTo(-4,-2);
	wheel.lineTo(-4,2);
	wheel.lineTo(4,2);
	wheel.lineTo(4,-2);
	wheel.lineTo(-4,-2);
	AffineTransform at2 = new AffineTransform();
	at2.setToTranslation(0,-6);
	wheel.transform(at2);
	g2.fill(wheel);
	at2.setToTranslation(0,12);
	wheel.transform(at2);
	g2.fill(wheel);
	Ellipse2D elip = new Ellipse2D.Double(-radius,-radius,2*radius,2*radius);
	g2.setColor(color);
	//g2.draw(elip);	
	/*
	Line2D line = new Line2D.Double(-20,0,20,0);
	g2.draw(line);
	line.setLine(0,-20,0,20);
	g2.draw(line);
	*/

	g2.setTransform(saveXform);
	g2.setColor(saved);	
    }

    public double intersect_window(int width, int height)
    {
	double t;
	double tx = 1000;
	if (vx<0) {
	    t = (px-radius)/(-vx);
	    if (t<tx) tx = t;
	}
	else if (vx>0) {
	    t = (width - px - radius)/vx;
            if (t<tx) tx = t;
      
	}
	if (vy<0) {
	    t = (py-radius)/(-vy);
	    if (t<tx) tx = t;
	}
	else if (vy>0) {
	    t = (height - py - radius)/vy;
            if (t<tx) tx = t;
	}
	return tx;
    }    
    
}