content: package aima.gui.swing.applications.robotics.simple;
  
  import aima.core.robotics.impl.datatypes.Angle;
  import aima.core.robotics.impl.datatypes.IPose2D;
  import aima.core.util.math.geom.shapes.Point2D;
  import aima.core.util.math.geom.shapes.Vector2D;
  
  /**
   * Implementation of {@link IPose2D} for {@link SimpleMove}.<br/>
   * This class is used in the context of the virtual robot. 
   * 
   * @author Arno von Borries
   * @author Jan Phillip Kretzschmar
   * @author Andreas Walscheid
   *
   */
  public final class SimplePose implements IPose2D<SimplePose,SimpleMove> {
  
  	private final Point2D position;
  	private final double heading;
  	
  	/**
  	 * @param point the position of the pose.
  	 * @param heading the direction in which the pose is facing. 0 means parallel to the X axis, {@code Math.PI/2} means parallel to the Y axis.
  	 */
  	public SimplePose(Point2D point, double heading) {
  		this.position = point;
  		this.heading = heading % (2*Math.PI);
  	}
  	
  	/**
  	 * @param x the X coordinate of the position of the pose.
  	 * @param y the Y coordinate of the position of the pose.
  	 * @param heading the direction in which the pose is facing. 0 means parallel to the X axis, {@code Math.PI/2} means parallel to the Y axis.
  	 */
  	public SimplePose(double x, double y, double heading) {
  		this.position = new Point2D(x, y);
  		this.heading = heading % (2*Math.PI);
  	}
  	
  	/**
  	 * Returns the position of the pose.
  	 * @return the position of the pose.
  	 */
  	public Point2D getPosition() {
  		return position;
  	}
  	
  	@Override
  	public SimplePose applyMovement(SimpleMove move) {
  		final double headingNew = heading + move.getFirstRotation();
  		Point2D positionNew = position.add(Vector2D.calculateFromPolar(move.getForward(),-headingNew));
  		return new SimplePose(positionNew,headingNew + move.getLastRotation());
  	}
  
  	@Override
  	public SimplePose addAngle(Angle angle) {
  		return new SimplePose(position.clone(),heading + angle.getValue());
  	}
  
  	@Override
  	public SimplePose clone() {
  		return new SimplePose(position.clone(),heading);
  	}
  
  	@Override
  	public double distanceTo(SimplePose pose) {
  		return this.position.distance(pose.getPosition());
  	}
  
  	@Override
  	public double getX() {
  		return position.getX();
  	}
  
  	@Override
  	public double getY() {
  		return position.getY();
  	}
  
  	@Override
  	public double getHeading() {
  		return heading;
  	}
  }
  