| 56 | | // Check the selection if it is suitable for the orthogonalisation |
| 57 | | for (OsmPrimitive osm : sel) { |
| 58 | | // Check if not more than two nodes in the selection |
| 59 | | if(osm instanceof Node) { |
| 60 | | if(dirnodes.size() == 2) { |
| 61 | | JOptionPane.showMessageDialog( |
| 62 | | Main.parent, |
| 63 | | tr("Only two nodes allowed"), |
| 64 | | tr("Information"), |
| 65 | | JOptionPane.INFORMATION_MESSAGE |
| 66 | | ); |
| 67 | | return; |
| | 61 | public class Undo extends JosmAction { |
| | 62 | public Undo() { |
| | 63 | super(tr("Orthogonalize Shape / Undo"), |
| | 64 | "ortho", |
| | 65 | tr("Undo orthogonalization for certain nodes"), |
| | 66 | Shortcut.registerShortcut("tools:orthogonalizeUndo", tr("Tool: {0}", tr("Orthogonalize Shape / Undo")), |
| | 67 | KeyEvent.VK_Q, |
| | 68 | Shortcut.GROUP_EDIT, Shortcut.SHIFT_DEFAULT), true); |
| | 69 | } |
| | 70 | public void actionPerformed(ActionEvent e) { |
| | 71 | if (!isEnabled()) |
| | 72 | return; |
| | 73 | final Collection<Command> commands = new LinkedList<Command>(); |
| | 74 | final Collection<OsmPrimitive> sel = getCurrentDataSet().getSelected(); |
| | 75 | try { |
| | 76 | for (OsmPrimitive p : sel) { |
| | 77 | if (! (p instanceof Node)) throw new InvalidUserInputException(); |
| | 78 | Node n = (Node) p; |
| | 79 | if (rememberMovements.containsKey(n)) { |
| | 80 | EastNorth tmp = rememberMovements.get(n); |
| | 81 | commands.add(new MoveCommand(n, - tmp.east(), - tmp.north())); |
| | 82 | rememberMovements.remove(n); |
| | 83 | } |
| 82 | | |
| 83 | | // Check if every way is made of at least four segments and closed |
| 84 | | Way way = (Way)osm; |
| 85 | | if ((way.getNodesCount() < 5) || !way.isClosed()) { |
| 86 | | JOptionPane.showMessageDialog( |
| 87 | | Main.parent, |
| 88 | | tr("Please select one or more closed ways of at least four nodes."), |
| 89 | | tr("Information"), |
| 90 | | JOptionPane.INFORMATION_MESSAGE |
| 91 | | ); |
| 92 | | return; |
| 93 | | } |
| 94 | | |
| 95 | | // Check if every edge in the way is a definite edge of at least 45 degrees of direction change |
| 96 | | // Otherwise, two segments could be turned into same direction and intersection would fail. |
| 97 | | // Or changes of shape would be too serious. |
| 98 | | for (int i1=0; i1 < way.getNodesCount()-1; i1++) { |
| 99 | | int i2 = (i1+1) % (way.getNodesCount()-1); |
| 100 | | int i3 = (i1+2) % (way.getNodesCount()-1); |
| 101 | | double angle1 =Math.abs(way.getNode(i1).getEastNorth().heading(way.getNode(i2).getEastNorth())); |
| 102 | | double angle2 = Math.abs(way.getNode(i2).getEastNorth().heading(way.getNode(i3).getEastNorth())); |
| 103 | | double delta = Math.abs(angle2 - angle1); |
| 104 | | while(delta > Math.PI) { |
| 105 | | delta -= Math.PI; |
| 106 | | } |
| 107 | | if(delta < Math.PI/4) { |
| 108 | | // not an edge |
| 109 | | alignNodes.add(way.getNode(i2)); |
| 110 | | } |
| 111 | | } |
| 112 | | |
| 113 | | // first node has to be an edge so we move the node to the end of the way |
| 114 | | while (alignNodes.contains(way.firstNode())) { |
| 115 | | Node n = way.firstNode(); |
| 116 | | way.removeNode(n); |
| 117 | | way.addNode(way.getNodesCount() - 2, n); // ! -2 because first node == last node in closed way |
| 118 | | } |
| 153 | | if (dirnodes.size() == 2) { |
| 154 | | // When selection contains two nodes, use the nodes to compute a direction |
| 155 | | // to align all ways to |
| 156 | | align_to_heading = normalize_angle(dirnodes.get(0).getEastNorth().heading(dirnodes.get(1).getEastNorth())); |
| 157 | | use_dirnodes = true; |
| 158 | | } |
| 159 | | |
| 160 | | for (OsmPrimitive osm : sel) { |
| 161 | | if(!(osm instanceof Way)) { |
| 162 | | continue; |
| | 124 | try { |
| | 125 | // collect nodes and ways from the selection |
| | 126 | for (OsmPrimitive p : sel) { |
| | 127 | if (p instanceof Node) { |
| | 128 | nodeList.add((Node) p); |
| | 129 | } |
| | 130 | else if (p instanceof Way) { |
| | 131 | wayDataList.add(new WayData((Way) p)); |
| | 132 | } |
| | 133 | else { // who knows... Maybe a relation got selected... |
| | 134 | throw new InvalidUserInputException("Selection must consist only of ways and nodes."); |
| | 135 | } |
| 174 | | int nodes = way.getNodesCount(); |
| 175 | | int sides = nodes - 1; |
| 176 | | // Copy necessary data into a more suitable data structure |
| 177 | | EastNorth en[] = new EastNorth[sides]; |
| 178 | | for (int i = 0; i < sides; i++) { |
| 179 | | en[i] = new EastNorth(way.getNode(i).getEastNorth().east(), way.getNode(i).getEastNorth().north()); |
| | 151 | } catch (InvalidUserInputException ex) { |
| | 152 | if (ex.getMessage().equals("usage")) { |
| | 153 | JOptionPane.showMessageDialog( |
| | 154 | Main.parent, |
| | 155 | tr(USAGE), |
| | 156 | tr("Orthogonalize Shape"), |
| | 157 | JOptionPane.INFORMATION_MESSAGE); |
| 182 | | if (! use_dirnodes) { |
| 183 | | // To find orientation of all segments, compute weighted average of all segment's headings |
| 184 | | // all headings are mapped into [-PI/4, PI/4] by PI/2 rotations so both main orientations are mapped into one |
| 185 | | // the headings are weighted by the length of the segment establishing it, so a longer segment, that is more |
| 186 | | // likely to have the correct orientation, has more influence in the computing than a short segment, that is easier to misalign. |
| 187 | | double headings[] = new double[sides]; |
| 188 | | double weights[] = new double[sides]; |
| 189 | | for (int i=0; i < sides; i++) { |
| 190 | | headings[i] = normalize_angle(way.getNode(i).getEastNorth().heading(way.getNode(i+1).getEastNorth())); |
| 191 | | weights[i] = way.getNode(i).getEastNorth().distance(way.getNode(i+1).getEastNorth()); |
| | 169 | /** |
| | 170 | * |
| | 171 | * Outline: |
| | 172 | * 1. Find direction of all segments |
| | 173 | * - direction = 0..3 (right,up,left,down) |
| | 174 | * - right is not really right, you may have to turn your screen |
| | 175 | * 2. Find average heading of all segments |
| | 176 | * - heading = angle of a vector in polar coordinates |
| | 177 | * - sum up horizontal segments (those with direction 0 or 2) |
| | 178 | * - sum up vertical segments |
| | 179 | * - turn the vertical sum by 90 degrees and add it to the horizontal sum |
| | 180 | * - get the average heading from this total sum |
| | 181 | * 3. Rotate all nodes by the average heading so that right is really right |
| | 182 | * and all segments are approximately NS or EW. |
| | 183 | * 4. If nodes are connected by a horizontal segment: Replace their y-Coordinate by |
| | 184 | * the mean value of their y-Coordinates. |
| | 185 | * - The same for vertical segments. |
| | 186 | * 5. Rotate back. |
| | 187 | * |
| | 188 | **/ |
| | 189 | private static void orthogonalize(ArrayList<WayData> wayDataList, ArrayList<Node> headingNodes) |
| | 190 | throws InvalidUserInputException |
| | 191 | { |
| | 192 | // find average heading |
| | 193 | double headingAll; |
| | 194 | try { |
| | 195 | if (headingNodes.isEmpty()) { |
| | 196 | // find directions of the segments and make them consistent between different ways |
| | 197 | wayDataList.get(0).calcDirections(Direction.RIGHT); |
| | 198 | double refHeading = wayDataList.get(0).heading; |
| | 199 | for (WayData w : wayDataList) { |
| | 200 | w.calcDirections(Direction.RIGHT); |
| | 201 | int directionOffset = angleToDirectionChange(w.heading - refHeading, TOLERANCE2); |
| | 202 | w.calcDirections(Direction.RIGHT.changeBy(directionOffset)); |
| | 203 | if (angleToDirectionChange(refHeading - w.heading, TOLERANCE2) != 0) throw new RuntimeException(); |
| 193 | | |
| 194 | | // CAVEAT: for orientations near -PI/4 or PI/4 the mapping into ONE orientation fails |
| 195 | | // resulting in a heading-difference between adjacent sides of almost PI/2 |
| 196 | | // and a totally wrong average |
| 197 | | // check for this (use PI/3 as arbitray limit) and rotate into ONE orientation |
| 198 | | double angle_diff_max = 0.0; |
| 199 | | for (int i=0; i < sides; i++) { |
| 200 | | double diff = 0.0; |
| 201 | | if (i == 0) { |
| 202 | | diff = heading_diff(headings[i], headings[sides - 1]); |
| 203 | | } else { |
| 204 | | diff = heading_diff(headings[i], headings[i - 1]); |
| 205 | | } |
| 206 | | if (diff > angle_diff_max) { |
| 207 | | angle_diff_max = diff; |
| 208 | | } |
| | 205 | EastNorth totSum = new EastNorth(0., 0.); |
| | 206 | for (WayData w : wayDataList) { |
| | 207 | totSum = EN.sum(totSum, w.segSum); |
| 210 | | |
| 211 | | if (angle_diff_max > Math.PI/3) { |
| 212 | | // rearrange headings: everything < 0 gets PI/2-rotated |
| 213 | | for (int i=0; i < sides; i++) { |
| 214 | | if (headings[i] < 0) { |
| 215 | | headings[i] += Math.PI/2; |
| 216 | | } |
| 217 | | } |
| | 209 | headingAll = EN.polar(new EastNorth(0., 0.), totSum); |
| | 210 | } |
| | 211 | else { |
| | 212 | headingAll = EN.polar(headingNodes.get(0).getEastNorth(), headingNodes.get(1).getEastNorth()); |
| | 213 | for (WayData w : wayDataList) { |
| | 214 | w.calcDirections(Direction.RIGHT); |
| | 215 | int directionOffset = angleToDirectionChange(w.heading - headingAll, TOLERANCE2); |
| | 216 | w.calcDirections(Direction.RIGHT.changeBy(directionOffset)); |
| 220 | | // TODO: |
| 221 | | // use angle_diff_max as an indicator that the way is already orthogonal |
| 222 | | // e.g. if angle_diff_max is less then Math.toRadians(0.5) |
| 223 | | // and do nothing in that case (?) |
| 224 | | |
| 225 | | // Compute the weighted average of the headings of all segments |
| 226 | | double sum_weighted_headings = 0.0; |
| 227 | | double sum_weights = 0.0; |
| 228 | | for (int i=0; i < sides; i++) { |
| 229 | | sum_weighted_headings += headings[i] * weights[i]; |
| 230 | | sum_weights += weights[i]; |
| 231 | | } |
| 232 | | align_to_heading = normalize_angle(sum_weighted_headings/sum_weights); |
| | 225 | // put the nodes of all ways in a set |
| | 226 | final HashSet<Node> allNodes = new HashSet<Node>(); |
| | 227 | for (WayData w : wayDataList) { |
| | 228 | for (Node n : w.way.getNodes()) { |
| | 229 | allNodes.add(n); |
| 242 | | for (int i=0; i < sides; i++) { |
| 243 | | // Compute handy indices of three nodes to be used in one loop iteration. |
| 244 | | // We use segments (i1,i2) and (i2,i3), align them and compute the new |
| 245 | | // position of the i2-node as the intersection of the realigned (i1,i2), (i2,i3) segments |
| 246 | | // Not the most efficient algorithm, but we don't handle millions of nodes... |
| 247 | | int i1 = i; |
| 248 | | int i2 = (i+1)%sides; |
| 249 | | int i3 = (i+2)%sides; |
| 250 | | double heading1, heading2; |
| 251 | | double delta1, delta2; |
| 252 | | // Compute neccessary rotation of first segment to align it with main orientation |
| 253 | | heading1 = normalize_angle(en[i1].heading(en[i2]), align_to_heading); |
| 254 | | delta1 = align_to_heading - heading1; |
| 255 | | // Compute neccessary rotation of second segment to align it with main orientation |
| 256 | | heading2 = normalize_angle(en[i2].heading(en[i3]), align_to_heading); |
| 257 | | delta2 = align_to_heading - heading2; |
| 258 | | // To align a segment, rotate around its center |
| 259 | | EastNorth pivot1 = new EastNorth((en[i1].east()+en[i2].east())/2, (en[i1].north()+en[i2].north())/2); |
| 260 | | EastNorth A=en[i1].rotate(pivot1, delta1); |
| 261 | | EastNorth B=en[i2].rotate(pivot1, delta1); |
| 262 | | EastNorth pivot2 = new EastNorth((en[i2].east()+en[i3].east())/2, (en[i2].north()+en[i3].north())/2); |
| 263 | | EastNorth C=en[i2].rotate(pivot2, delta2); |
| 264 | | EastNorth D=en[i3].rotate(pivot2, delta2); |
| | 237 | // caluclate the centroid of all nodes |
| | 238 | // it is used as rotation center |
| | 239 | EastNorth pivot = new EastNorth(0., 0.); |
| | 240 | for (Node n : allNodes) { |
| | 241 | pivot = EN.sum(pivot, n.getEastNorth()); |
| | 242 | } |
| | 243 | pivot = new EastNorth(pivot.east() / allNodes.size(), pivot.north() / allNodes.size()); |
| 270 | | // Check for parallel segments and do nothing if they are |
| 271 | | // In practice this will probably only happen when a way has |
| 272 | | // been duplicated |
| | 252 | // orthogonalize |
| | 253 | final Direction[] HORIZONTAL = {Direction.RIGHT, Direction.LEFT}; |
| | 254 | final Direction[] VERTICAL = {Direction.UP, Direction.DOWN}; |
| | 255 | final Direction[][] ORIENTATIONS = {HORIZONTAL, VERTICAL}; |
| | 256 | for (Direction[] orientation : ORIENTATIONS){ |
| | 257 | final HashSet<Node> s = new HashSet<Node>(allNodes); |
| | 258 | int s_size = s.size(); |
| | 259 | for (int dummy = 0; dummy < s_size; ++ dummy) { |
| | 260 | if (s.isEmpty()) break; |
| | 261 | final Node dummy_n = s.iterator().next(); // pick arbitrary element of s |
| 274 | | if (u == 0) { |
| 275 | | continue; |
| | 263 | final HashSet<Node> cs = new HashSet<Node>(); // will contain each node that can be reached from dummy_n |
| | 264 | cs.add(dummy_n); // walking only on horizontal / vertical segments |
| | 265 | |
| | 266 | boolean somethingHappened = true; |
| | 267 | while (somethingHappened) { |
| | 268 | somethingHappened = false; |
| | 269 | for (WayData w : wayDataList) { |
| | 270 | for (int i=0; i < w.nSeg; ++i) { |
| | 271 | Node n1 = w.way.getNodes().get(i); |
| | 272 | Node n2 = w.way.getNodes().get(i+1); |
| | 273 | if (Arrays.asList(orientation).contains(w.segDirections[i])) { |
| | 274 | if (cs.contains(n1) && ! cs.contains(n2)) { |
| | 275 | cs.add(n2); |
| | 276 | somethingHappened = true; |
| | 277 | } |
| | 278 | if (cs.contains(n2) && ! cs.contains(n1)) { |
| | 279 | cs.add(n1); |
| | 280 | somethingHappened = true; |
| | 281 | } |
| | 282 | } |
| | 283 | } |
| | 284 | } |
| 282 | | double q = det(B.north() - C.north(), B.east() - C.east(), |
| 283 | | D.north() - C.north(), D.east() - C.east()) / u; |
| 284 | | EastNorth intersection = new EastNorth( |
| 285 | | B.east() + q * (A.east() - B.east()), |
| 286 | | B.north() + q * (A.north() - B.north())); |
| 287 | | |
| 288 | | Node n = way.getNode(i2); |
| 289 | | |
| 290 | | LatLon ill = Main.proj.eastNorth2latlon(intersection); |
| 291 | | if (!ill.equalsEpsilon(n.getCoor())) { |
| 292 | | double dx = intersection.east()-n.getEastNorth().east(); |
| 293 | | double dy = intersection.north()-n.getEastNorth().north(); |
| 294 | | cmds.add(new MoveCommand(n, dx, dy)); |
| | 289 | double average = 0; |
| | 290 | for (Node n : cs) { |
| | 291 | average += nC.get(n).doubleValue(); |
| 320 | | if (cmds.size() > 0) { |
| 321 | | Main.main.undoRedo.add(new SequenceCommand(tr("Orthogonalize"), cmds)); |
| 322 | | Main.map.repaint(); |
| | 313 | // rotate back and log the change |
| | 314 | final Collection<Command> commands = new LinkedList<Command>(); |
| | 315 | OrthogonalizeAction.rememberMovements.clear(); |
| | 316 | for (Node n: allNodes) { |
| | 317 | EastNorth tmp = new EastNorth(nX.get(n), nY.get(n)); |
| | 318 | tmp = EN.rotate_cc(pivot, tmp, headingAll); |
| | 319 | final double dx = tmp.east() - n.getEastNorth().east(); |
| | 320 | final double dy = tmp.north() - n.getEastNorth().north(); |
| | 321 | if (headingNodes.contains(n)) { // The heading nodes should not have changed |
| | 322 | final double EPSILON = 1E-6; |
| | 323 | if (Math.abs(dx) > Math.abs(EPSILON * tmp.east()) || |
| | 324 | Math.abs(dy) > Math.abs(EPSILON * tmp.east())) { |
| | 325 | throw new AssertionError(); |
| | 326 | } |
| | 327 | } |
| | 328 | else { |
| | 329 | OrthogonalizeAction.rememberMovements.put(n, new EastNorth(dx, dy)); |
| | 330 | commands.add(new MoveCommand(n, dx, dy)); |
| | 331 | } |
| 326 | | private MoveCommand alignSide(ArrayList<Node> aNodes, EastNorth aligna, EastNorth alignb) { |
| | 338 | /** |
| | 339 | * Class contains everything we need to know about a singe way. |
| | 340 | */ |
| | 341 | private static class WayData { |
| | 342 | final public Way way; // The assigned way |
| | 343 | final public int nSeg; // Number of Segments of the Way |
| | 344 | final public int nNode; // Number of Nodes of the Way |
| | 345 | public Direction[] segDirections; // Direction of the segments |
| | 346 | // segment i goes from node i to node (i+1) |
| | 347 | public EastNorth segSum; // (Vector-)sum of all horizontal segments plus the sum of all vertical |
| | 348 | // segments turned by 90 degrees |
| | 349 | public double heading; // heading of segSum == approximate heading of the way |
| | 350 | public WayData(Way pWay) { |
| | 351 | way = pWay; |
| | 352 | nNode = way.getNodes().size(); |
| | 353 | nSeg = nNode - 1; |
| | 354 | } |
| | 355 | /** |
| | 356 | * Estimate the direction of the segments, given the first segment points in the |
| | 357 | * direction <code>pInitialDirection</code>. |
| | 358 | * Then sum up all horizontal / vertical segments to have a good guess for the |
| | 359 | * heading of the entire way. |
| | 360 | */ |
| | 361 | public void calcDirections(Direction pInitialDirection) throws InvalidUserInputException { |
| | 362 | final EastNorth[] en = new EastNorth[nNode]; // alias: way.getNodes().get(i).getEastNorth() ---> en[i] |
| | 363 | for (int i=0; i < nNode; i++) { |
| | 364 | en[i] = new EastNorth(way.getNodes().get(i).getEastNorth().east(), way.getNodes().get(i).getEastNorth().north()); |
| | 365 | } |
| | 366 | segDirections = new Direction[nSeg]; |
| | 367 | Direction direction = pInitialDirection; |
| | 368 | segDirections[0] = direction; |
| | 369 | for (int i=0; i < nSeg - 1; i++) { |
| | 370 | double h1 = EN.polar(en[i],en[i+1]); |
| | 371 | double h2 = EN.polar(en[i+1],en[i+2]); |
| | 372 | try { |
| | 373 | direction = direction.changeBy(angleToDirectionChange(h2 - h1, TOLERANCE1)); |
| | 374 | } catch (RejectedAngleException ex) { |
| | 375 | throw new InvalidUserInputException("Please select ways with angles of approximately 90 or 180 degrees."); |
| | 376 | } |
| | 377 | segDirections[i+1] = direction; |
| | 378 | } |
| 328 | | // Find out co-ords of A and B |
| 329 | | double ax = aligna.east(); |
| 330 | | double ay = aligna.north(); |
| 331 | | double bx = alignb.east(); |
| 332 | | double by = alignb.north(); |
| 333 | | |
| 334 | | // OK, for each node to move, work out where to move it! |
| 335 | | for (Node n1 : aNodes) { |
| 336 | | // Get existing co-ords of node to move |
| 337 | | double nx = n1.getEastNorth().east(); |
| 338 | | double ny = n1.getEastNorth().north(); |
| 339 | | |
| 340 | | if (ax == bx) { |
| 341 | | // Special case if AB is vertical... |
| 342 | | nx = ax; |
| 343 | | } else if (ay == by) { |
| 344 | | // ...or horizontal |
| 345 | | ny = ay; |
| 346 | | } else { |
| 347 | | // Otherwise calculate position by solving y=mx+c |
| 348 | | double m1 = (by - ay) / (bx - ax); |
| 349 | | double c1 = ay - (ax * m1); |
| 350 | | double m2 = (-1) / m1; |
| 351 | | double c2 = n1.getEastNorth().north() - (n1.getEastNorth().east() * m2); |
| 352 | | |
| 353 | | nx = (c2 - c1) / (m1 - m2); |
| 354 | | ny = (m1 * nx) + c1; |
| | 380 | // sum up segments |
| | 381 | EastNorth h = new EastNorth(0.,0.); |
| | 382 | double lh = EN.abs(h); |
| | 383 | EastNorth v = new EastNorth(0.,0.); |
| | 384 | double lv = EN.abs(v); |
| | 385 | for (int i = 0; i < nSeg; ++i) { |
| | 386 | EastNorth segment = EN.diff(en[i+1], en[i]); |
| | 387 | if (segDirections[i] == Direction.RIGHT) h = EN.sum(h,segment); |
| | 388 | else if (segDirections[i] == Direction.UP) v = EN.sum(v,segment); |
| | 389 | else if (segDirections[i] == Direction.LEFT) h = EN.diff(h,segment); |
| | 390 | else if (segDirections[i] == Direction.DOWN) v = EN.diff(v,segment); |
| | 391 | else throw new IllegalStateException(); |
| | 392 | /** |
| | 393 | * When summing up the length of the sum vector should increase. |
| | 394 | * However, it is possible to construct ways, such that this assertion fails. |
| | 395 | * So only uncomment this for testing |
| | 396 | **/ |
| | 397 | // if (segDirections[i].ordinal() % 2 == 0) { |
| | 398 | // if (EN.abs(h) < lh) throw new AssertionError(); |
| | 399 | // lh = EN.abs(h); |
| | 400 | // } else { |
| | 401 | // if (EN.abs(v) < lv) throw new AssertionError(); |
| | 402 | // lv = EN.abs(v); |
| | 403 | // } |
| 356 | | |
| 357 | | // Return the command to move the node to its new position. |
| 358 | | return new MoveCommand(n1, nx - n1.getEastNorth().east(), ny - n1.getEastNorth().north()); |
| | 405 | // rotate the vertical vector by 90 degrees (clockwise) and add it to the horizontal vector |
| | 406 | segSum = EN.sum(h, new EastNorth(v.north(), - v.east())); |
| | 407 | // if (EN.abs(segSum) < lh) throw new AssertionError(); |
| | 408 | this.heading = EN.polar(new EastNorth(0.,0.), segSum); |
| 363 | | private ArrayList<Node> findNodesToAlign(Way w, Node from, Node to) { |
| 364 | | ArrayList<Node> l = new ArrayList<Node>(); |
| 365 | | boolean start = false; |
| 366 | | for (int i = 0; i < w.getNodesCount(); i++) { |
| 367 | | Node n = w.getNode(i % w.getNodesCount()); |
| 368 | | if (n.equals(to)) { |
| 369 | | break; |
| 370 | | } |
| 371 | | if (start) { |
| 372 | | l.add(n); |
| 373 | | } |
| 374 | | if (n.equals(from)) { |
| 375 | | start = true; |
| 376 | | } |
| 377 | | |
| | 412 | private enum Direction { |
| | 413 | RIGHT, UP, LEFT, DOWN; |
| | 414 | public Direction changeBy(int directionChange) { |
| | 415 | int tmp = (this.ordinal() + directionChange) % 4; |
| | 416 | if (tmp < 0) tmp += 4; // the % operator can return negative value |
| | 417 | return Direction.values()[tmp]; |
| 390 | | static double normalize_angle(double h, double align_to) { |
| 391 | | double llimit = -Math.PI/4; |
| 392 | | double ulimit = Math.PI/4; |
| 393 | | while (h - align_to > ulimit) { |
| 394 | | h -= Math.PI/2; |
| | 438 | |
| | 439 | /** |
| | 440 | * Class contains some auxiliary functions |
| | 441 | */ |
| | 442 | private static class EN { |
| | 443 | // rotate counter-clock-wise |
| | 444 | public static EastNorth rotate_cc(EastNorth pivot, EastNorth en, double angle) { |
| | 445 | double cosPhi = Math.cos(angle); |
| | 446 | double sinPhi = Math.sin(angle); |
| | 447 | double x = en.east() - pivot.east(); |
| | 448 | double y = en.north() - pivot.north(); |
| | 449 | double nx = cosPhi * x - sinPhi * y + pivot.east(); |
| | 450 | double ny = sinPhi * x + cosPhi * y + pivot.north(); |
| | 451 | return new EastNorth(nx, ny); |
| | 456 | public static EastNorth diff(EastNorth en1, EastNorth en2) { |
| | 457 | return new EastNorth(en1.east() - en2.east(), en1.north() - en2.north()); |
| | 458 | } |
| | 459 | public static double abs(EastNorth en) { |
| | 460 | return Math.sqrt(en.east() * en.east() + en.north() * en.north()); |
| | 461 | } |
| | 462 | public static String toString(EastNorth en) { |
| | 463 | return "["+u(en.east())+","+u(en.north())+"]"; |
| | 464 | } |
| | 465 | public static long u(double d) { |
| | 466 | return Math.round(d * 1000000.); |
| | 467 | } |
| | 468 | public static double polar(EastNorth en1, EastNorth en2) { |
| | 469 | return Math.atan2(en2.north() - en1.north(), en2.east() - en1.east()); |
| | 470 | } |
| | 471 | } |
| 400 | | return h; |
| | 473 | /** |
| | 474 | * Recognize angle to be approximately 0, 90, 180 or 270 degrees. |
| | 475 | * returns a integral value, corresponding to a counter clockwise turn: |
| | 476 | */ |
| | 477 | private static int angleToDirectionChange(double a, double deltaMax) throws RejectedAngleException { |
| | 478 | a = standard_angle_mPI_to_PI(a); |
| | 479 | double d0 = Math.abs(a); |
| | 480 | double d90 = Math.abs(a - Math.PI / 2); |
| | 481 | double d_m90 = Math.abs(a + Math.PI / 2); |
| | 482 | int dirChange; |
| | 483 | if (d0 < deltaMax) dirChange = 0; |
| | 484 | else if (d90 < deltaMax) dirChange = 1; |
| | 485 | else if (d_m90 < deltaMax) dirChange = -1; |
| | 486 | else { |
| | 487 | a = standard_angle_0_to_2PI(a); |
| | 488 | double d180 = Math.abs(a - Math.PI); |
| | 489 | if (d180 < deltaMax) dirChange = 2; |
| | 490 | else { |
| | 491 | throw new RejectedAngleException(); |
| | 492 | } |
| | 493 | } |
| | 494 | return dirChange; |
