private boolean isRequiredWay(Way way) {
long wayId = way.getId();
if (requiredWays.get(wayId)) return true;
return usedByTag && !isEmptyTagSet(way.getTags());
}
public Osmformat.PrimitiveGroup serialize() {
if (contents.size() == 0) {
return null;
}
// System.out.format("%d Ways ",contents.size());
StringTable stable = serializer.getStringTable();
Osmformat.PrimitiveGroup.Builder builder = Osmformat.PrimitiveGroup.newBuilder();
for (Way i : contents) {
Osmformat.Way.Builder bi = Osmformat.Way.newBuilder();
bi.setId(i.getId());
long lastid = 0;
for (long j : i.getRefs()) {
long id = j;
bi.addRefs(id - lastid);
lastid = id;
}
Iterator<Element.Tag> tags = i.tagsIterator();
while (tags.hasNext()) {
Element.Tag t = tags.next();
bi.addKeys(stable.getIndex(t.getKey()));
bi.addVals(stable.getIndex(t.getValue()));
}
if (!omit_metadata) {
bi.setInfo(serializeMetadata(i));
}
builder.addWays(bi);
}
return builder.build();
}
private void sendNodes() {
// mark all nodes as required
ReleasableIterator<WayContainer> wayIterator = allWays.iterate();
while (wayIterator.hasNext()) {
Way way = wayIterator.next().getEntity();
if (!isRequiredWay(way)) continue;
for (WayNode nodeReference : way.getWayNodes()) {
long nodeId = nodeReference.getNodeId();
requiredNodes.set(nodeId);
}
}
wayIterator.release();
ReleasableIterator<NodeContainer> nodeIterator = allNodes.iterate();
while (nodeIterator.hasNext()) {
NodeContainer nodeContainer = nodeIterator.next();
if (!isRequiredNode(nodeContainer.getEntity())) {
continue;
}
sink.process(nodeContainer);
}
nodeIterator.release();
}
private boolean hitTheWall(Pose pose) {
float poseAngle = (float) Math.toRadians(Const.angleToByte(pose.getHeading()) * 90);
byte sin = (byte) Math.round(Math.sin(poseAngle)), cos = (byte) Math.round(Math.cos(poseAngle));
byte mapPoseX = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getX()), "X") + cos),
mapPoseY = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getY()), "Y") + sin);
// return true and update pose if it hit an obstacle
if (mapPoseX < 0
|| mapPoseY < 0
|| mapPoseX > 8
|| mapPoseY > 5
|| wayClass.getOccupancyMap(mapPoseX, mapPoseY) > 0) {
Rover.poseProvider.setPose(
new Pose(
cos != 0
? (mapPoseX * Const.BLOCK_WIDTH + (cos > 0 ? -10 : 10 + Const.BLOCK_WIDTH))
: pose.getX(),
sin != 0
? (mapPoseY * Const.BLOCK_WIDTH + (sin > 0 ? -10 : 10 + Const.BLOCK_WIDTH))
: pose.getY(),
cacheWayAngle));
return true;
}
return false;
}
/** _more_ */
protected void createChart() {
if (madeChart) {
return;
}
madeChart = true;
final JCheckBox chartDiffCbx = new JCheckBox("Use Difference", chartDifference);
chartDiffCbx.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent ae) {
chartDifference = chartDiffCbx.isSelected();
updateChart();
}
});
chartTimeBox = new JList();
chartTimeBox.setVisibleRowCount(3);
chartTimeBox.setSelectionMode(ListSelectionModel.MULTIPLE_INTERVAL_SELECTION);
chartTimeBox.addListSelectionListener(
new ListSelectionListener() {
public void valueChanged(ListSelectionEvent e) {
if (ignoreChartTimeChanges) {
return;
}
updateChart();
}
});
List<StormParam> params = getStormTrackParams();
Insets inset = new Insets(3, 7, 7, 0);
List chartComps = new ArrayList();
List<Way> ways = Misc.sort(stormDisplayState.getTrackCollection().getWayList());
List wayComps = new ArrayList();
for (Way way : ways) {
final Way theWay = way;
if (way.isObservation() && !chartWays.contains(way)) {
chartWays.add(way);
}
final JCheckBox cbx = new JCheckBox(way.toString(), chartWays.contains(theWay));
cbx.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent ae) {
if (cbx.isSelected()) {
addChartWay(theWay);
} else {
removeChartWay(theWay);
}
}
});
if (!way.isObservation()) {
wayComps.add(cbx);
} else {
wayComps.add(0, cbx);
}
}
chartComps.add(new JLabel(stormDisplayState.getStormTrackControl().getWaysName() + ":"));
JComponent chartWayComp = GuiUtils.vbox(wayComps);
if (wayComps.size() > 6) {
chartWayComp = makeScroller(chartWayComp, 100, 150);
}
chartComps.add(GuiUtils.inset(chartWayComp, inset));
chartComps.add(GuiUtils.lLabel((isHourly() ? "Forecast Hour:" : "Forecast Time:")));
JScrollPane sp = new JScrollPane(chartTimeBox);
chartComps.add(GuiUtils.inset(sp, inset));
List paramComps = new ArrayList();
for (StormParam param : params) {
// if (param.getIsChartParam() == false) {
// continue;
// }
final StormParam theParam = param;
boolean useChartParam = chartParams.contains(theParam);
final JCheckBox cbx = new JCheckBox(param.toString(), useChartParam);
cbx.addActionListener(
new ActionListener() {
public void actionPerformed(ActionEvent ae) {
if (cbx.isSelected()) {
addChartParam(theParam);
} else {
removeChartParam(theParam);
}
}
});
paramComps.add(cbx);
}
chartComps.add(new JLabel("Parameters:"));
JComponent paramComp = GuiUtils.vbox(paramComps);
if (paramComps.size() > 6) {
paramComp = makeScroller(paramComp, 100, 150);
}
chartComps.add(GuiUtils.inset(paramComp, inset));
chartComps.add(chartDiffCbx);
JButton removeBtn = GuiUtils.makeButton("Remove Chart", this, "removeChart");
chartComps.add(GuiUtils.filler(5, 10));
chartComps.add(removeBtn);
// JComponent top = GuiUtils.left(GuiUtils.hbox(
// GuiUtils.label("Forecast Time: ",
// chartTimeBox),
// chartDiffCbx));
// top = GuiUtils.inset(top,5);
// chartTop.add(BorderLayout.NORTH, top);
JComponent left = GuiUtils.doLayout(chartComps, 1, GuiUtils.WT_N, new double[] {0, 1, 0, 1, 0});
chartLeft.add(BorderLayout.CENTER, GuiUtils.inset(left, 5));
chartLeft.invalidate();
chartLeft.validate();
chartLeft.repaint();
}
/**
* Reverse Way enum: for example: North > South
*
* @param way: the direction
* @return Way
*/
public static Way reverseWay(Way way) {
if (way.ordinal() % 2 == 0) return Way.values()[way.ordinal() + 1];
return Way.values()[way.ordinal() - 1];
}
public void run() {
while (true) {
if (_suppress) {
Thread.yield();
continue;
}
Pose oldPose = cachePose;
cachePose = Rover.poseProvider.getPose();
// get angle and distance where to travel
int[] way = wayClass.getPredefinedWay(oldPose, cachePose);
cacheWayAngle = way[1];
cacheWayDistance = way[0] * Const.BLOCK_WIDTH;
float angle = Const.normalizeAngle(cacheWayAngle - cachePose.getHeading());
if (Const.angleToByte(angle) == 2 && cacheWayDistance == Const.BLOCK_WIDTH) {
// if the robot is supposed to rotate 180 deg and travel one block, back out
Rover.pilot.travel(-cacheWayDistance, true);
} else {
// otherwise rotate and travel the distance
if (Math.abs(angle) > 2) {
// float cacheGyro = Rover.getGyro();
boolean touched = false;
Rover.pilot.rotate(angle, true);
while (Rover.pilot.isMoving()) {
boolean[] touch = Rover.getTouch();
if (touch[0] || touch[1]) touched = true;
Thread.yield();
}
Pose newPose = Rover.poseProvider.getPose();
/*
float angleGyro = Rover.getGyro() - cacheGyro;
// if the turn is incomplete, rotate by the difference
// TODO maybe turn only when the difference is bigger
if (angle > angleGyro + 5 || angle < angleGyro - 5) {
Rover.pilot.travel(touched ? -5 : 5);
cacheWayDistance += touched ? 5 : -5;
Rover.pilot.rotate(angle - angleGyro);
newPose = Rover.poseProvider.getPose();
} */
// update pose with normalize heading
Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));
}
float blockAngle = (float) Math.toRadians(cacheWayAngle);
byte sin = (byte) Math.round(Math.sin(blockAngle)),
cos = (byte) Math.round(Math.cos(blockAngle));
byte
blockX =
(byte)
(Const.roundToMapArea(Const.middlePointToBlock(cachePose.getX()), "X")
+ cos * (way[0] + 1)),
blockY =
(byte)
(Const.roundToMapArea(Const.middlePointToBlock(cachePose.getY()), "Y")
+ sin * (way[0] + 1));
// extend the distance to hit a wall
if (blockX < 0
|| blockY < 0
|| blockX > 8
|| blockY > 5
|| wayClass.getOccupancyMap(blockX, blockY) > 0) {
cacheWayDistance += Const.BLOCK_WIDTH;
// TODO no way of updating pose if it doesn't hit wall
}
Rover.pilot.travel(cacheWayDistance, true);
}
// while the rover is traveling
while (Rover.pilot.isMoving()) {
boolean[] touch = Rover.getTouch();
if (touch[0] || touch[1]) {
Rover.pilot.quickStop();
float sonic = Rover.getSonic();
Pose hitPose = Rover.poseProvider.getPose();
if (hitTheWall(hitPose)) {
Rover.pilot.travel(-4);
} else if (sonic > 15 && !(touch[0] && touch[1])) {
// if the rover touches the obstacle by just one sensor
// and distance is more than 15 cm
float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
Rover.pilot.travel(-4);
Rover.pilot.rotate(touch[0] ? -20 : 20);
Pose newPose = Rover.poseProvider.getPose();
Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));
cacheWayDistance -= distanceTraveled - 4;
Rover.pilot.travel(cacheWayDistance, true);
} else {
// if the obstacle touched by both sensors
// or the distance is less than 15 cm
Pose p = Rover.poseProvider.getPose();
byte blockX = Const.roundToMapArea(Const.middlePointToBlock(p.getX()), "X"),
blockY = Const.roundToMapArea(Const.middlePointToBlock(p.getY()), "Y");
float blockAngle = (float) Math.toRadians(Const.angleToByte(p.getHeading()) * 90);
byte sin = (byte) Math.round(Math.sin(blockAngle)),
cos = (byte) Math.round(Math.cos(blockAngle));
boolean isFree;
try {
isFree = wayClass.getOccupancyMap((byte) (blockX + cos), (byte) (blockY + sin)) < 0;
} catch (ArrayIndexOutOfBoundsException e) {
isFree = false;
}
if (!(touch[0] && touch[1]) && isFree) {
Rover.pilot.rotate(touch[0] ? -90 : 90);
Rover.pilot.travel(15);
Rover.pilot.rotate(touch[0] ? 90 : -90);
Rover.poseProvider.setPose(
new Pose(
(blockX + 1 / 2) * Const.BLOCK_WIDTH,
(blockY + 1 / 2) * Const.BLOCK_WIDTH,
Const.angleToByte(p.getHeading()) * 90));
float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
cacheWayDistance -= distanceTraveled;
Rover.pilot.travel(cacheWayDistance, true);
} else {
System.out.println("ERROR: TRAVEL CORRECTION");
// This shouldn't happen any more
Button.LEDPattern(5);
}
}
}
Thread.yield();
}
}
}
public static Way findway(
ItemObject itemChecked, ItemObject itemSelected, ItemObject[][] boards) {
List<Way> quesx = new ArrayList<Way>();
Map<ItemObject, String> danhdau = new HashMap<ItemObject, String>();
quesx.add(new Way(null, itemChecked));
danhdau.put(itemChecked, "1");
while (quesx.size() > 0) {
List<Way> tempQue = new ArrayList<Way>();
tempQue.addAll(quesx);
quesx.clear();
while (tempQue.size() > 0) {
Way item = tempQue.get(tempQue.size() - 1);
danhdau.put(item.getIt(), "1");
tempQue.remove(item);
int x = item.getIt().getPosition().x;
int y = item.getIt().getPosition().y;
// check 4 o
// x, y +1
if (y < COLUMNS - 1) {
ItemObject it = boards[x][y + 1];
Way way = new Way(item, it);
if (it == itemSelected) {
return way;
}
if (danhdau.get(it) == null && !it.isBig()) {
quesx.add(way);
}
}
// x , y-1
if (y > 0) {
ItemObject it = boards[x][y - 1];
Way way = new Way(item, it);
if (it == itemSelected) {
return way;
}
if (danhdau.get(it) == null && !it.isBig()) {
quesx.add(way);
}
}
// x -1, y
if (x > 0) {
ItemObject it = boards[x - 1][y];
Way way = new Way(item, it);
if (it == itemSelected) {
return way;
}
if (danhdau.get(it) == null && !it.isBig()) {
quesx.add(way);
}
}
// x+ 1, y
if (x < ROWS - 1) {
ItemObject it = boards[x + 1][y];
Way way = new Way(item, it);
if (it == itemSelected) {
return way;
}
if (danhdau.get(it) == null && !it.isBig()) {
quesx.add(way);
}
}
}
// neu 1 trong 4 o la diem den --> ket thuc
}
return null;
}
