92 | | // Fix #7341. Find the first way having all nodes in common to sort them in its nodes order |
93 | | List<Node> consideredNodes = Arrays.asList(n1, n2, n3); |
94 | | for (Way w : selectedWays) { |
95 | | final List<Node> nodes = w.getNodes(); |
96 | | if (nodes.containsAll(consideredNodes)) { |
97 | | Collections.sort(consideredNodes, new Comparator<Node>() { |
98 | | @Override |
99 | | public int compare(Node a, Node b) { |
100 | | return nodes.indexOf(a) - nodes.indexOf(b); |
101 | | } |
102 | | }); |
103 | | n1 = consideredNodes.get(0); |
104 | | n2 = consideredNodes.get(1); |
105 | | n3 = consideredNodes.get(2); |
106 | | break; |
107 | | } |
108 | | } |
109 | | |
110 | | for (Node n : consideredNodes) { |
111 | | targetWays.addAll(n.getParentWays()); |
112 | | } |
131 | | //// Create the new arc nodes. Insert anchor nodes at correct positions. |
132 | | List<Node> arcNodes = new ArrayList<>(points.size()); |
133 | | arcNodes.add(n1); |
134 | | int i = 1; |
135 | | for (EastNorth p : slice(points, 1, -2)) { |
136 | | if (p2Index.value != null && i == p2Index.value) { |
137 | | Node n2new = new Node(n2); |
138 | | n2new.setEastNorth(p); |
139 | | arcNodes.add(n2); // add the original n2, or else we can't find it in the target ways |
140 | | cmds.add(new ChangeCommand(n2, n2new)); |
141 | | } else { |
142 | | Node n = new Node(p); |
143 | | arcNodes.add(n); |
144 | | cmds.add(new AddCommand(ds, n)); |
145 | | } |
146 | | i++; |
| 107 | Way w = null; |
| 108 | if (selectedWays.isEmpty()) { |
| 109 | w = new Way(); |
| 110 | w.setNodes(anchorNodes); |
| 111 | cmds.add(new AddCommand(ds, w)); |
| 112 | } else { |
| 113 | w = selectedWays.get(0); |
| 114 | } |
| 115 | final List<Node> nodes = new ArrayList<>(w.getNodes()); |
| 116 | if (!selectedWays.isEmpty()) { |
| 117 | // Fix #7341. sort nodes in ways nodes order |
| 118 | List<Node> consideredNodes = Arrays.asList(n1, n2, n3); |
| 119 | Collections.sort(consideredNodes, (o1, o2) -> nodes.indexOf(o1) - nodes.indexOf(o2)); |
| 120 | n1 = consideredNodes.get(0); |
| 121 | n2 = consideredNodes.get(1); |
| 122 | n3 = consideredNodes.get(2); |
150 | | Node[] anchorNodes = {n1, n2, n3}; |
151 | | //// "Fuse" the arc with all target ways |
152 | | fuseArc(ds, anchorNodes, arcNodes, targetWays, cmds); |
153 | | |
| 125 | int pos1 = nodes.indexOf(n1); |
| 126 | int pos3 = nodes.indexOf(n3); |
| 127 | Set<Node> fixNodes = new HashSet<>(anchorNodes); |
| 128 | if (!selectedWays.isEmpty()) { |
| 129 | nodes.stream().filter(n -> n.getParentWays().size() > 1).forEach(fixNodes::add); |
| 130 | } |
| 131 | boolean needsUndo = false; |
| 132 | if (!cmds.isEmpty()) { |
| 133 | UndoRedoHandler.getInstance().add(new SequenceCommand("add nodes", cmds)); |
| 134 | needsUndo = true; |
| 135 | } |
| 136 | List<Node> toModify = new ArrayList<>(nodes.subList(pos1, pos3 + 1)); |
| 137 | cmds.addAll(worker(toModify, fixNodes, center, radius, angleSeparation)); |
| 138 | if (toModify.size() > pos3 + 1 - pos1) { |
| 139 | List<Node> changed = new ArrayList<>(); |
| 140 | changed.addAll(nodes.subList(0, pos1)); |
| 141 | changed.addAll(toModify); |
| 142 | changed.addAll(nodes.subList(pos3 + 1, nodes.size())); |
| 143 | Way wNew = new Way(w); |
| 144 | wNew.setNodes(changed); |
| 145 | cmds.add(new ChangeCommand(w, wNew)); |
| 146 | } |
| 147 | if(needsUndo) { |
| 148 | // make sure we don't add the new nodes twice |
| 149 | UndoRedoHandler.getInstance().undo(1); |
| 150 | } |
159 | | for (Way originalTw : targetWays) { |
160 | | Way tw = new Way(originalTw); |
161 | | boolean didChangeTw = false; |
162 | | /// Do one segment at the time (so ways only sharing one segment is fused too) |
163 | | for (int a = 0; a < 2; a++) { |
164 | | int anchorBi = arcNodes.indexOf(anchorNodes[a]); // TODO: optimize away |
165 | | int anchorEi = arcNodes.indexOf(anchorNodes[a + 1]); |
166 | | /// Find the anchor node indices in current target way |
167 | | int bi = -1, ei = -1; |
168 | | int i = -1; |
169 | | // Caution: nodes might appear multiple times. For now only handle simple closed ways |
170 | | for (Node n : tw.getNodes()) { |
171 | | i++; |
172 | | // We look for the first anchor node. The next should be directly to the left or right. |
173 | | // Exception when the way is closed |
174 | | if (Objects.equals(n, anchorNodes[a])) { |
175 | | bi = i; |
176 | | Node otherAnchor = anchorNodes[a + 1]; |
177 | | if (i > 0 && Objects.equals(tw.getNode(i - 1), otherAnchor)) { |
178 | | ei = i - 1; |
179 | | } else if (i < (tw.getNodesCount() - 1) && Objects.equals(tw.getNode(i + 1), otherAnchor)) { |
180 | | ei = i + 1; |
181 | | } else { |
182 | | continue; // this can happen with closed ways. Continue searching for the correct index |
183 | | } |
184 | | break; |
185 | | } |
186 | | } |
187 | | if (bi == -1 || ei == -1) { |
188 | | continue; // this segment is not part of the target way |
189 | | } |
190 | | didChangeTw = true; |
| 157 | // Move each node to that distance from the center. |
| 158 | // Nodes that are not "fix" will be adjust making regular arcs. |
| 159 | int nodeCount = nodes.size(); |
192 | | /// Insert the nodes of this segment |
193 | | // Direction of target way relative to the arc node order |
194 | | int twDirection = ei > bi ? 1 : 0; |
195 | | int anchorI = anchorBi + 1; // don't insert the anchor nodes again |
196 | | int twI = bi + (twDirection == 1 ? 1 : 0); // TODO: explain |
197 | | while (anchorI < anchorEi) { |
198 | | tw.addNode(twI, arcNodes.get(anchorI)); |
199 | | anchorI++; |
200 | | twI += twDirection; |
201 | | } |
202 | | } |
203 | | if (didChangeTw) |
204 | | cmds.add(new ChangeCommand(ds, originalTw, tw)); |
205 | | } |
206 | | } |
| 161 | List<Node> cwTest = new ArrayList<>(nodes); |
| 162 | if (cwTest.get(0) != cwTest.get(cwTest.size() - 1)) { |
| 163 | cwTest.add(cwTest.get(0)); |
| 164 | } |
| 165 | boolean clockWise = Geometry.isClockwise(cwTest); |
| 166 | double maxStep = Math.PI * 2 / (360.0 / angleSeparation); |
208 | | /** |
209 | | * Return a list of coordinates lying an the circle arc determined by n1, n2 and n3. |
210 | | * The order of the list and which of the 3 possible arcs to construct are given by the order of n1, n2, n3 |
211 | | * @param p1 n1 |
212 | | * @param p2 n2 |
213 | | * @param p3 n3 |
214 | | * @param angleSeparation maximum angle separation between the arc points |
215 | | * @param includeAnchors include the anchor points in the list. The original objects will be used, not copies. |
216 | | * If {@code false}, p2 will be replaced by the closest arcpoint. |
217 | | * @param anchor2Index if non-null, it's value will be set to p2's index in the returned list. |
218 | | * @return list of coordinates lying an the circle arc determined by n1, n2 and n3 |
219 | | */ |
220 | | private static List<EastNorth> circleArcPoints(EastNorth p1, EastNorth p2, EastNorth p3, |
221 | | int angleSeparation, boolean includeAnchors, ReturnValue<Integer> anchor2Index) { |
222 | | |
223 | | // triangle: three single nodes needed or a way with three nodes |
224 | | |
225 | | // let's get some shorter names |
226 | | double x1 = p1.east(); |
227 | | double y1 = p1.north(); |
228 | | double x2 = p2.east(); |
229 | | double y2 = p2.north(); |
230 | | double x3 = p3.east(); |
231 | | double y3 = p3.north(); |
232 | | |
233 | | // calculate the center (xc,yc) |
234 | | double s = 0.5 * ((x2 - x3) * (x1 - x3) - (y2 - y3) * (y3 - y1)); |
235 | | double sUnder = (x1 - x2) * (y3 - y1) - (y2 - y1) * (x1 - x3); |
236 | | |
237 | | assert (sUnder != 0); |
238 | | |
239 | | s /= sUnder; |
240 | | |
241 | | double xc = 0.5 * (x1 + x2) + s * (y2 - y1); |
242 | | double yc = 0.5 * (y1 + y2) + s * (x1 - x2); |
243 | | |
244 | | // calculate the radius (r) |
245 | | double r = Math.sqrt(Math.pow(xc - x1, 2) + Math.pow(yc - y1, 2)); |
246 | | |
247 | | // The angles of the anchor points relative to the center |
248 | | double realA1 = calcang(xc, yc, x1, y1); |
249 | | double realA2 = calcang(xc, yc, x2, y2); |
250 | | double realA3 = calcang(xc, yc, x3, y3); |
251 | | |
252 | | double startAngle = realA1; |
253 | | // Transform the angles to get a consistent starting point |
254 | | double a2 = normalizeAngle(realA2 - startAngle); |
255 | | double a3 = normalizeAngle(realA3 - startAngle); |
256 | | int direction = a3 > a2 ? 1 : -1; |
257 | | |
258 | | double radialLength = 0; |
259 | | if (direction == 1) { // counter clockwise |
260 | | radialLength = a3; |
261 | | } else { // clockwise |
262 | | radialLength = Math.PI * 2 - a3; |
263 | | // make the angles consistent with the direction. |
264 | | a2 = (Math.PI * 2 - a2); |
| 168 | // Search first fixed node |
| 169 | int startPosition; |
| 170 | for (startPosition = 0; startPosition < nodeCount; startPosition++) { |
| 171 | if (fixNodes.contains(nodes.get(startPosition))) |
| 172 | break; |
270 | | // Calculate the circle points in order |
271 | | double stepLength = radialLength / (numberOfNodesInArc-1); |
272 | | // Determine closest index to p2 |
273 | | |
274 | | int indexJustBeforeP2 = (int) Math.floor(a2 / stepLength); |
275 | | int closestIndexToP2 = indexJustBeforeP2; |
276 | | if ((a2 - indexJustBeforeP2 * stepLength) > ((indexJustBeforeP2 + 1) * stepLength - a2)) { |
277 | | closestIndexToP2 = indexJustBeforeP2 + 1; |
| 183 | PolarCoor pcFirst = new PolarCoor(radius, PolarCoor.computeAngle(first.getEastNorth(), center), center); |
| 184 | addMoveCommandIfNeeded(first, pcFirst, cmds); |
| 185 | if (j < nodeCount) { |
| 186 | double delta; |
| 187 | PolarCoor pcLast = new PolarCoor(nodes.get(j).getEastNorth(), center); |
| 188 | delta = pcLast.angle - pcFirst.angle; |
| 189 | if (!clockWise && delta < 0) { |
| 190 | delta += 2 * Math.PI; |
| 191 | } else if (clockWise && delta > 0) { |
| 192 | delta -= 2 * Math.PI; |
| 193 | } |
| 194 | // do we have enough nodes to produce a nice circle? |
| 195 | int numToAdd = Math.max((int) Math.ceil(Math.abs(delta / maxStep)), j - i) - (j-i); |
| 196 | double step = delta / (numToAdd + j - i); |
| 197 | for (int k = i + 1; k < j; k++) { |
| 198 | PolarCoor p = new PolarCoor(radius, pcFirst.angle + (k - i) * step, center); |
| 199 | addMoveCommandIfNeeded(nodes.get(k), p, cmds); |
| 200 | } |
| 201 | // add needed nodes |
| 202 | for (int k = j; k < j + numToAdd; k++) { |
| 203 | PolarCoor p = new PolarCoor(radius, pcFirst.angle + (k - i) * step, center); |
| 204 | Node nNew = new Node(p.toEastNorth()); |
| 205 | nodes.add(k, nNew); |
| 206 | cmds.add(new AddCommand(nodes.get(0).getDataSet(), nNew)); |
| 207 | } |
| 208 | j += numToAdd; |
| 209 | nodeCount += numToAdd; |
| 210 | } |
| 211 | i = j; // Update start point for next iteration |
279 | | // can't merge with end node |
280 | | if (closestIndexToP2 == numberOfNodesInArc - 1) { |
281 | | closestIndexToP2--; |
282 | | } else if (closestIndexToP2 == 0) { |
283 | | closestIndexToP2++; |
284 | | } |
285 | | assert (closestIndexToP2 != 0); |
286 | | |
287 | | double a = direction * stepLength; |
288 | | points.add(p1); |
289 | | if (indexJustBeforeP2 == 0 && includeAnchors) { |
290 | | points.add(p2); |
291 | | } |
292 | | // i is ahead of the real index by one, since we need to be ahead in the angle calculation |
293 | | for (int i = 2; i < numberOfNodesInArc; i++) { |
294 | | double nextA = direction * (i * stepLength); |
295 | | double realAngle = a + startAngle; |
296 | | double x = xc + r * Math.cos(realAngle); |
297 | | double y = yc + r * Math.sin(realAngle); |
298 | | |
299 | | points.add(new EastNorth(x, y)); |
300 | | if (i - 1 == indexJustBeforeP2 && includeAnchors) { |
301 | | points.add(p2); |
302 | | } |
303 | | a = nextA; |
304 | | } |
305 | | points.add(p3); |
306 | | if (anchor2Index != null) { |
307 | | anchor2Index.value = closestIndexToP2; |
308 | | } |
309 | | return points; |
| 213 | return cmds; |
312 | | // gah... why can't java support "reverse indices"? |
313 | | private static <T> List<T> slice(List<T> list, int from, int to) { |
314 | | if (to < 0) |
315 | | to += list.size() + 1; |
316 | | return list.subList(from, to); |
317 | | } |
| 216 | private static void addMoveCommandIfNeeded(Node n, PolarCoor coor, Collection<Command> cmds) { |
| 217 | EastNorth en = coor.toEastNorth(); |
| 218 | double deltaEast = en.east() - n.getEastNorth().east(); |
| 219 | double deltaNorth = en.north() - n.getEastNorth().north(); |
| 220 | if (Math.abs(deltaEast) > 1e-7 || Math.abs(deltaNorth) > 1e-7) { |
| 221 | cmds.add(new MoveCommand(n, deltaEast, deltaNorth)); |
| 222 | } |
| 223 | } |
319 | | /** |
320 | | * Normalizes {@code angle} so it is between 0 and 2 PI |
321 | | * @param angle the angle |
322 | | * @return the normalized angle |
323 | | */ |
324 | | private static double normalizeAngle(double angle) { |
325 | | double PI2 = Math.PI * 2; |
326 | | if (angle < 0) { |
327 | | angle = angle + (Math.floor(-angle / PI2) + 1) * PI2; |
328 | | } else if (angle >= PI2) { |
329 | | angle = angle - Math.floor(angle / PI2) * PI2; |
330 | | } |
331 | | return angle; |
332 | | } |
333 | | |
334 | | private static double calcang(double xc, double yc, double x, double y) { |
335 | | // calculate the angle from xc|yc to x|y |
336 | | if (xc == x && yc == y) |
337 | | return 0; // actually invalid, but we won't have this case in this context |
338 | | double yd = Math.abs(y - yc); |
339 | | if (yd == 0 && xc < x) |
340 | | return 0; |
341 | | if (yd == 0 && xc > x) |
342 | | return Math.PI; |
343 | | double xd = Math.abs(x - xc); |
344 | | double a = Math.atan2(xd, yd); |
345 | | if (y > yc) { |
346 | | a = Math.PI - a; |
347 | | } |
348 | | if (x < xc) { |
349 | | a = -a; |
350 | | } |
351 | | a = 1.5 * Math.PI + a; |
352 | | if (a < 0) { |
353 | | a += 2 * Math.PI; |
354 | | } |
355 | | if (a >= 2 * Math.PI) { |
356 | | a -= 2 * Math.PI; |
357 | | } |
358 | | return a; |
359 | | } |
360 | | |
361 | | public static class ReturnValue<T> { |
362 | | public T value; |
363 | | } |