1718192021222324
/** * allocate a new Pose at the origin, heading = 0:the direction the positive X axis */ public Pose() { _location = new Point(0,0); _heading = 0; }
2930313233343536
* @param y the Y coordinate * @param heading the heading */ public Pose(float x, float y, float heading) { _location = new Point(x,y); _heading = heading; }
100101102103104105106107
* @param destination * @return angle in degrees */ public float angleTo(Point destination) { Point d = delta(destination); return (float)Math.toDegrees(Math.atan2(d.getY(),d.getX())); }
111112113114115116117118
* @param destination * @return the distance */ public float distanceTo(Point destination) { Point d = delta(destination); return (float) Math.sqrt( d.getX()*d.getX() + d.getY()*d.getY()); }
116117118119120121122123
Point d = delta(destination); return (float) Math.sqrt( d.getX()*d.getX() + d.getY()*d.getY()); } private Point delta(Point d) { return new Point((float)(d.getX() - _location.getX()), (float) (d.getY() - _location.getY())); }