WayPoint.java

package lejos.robotics.proposal;

import lejos.geom.Point;
import lejos.robotics.Pose;

/*
 * WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
 * DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
 */

public class WayPoint extends Point {
	protected float heading = 0;
	protected boolean headingRequired;
	protected float maxPositionError = -1;
	protected float maxHeadingError = -1;
	
	public WayPoint(float x, float y) {
		super(x,y);
		headingRequired = false; 
	}
	
	public WayPoint(Point p) {
		super((float) p.getX(),(float) p.getY());
		headingRequired = true;
	}
	
	public WayPoint(Pose p) {
		super(p.getX(),p.getY());
		headingRequired = true;
		this.heading = p.getHeading();
	}
	
	public float getHeading() {
		return heading;
	}
	
	public float getMaxPositionError() {
		return maxPositionError;
	}
	
	public void setMaxPositionError(float distance) {
		maxPositionError = distance;
	}
	
	public float getMaxHeadingError() {
		return maxHeadingError;
	}
	
	public void setMaxHeadingError(float distance) {
		maxHeadingError = distance;
	}
	
	public Pose getPose() {
		return new Pose(x,y,heading);
	}
	
	/**
	 * Check that the given pose satisfies the conditions for this way point
	 * @param p the Pose
	 */
	public boolean checkValidity(Pose p) {
		if (maxPositionError >= 0 && 
		    p.distanceTo(this) > maxPositionError) return false;
		if (headingRequired && maxHeadingError >= 0 && 
			    Math.abs(p.getHeading() - heading) > maxPositionError) return false;
		return true;
	}
}