public List<Node> move(Node[][] matrix, Direction... directions) {
List<Node> nodes = new ArrayList<Node>();
int currentRow = 0;
int currentCol = 0;
visit(nodes, matrix[currentRow][currentCol]);
for (Direction direction : directions) {
while (true) {
int nextRow = currentRow;
int nextCol = currentCol;
if (Direction.LEFT.equals(direction)) {
nextCol--;
} else if (Direction.RIGHT.equals(direction)) {
nextCol++;
} else if (Direction.LEFT_DOWN.equals(direction)) {
nextCol--;
nextRow++;
} else if (Direction.UP.equals(direction)) {
nextRow--;
}
// Check whether next position exceed border limit
if (nextCol >= matrix[0].length || nextCol < 0 || nextRow >= matrix.length || nextRow < 0) {
System.out.println("Hit borders");
break;
}
// check whether next node has been visited
Node nextNode = matrix[nextRow][nextCol];
if (nextNode.visisted) {
break;
}
currentRow = nextRow;
currentCol = nextCol;
nextNode.visisted = true;
nodes.add(nextNode);
System.out.println("Row:" + nextRow + ", Col:" + nextCol);
}
}
return nodes;
}
/**
* Outline: 1. Find direction of all segments - direction = 0..3 (right,up,left,down) - right is
* not really right, you may have to turn your screen 2. Find average heading of all segments -
* heading = angle of a vector in polar coordinates - sum up horizontal segments (those with
* direction 0 or 2) - sum up vertical segments - turn the vertical sum by 90 degrees and add it
* to the horizontal sum - get the average heading from this total sum 3. Rotate all nodes by the
* average heading so that right is really right and all segments are approximately NS or EW. 4.
* If nodes are connected by a horizontal segment: Replace their y-Coordinate by the mean value of
* their y-Coordinates. - The same for vertical segments. 5. Rotate back.
*/
private static Collection<Command> orthogonalize(
List<WayData> wayDataList, List<Node> headingNodes) throws InvalidUserInputException {
// find average heading
double headingAll;
try {
if (headingNodes.isEmpty()) {
// find directions of the segments and make them consistent between different ways
wayDataList.get(0).calcDirections(Direction.RIGHT);
double refHeading = wayDataList.get(0).heading;
EastNorth totSum = new EastNorth(0., 0.);
for (WayData w : wayDataList) {
w.calcDirections(Direction.RIGHT);
int directionOffset = angleToDirectionChange(w.heading - refHeading, TOLERANCE2);
w.calcDirections(Direction.RIGHT.changeBy(directionOffset));
if (angleToDirectionChange(refHeading - w.heading, TOLERANCE2) != 0)
throw new RuntimeException();
totSum = EN.sum(totSum, w.segSum);
}
headingAll = EN.polar(new EastNorth(0., 0.), totSum);
} else {
headingAll =
EN.polar(headingNodes.get(0).getEastNorth(), headingNodes.get(1).getEastNorth());
for (WayData w : wayDataList) {
w.calcDirections(Direction.RIGHT);
int directionOffset = angleToDirectionChange(w.heading - headingAll, TOLERANCE2);
w.calcDirections(Direction.RIGHT.changeBy(directionOffset));
}
}
} catch (RejectedAngleException ex) {
throw new InvalidUserInputException(
tr(
"<html>Please make sure all selected ways head in a similar direction<br>"
+ "or orthogonalize them one by one.</html>"),
ex);
}
// put the nodes of all ways in a set
final Set<Node> allNodes = new HashSet<>();
for (WayData w : wayDataList) {
for (Node n : w.way.getNodes()) {
allNodes.add(n);
}
}
// the new x and y value for each node
final Map<Node, Double> nX = new HashMap<>();
final Map<Node, Double> nY = new HashMap<>();
// calculate the centroid of all nodes
// it is used as rotation center
EastNorth pivot = new EastNorth(0., 0.);
for (Node n : allNodes) {
pivot = EN.sum(pivot, n.getEastNorth());
}
pivot = new EastNorth(pivot.east() / allNodes.size(), pivot.north() / allNodes.size());
// rotate
for (Node n : allNodes) {
EastNorth tmp = EN.rotateCC(pivot, n.getEastNorth(), -headingAll);
nX.put(n, tmp.east());
nY.put(n, tmp.north());
}
// orthogonalize
final Direction[] HORIZONTAL = {Direction.RIGHT, Direction.LEFT};
final Direction[] VERTICAL = {Direction.UP, Direction.DOWN};
final Direction[][] ORIENTATIONS = {HORIZONTAL, VERTICAL};
for (Direction[] orientation : ORIENTATIONS) {
final Set<Node> s = new HashSet<>(allNodes);
int s_size = s.size();
for (int dummy = 0; dummy < s_size; ++dummy) {
if (s.isEmpty()) {
break;
}
final Node dummy_n = s.iterator().next(); // pick arbitrary element of s
final Set<Node> cs =
new HashSet<>(); // will contain each node that can be reached from dummy_n
cs.add(dummy_n); // walking only on horizontal / vertical segments
boolean somethingHappened = true;
while (somethingHappened) {
somethingHappened = false;
for (WayData w : wayDataList) {
for (int i = 0; i < w.nSeg; ++i) {
Node n1 = w.way.getNodes().get(i);
Node n2 = w.way.getNodes().get(i + 1);
if (Arrays.asList(orientation).contains(w.segDirections[i])) {
if (cs.contains(n1) && !cs.contains(n2)) {
cs.add(n2);
somethingHappened = true;
}
if (cs.contains(n2) && !cs.contains(n1)) {
cs.add(n1);
somethingHappened = true;
}
}
}
}
}
final Map<Node, Double> nC = (orientation == HORIZONTAL) ? nY : nX;
double average = 0;
for (Node n : cs) {
s.remove(n);
average += nC.get(n).doubleValue();
}
average = average / cs.size();
// if one of the nodes is a heading node, forget about the average and use its value
for (Node fn : headingNodes) {
if (cs.contains(fn)) {
average = nC.get(fn);
}
}
// At this point, the two heading nodes (if any) are horizontally aligned, i.e. they
// have the same y coordinate. So in general we shouldn't find them in a vertical string
// of segments. This can still happen in some pathological cases (see #7889). To avoid
// both heading nodes collapsing to one point, we simply skip this segment string and
// don't touch the node coordinates.
if (orientation == VERTICAL && headingNodes.size() == 2 && cs.containsAll(headingNodes)) {
continue;
}
for (Node n : cs) {
nC.put(n, average);
}
}
if (!s.isEmpty()) throw new RuntimeException();
}
// rotate back and log the change
final Collection<Command> commands = new LinkedList<>();
for (Node n : allNodes) {
EastNorth tmp = new EastNorth(nX.get(n), nY.get(n));
tmp = EN.rotateCC(pivot, tmp, headingAll);
final double dx = tmp.east() - n.getEastNorth().east();
final double dy = tmp.north() - n.getEastNorth().north();
if (headingNodes.contains(n)) { // The heading nodes should not have changed
final double EPSILON = 1E-6;
if (Math.abs(dx) > Math.abs(EPSILON * tmp.east())
|| Math.abs(dy) > Math.abs(EPSILON * tmp.east())) throw new AssertionError();
} else {
OrthogonalizeAction.rememberMovements.put(n, new EastNorth(dx, dy));
commands.add(new MoveCommand(n, dx, dy));
}
}
return commands;
}
public Turn howToTurn(Direction to) {
to = Direction.values()[(4 + to.ordinal() - this.ordinal()) % 4];
return Direction.RIGHT.howToTurn(to);
}
