public Position placeObstacle(Position robotPos, Bearing landmark) {
Position rotlandMark = new Position(0, 0, 0); // position data for rotating landmark
Position result =
new Position(0, 0, 0); // position data holding dist and angle to landmark from particle
rotlandMark.setX(landmark.getR() * Math.cos(landmark.getTheta()));
rotlandMark.setY(landmark.getR() * Math.sin(landmark.getTheta()));
rotlandMark.setTheta(-landmark.getTheta());
result = rot(rotlandMark);
return new Position(result.getX() + robotPos.getX(), result.getY() + robotPos.getY(), 0);
}
@Override
public int compareTo(Bearing o) {
return Float.compare(this.getBearing(), o.getBearing());
}
