Robot.java

import javafx.geometry.Pos;
import javafx.scene.Group;
import javafx.scene.layout.Pane;
import javafx.scene.paint.*;
import javafx.scene.shape.*;
import javafx.collections.*;

public class Robot
{
    double px, py, theta;
    double vl, vr;
    double vx, vy;
    double mass;
    double radius = 15.0;;
    Color color;
    Group grp = new Group();
    
    Robot()
    {
	color = Color.WHITE;
	setPose(50,50,0);
	setVelocity(7,-7);
    }

    Robot(int px, int py, int vr, int vl, Color color)
    {
	setPose(px,py,0);
	setVelocity(vr,vl);
	this.color = color;
    }
    
    public String toString()
    {
	String str = "("+px+", "+py+") " + color;
	return str;
    }
    
    public void move()
    {
	px += vl;
	py += vr;
    }

    public void setPose(double x, double y, double angle)
    {
	px = x;
	py = y;
	theta=angle;
	grp.setTranslateX(px);
	grp.setTranslateY(py);
	grp.setRotate(theta);
    }

    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");
	PERIOD = Math.abs(PERIOD);
	if (RADIUS==0.0) return String.format("vr %.1f vl %.1f    spin,    period = %.1f PI %s",vr,vl,PERIOD,dir);
	else return String.format("vr %.1f vl %.1f    radius %.1f,   period %.1f PI %s",vr,vl,RADIUS,PERIOD,dir);
    }

    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 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;
 	grp.setTranslateX(px);
	grp.setTranslateY(py);
	grp.setRotate(theta);
   }
    
    public void draw(Pane parent) 
    {
	//double resc = 3.0;
	//grp.setScaleX(resc);
	//grp.setScaleY(resc);
	Circle cir = new Circle(0.0,0.0,radius);
	cir.setFill(Color.TRANSPARENT);
	//cir.setStroke(Color.BLACK);

	Path p = new Path();
	ObservableList<PathElement> pl = p.getElements();
	pl.add(new MoveTo(-8,10));
	pl.add(new LineTo(12,10));
	pl.add(new LineTo(16,0));
	pl.add(new LineTo(12,-10));
	pl.add(new LineTo(-8,-10));
	pl.add(new ClosePath());
	p.setFill(color);

	Rectangle wheel1 = new Rectangle(-4,-8,8,4);
	wheel1.setFill(Color.BLACK);
	Rectangle wheel2 =  new Rectangle(-4,4,8,4);
	wheel2.setFill(Color.BLACK);

	grp.getChildren().addAll(cir,p,wheel1,wheel2);

	parent.getChildren().add(grp);

    }   

    // returns time before striking window
    public double intersect_window(double width, double 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;
    }    
}