Ticket #18847: 18847.1.patch

File 18847.1.patch, 18.9 KB (added by GerdP, 3 months ago)
  • src/org/openstreetmap/josm/plugins/utilsplugin2/curves/CircleArcMaker.java

     
    55import java.util.Arrays;
    66import java.util.Collection;
    77import java.util.Collections;
    8 import java.util.Comparator;
    98import java.util.HashSet;
    109import java.util.Iterator;
    1110import java.util.LinkedList;
    1211import java.util.List;
    13 import java.util.Objects;
    1412import java.util.Set;
    1513
    1614import org.openstreetmap.josm.command.AddCommand;
    1715import org.openstreetmap.josm.command.ChangeCommand;
    1816import org.openstreetmap.josm.command.Command;
     17import org.openstreetmap.josm.command.MoveCommand;
     18import org.openstreetmap.josm.command.SequenceCommand;
     19import org.openstreetmap.josm.data.UndoRedoHandler;
    1920import org.openstreetmap.josm.data.coor.EastNorth;
     21import org.openstreetmap.josm.data.coor.PolarCoor;
    2022import org.openstreetmap.josm.data.osm.DataSet;
    2123import org.openstreetmap.josm.data.osm.Node;
    2224import org.openstreetmap.josm.data.osm.Way;
    2325import org.openstreetmap.josm.gui.MainApplication;
     26import org.openstreetmap.josm.tools.Geometry;
    2427
    2528/**
    2629 * Create a circle arc
     
    3235    }
    3336
    3437    public static Collection<Command> doCircleArc(List<Node> selectedNodes, List<Way> selectedWays, int angleSeparation) {
    35         Collection<Command> cmds = new LinkedList<>();
     38        List<Command> cmds = new LinkedList<>();
    3639
    3740        //// Decides which nodes to use as anchors based on selection
    3841        /*
     
    5558        //// Anchor nodes
    5659        Node n1 = null, n2 = null, n3 = null;
    5760
    58         Set<Way> targetWays = new HashSet<>();
    5961        DataSet ds = MainApplication.getLayerManager().getEditDataSet();
    6062
     63        if (selectedWays.size() > 1)
     64                return Collections.emptyList();
     65
    6166        boolean nodesHaveBeenChoosen = false;
    6267        if (selectedNodes.size() == 3) {
    6368            Iterator<Node> nodeIter = selectedNodes.iterator();
     
    6570            n2 = nodeIter.next();
    6671            n3 = nodeIter.next();
    6772            nodesHaveBeenChoosen = true;
    68             if (selectedWays.isEmpty()) { // Create a brand new way
    69                 Way newWay = new Way();
    70                 targetWays.add(newWay);
    71                 cmds.add(new AddCommand(ds, newWay));
    72                 newWay.addNode(n1);
    73                 newWay.addNode(n2);
    74                 newWay.addNode(n3);
    75             }
    7673        }
    7774        if (!selectedWays.isEmpty()) {
    7875            // TODO: use only two nodes inferring the orientation from the parent way.
    79 
     76                Way w = selectedWays.iterator().next();
    8077            if (!nodesHaveBeenChoosen) {
    8178                // Use the three last nodes in the way as anchors. This is intended to be used with the
    8279                // built in draw mode
    83                 Way w = selectedWays.iterator().next(); //TODO: select last selected way instead
     80
    8481                int nodeCount = w.getNodesCount();
    8582                if (nodeCount < 3)
    86                     return null;
     83                        return Collections.emptyList();
    8784                n3 = w.getNode(nodeCount - 1);
    8885                n2 = w.getNode(nodeCount - 2);
    8986                n1 = w.getNode(nodeCount - 3);
    9087                nodesHaveBeenChoosen = true;
    9188            }
    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             }
    11389        }
    114         if (!nodesHaveBeenChoosen) {
    115             return null;
     90        List<Node> anchorNodes = Arrays.asList(n1, n2, n3);
     91        if (!nodesHaveBeenChoosen || (!selectedWays.isEmpty() && !selectedWays.get(0).getNodes().containsAll(anchorNodes)))  {
     92                return Collections.emptyList();
    11693        }
    11794
    11895        EastNorth p1 = n1.getEastNorth();
     
    12198
    12299        // make sure that points are different
    123100        if (p1.equals(p2) || p1.equals(p3) || p2.equals(p3)) {
    124             return null;
     101                return Collections.emptyList();
    125102        }
    126103
    127         // // Calculate the new points in the arc
    128         ReturnValue<Integer> p2Index = new ReturnValue<>();
    129         List<EastNorth> points = circleArcPoints(p1, p2, p3, angleSeparation, false, p2Index);
     104        EastNorth center = Geometry.getCenter(anchorNodes);
     105        double radius = center.distance(n1.getEastNorth());
    130106
    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);
    147123        }
    148         arcNodes.add(n3);
    149124
    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        }
    154151        return cmds;
    155152    }
    156153
    157     private static void fuseArc(DataSet ds, Node[] anchorNodes, List<Node> arcNodes, Set<Way> targetWays, Collection<Command> cmds) {
     154    private static List<Command> worker(List<Node> nodes, Set<Node> fixNodes, EastNorth center, double radius, int angleSeparation) {
     155        List<Command> cmds = new LinkedList<>();
    158156
    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();
    191160
    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);
    207167
    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;
    265173        }
    266         int numberOfNodesInArc = Math.max((int) Math.ceil((radialLength / Math.PI) * 180 / angleSeparation)+1,
    267                 3);
    268         List<EastNorth> points = new ArrayList<>(numberOfNodesInArc);
     174        int i = startPosition; // Start position for current arc
     175        int j; // End position for current arc
     176        while (i < nodeCount) {
     177                        for (j = i + 1; j < nodeCount; j++) {
     178                                if (fixNodes.contains(nodes.get(j)))
     179                                        break;
     180                        }
     181            Node first = nodes.get(i);
    269182
    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
    278212        }
    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;
    310214    }
    311215
    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        }
    318224
    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     }
    364225}