Changeset 2268 in josm for trunk

Timestamp:

2009-10-10T20:33:54+02:00 (16 years ago)

Author:

stoecker

Message:

fix #3571 - patch by bastiK - improve orthogonalization action

Location:

trunk/src/org/openstreetmap/josm

Files:

• actions/OrthogonalizeAction.java (modified) (4 diffs)
• gui/MainMenu.java (modified) (1 diff)

trunk/src/org/openstreetmap/josm/actions/OrthogonalizeAction.java

@@ -8 +8 @@
 import java.awt.event.KeyEvent;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.Collection;
+import java.util.HashSet;
+import java.util.HashMap;
 import java.util.LinkedList;
 
@@ -26 +29 @@
 
 /**
- * Align edges of a way so all angles are right angles.
+ * Tools / Orthogonalize
  *
- * 1. Find orientation of all edges
- * 2. Compute main orientation, weighted by length of edge, normalized to angles between 0 and pi/2
- * 3. Rotate every edge around its center to align with main orientation or perpendicular to it
- * 4. Compute new intersection points of two adjascent edges
- * 5. Move nodes to these points
- * 6. if there are nodes between edges then align the nodes
+ * Align edges of a way so all angles are angles of 90 or 180 degrees.
+ * See USAGE String below.
  */
 public final class OrthogonalizeAction extends JosmAction {
+    String USAGE = "<h3>"+
+            "When one or more ways are selected, the shape is adjusted, such that all angles are 90 or 180 degrees.<h3>"+
+            "You can add two nodes to the selection. Then the direction is fixed by these two reference nodes.<h3>"+
+            "(Afterwards, you can undo the movement for certain nodes:<br>"+
+            "Select them and press the shortcut for Orthogonalize / Undo. The default is Shift-Q.)";
 
     public OrthogonalizeAction() {
@@ -46 +50 @@
     }
 
+    /**
+     * excepted deviation from an angle of 0, 90, 180, 360 degrees
+     * maximum value: 45 degrees
+     */
+    private static final double TOLERANCE1 = Math.toRadians(35.);   // within a way
+    private static final double TOLERANCE2 = Math.toRadians(35.);   // ways relative to each other
+
+    /**
+     * Remember movements, so the user can later undo it for certain nodes
+     */
+    private static final HashMap<Node, EastNorth> rememberMovements = new HashMap<Node, EastNorth>();
+
+    /**
+     * Undo the previous orthogonalization for certain nodes.
+     *
+     * This is useful, if the way shares nodes that you don't like to change, e.g. imports or
+     * work of another user.
+     *
+     * This action can be triggered by shortcut only.
+     */
+    public class Undo extends JosmAction {
+        public Undo() {
+            super(tr("Orthogonalize Shape / Undo"),
+                "ortho",
+                tr("Undo orthogonalization for certain nodes"),
+                Shortcut.registerShortcut("tools:orthogonalizeUndo", tr("Tool: {0}", tr("Orthogonalize Shape / Undo")),
+                        KeyEvent.VK_Q,
+                        Shortcut.GROUP_EDIT, Shortcut.SHIFT_DEFAULT), true);
+        }
+        public void actionPerformed(ActionEvent e) {
+            if (!isEnabled())
+                return;
+            final Collection<Command> commands = new LinkedList<Command>();
+            final Collection<OsmPrimitive> sel = getCurrentDataSet().getSelected();
+            try {
+                for (OsmPrimitive p : sel) {
+                    if (! (p instanceof Node)) throw new InvalidUserInputException();
+                    Node n = (Node) p;
+                    if (rememberMovements.containsKey(n)) {
+                        EastNorth tmp = rememberMovements.get(n);
+                        commands.add(new MoveCommand(n, - tmp.east(), - tmp.north()));
+                        rememberMovements.remove(n);
+                    }
+                }
+                if (commands.size() > 0) {
+                    Main.main.undoRedo.add(new SequenceCommand(tr("Orthogonalize / Undo"), commands));
+                    Main.map.repaint();
+                } else throw new InvalidUserInputException();
+            }
+            catch (InvalidUserInputException ex) {
+                JOptionPane.showMessageDialog(
+                    Main.parent,
+                    tr("Orthogonalize Shape / Undo\n"+
+                        "Please select nodes that were moved by the previous Orthogonalize Shape action!"),
+                    tr("Undo Orthogonalize Shape"),
+                    JOptionPane.INFORMATION_MESSAGE);
+            }
+        }
+    }
+
     public void actionPerformed(ActionEvent e) {
         if (!isEnabled())
             return;
-        Collection<OsmPrimitive> sel = getCurrentDataSet().getSelected();
-
-        ArrayList<Node> dirnodes = new ArrayList<Node>();
-        ArrayList<Node> alignNodes = new ArrayList<Node>();
-
-        // Check the selection if it is suitable for the orthogonalisation
-        for (OsmPrimitive osm : sel) {
-            // Check if not more than two nodes in the selection
-            if(osm instanceof Node) {
-                if(dirnodes.size() == 2) {
-                    JOptionPane.showMessageDialog(
-                            Main.parent,
-                            tr("Only two nodes allowed"),
-                            tr("Information"),
-                            JOptionPane.INFORMATION_MESSAGE
-                    );
-                    return;
-                }
-                dirnodes.add((Node) osm);
-                continue;
-            }
-            // Check if selection consists now only of ways
-            if (!(osm instanceof Way)) {
-                JOptionPane.showMessageDialog(
-                        Main.parent,
-                        tr("Selection must consist only of ways."),
-                        tr("Information"),
-                        JOptionPane.INFORMATION_MESSAGE
-                );
-                return;
-            }
-
-            // Check if every way is made of at least four segments and closed
-            Way way = (Way)osm;
-            if ((way.getNodesCount() < 5) || !way.isClosed()) {
-                JOptionPane.showMessageDialog(
-                        Main.parent,
-                        tr("Please select one or more closed ways of at least four nodes."),
-                        tr("Information"),
-                        JOptionPane.INFORMATION_MESSAGE
-                );
-                return;
-            }
-
-            // Check if every edge in the way is a definite edge of at least 45 degrees of direction change
-            // Otherwise, two segments could be turned into same direction and intersection would fail.
-            // Or changes of shape would be too serious.
-            for (int i1=0; i1 < way.getNodesCount()-1; i1++) {
-                int i2 = (i1+1) % (way.getNodesCount()-1);
-                int i3 = (i1+2) % (way.getNodesCount()-1);
-                double angle1  =Math.abs(way.getNode(i1).getEastNorth().heading(way.getNode(i2).getEastNorth()));
-                double angle2 = Math.abs(way.getNode(i2).getEastNorth().heading(way.getNode(i3).getEastNorth()));
-                double delta = Math.abs(angle2 - angle1);
-                while(delta > Math.PI) {
-                    delta -= Math.PI;
-                }
-                if(delta < Math.PI/4) {
-                    // not an edge
-                    alignNodes.add(way.getNode(i2));
-                }
-            }
-
-            // first node has to be an edge so we move the node to the end of the way
-            while (alignNodes.contains(way.firstNode())) {
-                Node n = way.firstNode();
-                way.removeNode(n);
-                way.addNode(way.getNodesCount() - 2, n); // ! -2 because first node == last node in closed way
-            }
-        }
-
         if ("EPSG:4326".equals(Main.proj.toString())) {
             String msg = tr("<html>You are using the EPSG:4326 projection which might lead<br>" +
@@ -134 +128 @@
                 return;
         }
-        // Check, if selection held neither none nor two nodes
-        if(dirnodes.size() == 1) {
-            JOptionPane.showMessageDialog(
+
+        final ArrayList<Node> nodeList = new ArrayList<Node>();
+        final ArrayList<WayData> wayDataList = new ArrayList<WayData>();
+        final Collection<OsmPrimitive> sel = getCurrentDataSet().getSelected();
+
+        try {
+            // collect nodes and ways from the selection
+            for (OsmPrimitive p : sel) {
+                if (p instanceof Node) {
+                    nodeList.add((Node) p);
+                }
+                else if (p instanceof Way) {
+                    wayDataList.add(new WayData((Way) p));
+                }
+                else {      // maybe a relation got selected...
+                    throw new InvalidUserInputException("Selection must consist only of ways and nodes.");
+                }
+            }
+            if (wayDataList.isEmpty()) {
+                throw new InvalidUserInputException("usage");
+            }
+            else  {
+                if (nodeList.size() == 2) {
+                    orthogonalize(wayDataList, nodeList);
+                }
+                else if (nodeList.isEmpty()) {
+                    orthogonalize(wayDataList, nodeList);
+                }
+                else {
+                    throw new InvalidUserInputException("usage");
+                }
+            }
+        } catch (InvalidUserInputException ex) {
+            if (ex.getMessage().equals("usage")) {
+                JOptionPane.showMessageDialog(
                     Main.parent,
-                    tr("Only one node selected"),
-                    tr("Warning"),
-                    JOptionPane.WARNING_MESSAGE
-            );
-            return;
-        }
-
-        // Now all checks are done and we can now do the neccessary computations
-        // From here it is assumed that the above checks hold
-        Collection<Command> cmds = new LinkedList<Command>();
-        double align_to_heading = 0.0;
-        boolean use_dirnodes = false;
-
-        if (dirnodes.size() == 2) {
-            // When selection contains two nodes, use the nodes to compute a direction
-            // to align all ways to
-            align_to_heading = normalize_angle(dirnodes.get(0).getEastNorth().heading(dirnodes.get(1).getEastNorth()));
-            use_dirnodes = true;
-        }
-
-        for (OsmPrimitive osm : sel) {
-            if(!(osm instanceof Way)) {
-                continue;
-            }
-
-            Way oldWay = (Way) osm;
-            Way way = new Way();
-            // copy only edges into way
-            for (Node origNode : oldWay.getNodes()) {
-                if (alignNodes.contains(origNode)) {
-                    continue;
-                }
-                way.addNode(origNode);
-            }
-            int nodes = way.getNodesCount();
-            int sides = nodes - 1;
-            // Copy necessary data into a more suitable data structure
-            EastNorth en[] = new EastNorth[sides];
-            for (int i = 0; i < sides; i++) {
-                en[i] = new EastNorth(way.getNode(i).getEastNorth().east(), way.getNode(i).getEastNorth().north());
-            }
-
-            if (! use_dirnodes) {
-                // To find orientation of all segments, compute weighted average of all segment's headings
-                // all headings are mapped into [-PI/4, PI/4] by PI/2 rotations so both main orientations are mapped into one
-                // the headings are weighted by the length of the segment establishing it, so a longer segment, that is more
-                // likely to have the correct orientation, has more influence in the computing than a short segment, that is easier to misalign.
-                double headings[] = new double[sides];
-                double weights[] = new double[sides];
-                for (int i=0; i < sides; i++) {
-                    headings[i] = normalize_angle(way.getNode(i).getEastNorth().heading(way.getNode(i+1).getEastNorth()));
-                    weights[i] = way.getNode(i).getEastNorth().distance(way.getNode(i+1).getEastNorth());
-                }
-
-                // CAVEAT: for orientations near -PI/4 or PI/4 the mapping into ONE orientation fails
-                //         resulting in a heading-difference between adjacent sides of almost PI/2
-                //         and a totally wrong average
-                // check for this (use PI/3 as arbitray limit) and rotate into ONE orientation
-                double angle_diff_max = 0.0;
-                for (int i=0; i < sides; i++) {
-                    double diff = 0.0;
-                    if (i == 0) {
-                        diff = heading_diff(headings[i], headings[sides - 1]);
-                    } else {
-                        diff = heading_diff(headings[i], headings[i - 1]);
-                    }
-                    if (diff > angle_diff_max) {
-                        angle_diff_max = diff;
-                    }
-                }
-
-                if (angle_diff_max > Math.PI/3) {
-                    // rearrange headings: everything < 0 gets PI/2-rotated
-                    for (int i=0; i < sides; i++) {
-                        if (headings[i] < 0) {
-                            headings[i] += Math.PI/2;
+                    "<html><h2>"+tr("Usage")+tr(USAGE),
+                    tr("Orthogonalize Shape"),
+                    JOptionPane.INFORMATION_MESSAGE);
+            }
+            else {
+                JOptionPane.showMessageDialog(
+                    Main.parent, 
+                    "<html><h3>"+tr(ex.getMessage())+"<br><hr><h3>"+tr("Usage")+tr(USAGE),
+                    tr("Selected Elements cannot be orthogonalized"),
+                    JOptionPane.INFORMATION_MESSAGE);
+            }
+        }
+    }
+
+    /**
+     *
+     *  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 void orthogonalize(ArrayList<WayData> wayDataList, ArrayList<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;
+                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();
+                }
+                EastNorth totSum = new EastNorth(0., 0.);
+                for (WayData w : wayDataList) {
+                    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(
+                "<html>Please make sure all selected ways head in a similar direction<br>"+
+                "or orthogonalize them one by one.");
+        }
+
+        // put the nodes of all ways in a set
+        final HashSet<Node> allNodes = new HashSet<Node>();
+        for (WayData w : wayDataList) {
+            for (Node n : w.way.getNodes()) {
+                allNodes.add(n);
+            }
+        }
+
+        // the new x and y value for each node
+        final HashMap<Node, Double> nX = new HashMap<Node, Double>();
+        final HashMap<Node, Double> nY = new HashMap<Node, Double>();
+
+        // caluclate 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.rotate_cc(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 HashSet<Node> s = new HashSet<Node>(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 HashSet<Node> cs = new HashSet<Node>(); // 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;
+                                }
+                            }
                         }
                     }
                 }
 
-                // TODO:
-                // use angle_diff_max as an indicator that the way is already orthogonal
-                // e.g. if angle_diff_max is less then Math.toRadians(0.5)
-                // and do nothing in that case (?)
-
-                // Compute the weighted average of the headings of all segments
-                double sum_weighted_headings = 0.0;
-                double sum_weights = 0.0;
-                for (int i=0; i < sides; i++) {
-                    sum_weighted_headings += headings[i] * weights[i];
-                    sum_weights += weights[i];
-                }
-                align_to_heading = normalize_angle(sum_weighted_headings/sum_weights);
-            }
-
-            EastNorth aligna = null;
-            EastNorth alignb = null;
-            EastNorth align0 = null;
-            Node nodea = null;
-            Node nodeb = null;
-            Node node0 = null;
-
-            for (int i=0; i < sides; i++) {
-                // Compute handy indices of three nodes to be used in one loop iteration.
-                // We use segments (i1,i2) and (i2,i3), align them and compute the new
-                // position of the i2-node as the intersection of the realigned (i1,i2), (i2,i3) segments
-                // Not the most efficient algorithm, but we don't handle millions of nodes...
-                int i1 = i;
-                int i2 = (i+1)%sides;
-                int i3 = (i+2)%sides;
-                double heading1, heading2;
-                double delta1, delta2;
-                // Compute neccessary rotation of first segment to align it with main orientation
-                heading1 = normalize_angle(en[i1].heading(en[i2]), align_to_heading);
-                delta1 = align_to_heading - heading1;
-                // Compute neccessary rotation of second segment to align it with main orientation
-                heading2 = normalize_angle(en[i2].heading(en[i3]), align_to_heading);
-                delta2 = align_to_heading - heading2;
-                // To align a segment, rotate around its center
-                EastNorth pivot1 = new EastNorth((en[i1].east()+en[i2].east())/2, (en[i1].north()+en[i2].north())/2);
-                EastNorth A=en[i1].rotate(pivot1, delta1);
-                EastNorth B=en[i2].rotate(pivot1, delta1);
-                EastNorth pivot2 = new EastNorth((en[i2].east()+en[i3].east())/2, (en[i2].north()+en[i3].north())/2);
-                EastNorth C=en[i2].rotate(pivot2, delta2);
-                EastNorth D=en[i3].rotate(pivot2, delta2);
-
-                // compute intersection of segments
-                double u=det(B.east() - A.east(), B.north() - A.north(),
-                        C.east() - D.east(), C.north() - D.north());
-
-                // Check for parallel segments and do nothing if they are
-                // In practice this will probably only happen when a way has
-                // been duplicated
-
-                if (u == 0) {
-                    continue;
-                }
-
-                // q is a number between 0 and 1
-                // It is the point in the segment where the intersection occurs
-                // if the segment is scaled to length 1
-
-                double q = det(B.north() - C.north(), B.east() - C.east(),
-                        D.north() - C.north(), D.east() - C.east()) / u;
-                EastNorth intersection = new EastNorth(
-                        B.east() + q * (A.east() - B.east()),
-                        B.north() + q * (A.north() - B.north()));
-
-                Node n = way.getNode(i2);
-
-                LatLon ill = Main.proj.eastNorth2latlon(intersection);
-                if (!ill.equalsEpsilon(n.getCoor())) {
-                    double dx = intersection.east()-n.getEastNorth().east();
-                    double dy = intersection.north()-n.getEastNorth().north();
-                    cmds.add(new MoveCommand(n, dx, dy));
-                }
-
-                // align all nodes between two edges
-                aligna = alignb;
-                alignb = intersection;
-                nodea = nodeb;
-                nodeb = n;
-                if (aligna != null) {
-
-                    MoveCommand cmd = alignSide(findNodesToAlign(oldWay, nodea, nodeb), aligna, alignb);
-                    if (cmd != null) {
-                        cmds.add(cmd);
+                final HashMap<Node, Double> nC = (orientation == HORIZONTAL) ? nY : nX;
+
+                double average = 0;
+                for (Node n : cs) {
+                    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);
                     }
-
-                } else {
-                    align0 = alignb;
-                    node0 = nodeb;
-                }
-            }
-            MoveCommand cmd = alignSide(findNodesToAlign(oldWay, nodeb, node0), alignb, align0);
-            if (cmd != null) {
-                cmds.add(cmd);
-            }
-        }
-
-        if (cmds.size() > 0) {
-            Main.main.undoRedo.add(new SequenceCommand(tr("Orthogonalize"), cmds));
-            Main.map.repaint();
-        }
-    }
-
-    private MoveCommand alignSide(ArrayList<Node> aNodes, EastNorth aligna, EastNorth alignb) {
-
-        // Find out co-ords of A and B
-        double ax = aligna.east();
-        double ay = aligna.north();
-        double bx = alignb.east();
-        double by = alignb.north();
-
-        // OK, for each node to move, work out where to move it!
-        for (Node n1 : aNodes) {
-            // Get existing co-ords of node to move
-            double nx = n1.getEastNorth().east();
-            double ny = n1.getEastNorth().north();
-
-            if (ax == bx) {
-                // Special case if AB is vertical...
-                nx = ax;
-            } else if (ay == by) {
-                // ...or horizontal
-                ny = ay;
-            } else {
-                // Otherwise calculate position by solving y=mx+c
-                double m1 = (by - ay) / (bx - ax);
-                double c1 = ay - (ax * m1);
-                double m2 = (-1) / m1;
-                double c2 = n1.getEastNorth().north() - (n1.getEastNorth().east() * m2);
-
-                nx = (c2 - c1) / (m1 - m2);
-                ny = (m1 * nx) + c1;
-            }
-
-            // Return the command to move the node to its new position.
-            return new MoveCommand(n1, nx - n1.getEastNorth().east(), ny - n1.getEastNorth().north());
-        }
-        return null;
-    }
-
-    private ArrayList<Node> findNodesToAlign(Way w, Node from, Node to) {
-        ArrayList<Node> l = new ArrayList<Node>();
-        boolean start = false;
-        for (int i = 0; i < w.getNodesCount(); i++) {
-            Node n = w.getNode(i % w.getNodesCount());
-            if (n.equals(to)) {
-                break;
-            }
-            if (start) {
-                l.add(n);
-            }
-            if (n.equals(from)) {
-                start = true;
-            }
-
-        }
-        return l;
-    }
-
-    static double det(double a, double b, double c, double d)
-    {
-        return a * d - b * c;
-    }
-
-    static double normalize_angle(double h) {
-        return normalize_angle(h, 0.0);
-    }
-    static double normalize_angle(double h, double align_to) {
-        double llimit = -Math.PI/4;
-        double ulimit = Math.PI/4;
-        while (h - align_to > ulimit) {
-            h -= Math.PI/2;
-        }
-        while (h - align_to < llimit) {
-            h += Math.PI/2;
-        }
-
-        return h;
-    }
-
-    static double heading_diff(double h1, double h2) {
-        double heading_delta = h1 > h2 ? h1 - h2 : h2 - h1;
-        return heading_delta;
-    }
-
+                }
+
+                for (Node n : cs) {
+                    nC.put(n, average);
+                }
+
+                for (Node n : cs) {
+                    s.remove(n);
+                }
+            }
+            if (!s.isEmpty()) throw new RuntimeException();
+        }
+
+        // rotate back and log the change
+        final Collection<Command> commands = new LinkedList<Command>();
+        OrthogonalizeAction.rememberMovements.clear();
+        for (Node n: allNodes) {
+            EastNorth tmp = new EastNorth(nX.get(n), nY.get(n));
+            tmp = EN.rotate_cc(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));
+            }
+        }
+        Main.main.undoRedo.add(new SequenceCommand(tr("Orthogonalize"), commands));
+        Main.map.repaint();
+    }
+    
+
+    /**
+     * Class contains everything we need to know about a singe way.
+     */
+    private static class WayData {
+        final public Way way;             // The assigned way
+        final public int nSeg;            // Number of Segments of the Way
+        final public int nNode;           // Number of Nodes of the Way
+        public Direction[] segDirections; // Direction of the segments
+                                          // segment i goes from node i to node (i+1)
+        public EastNorth segSum;          // (Vector-)sum of all horizontal segments plus the sum of all vertical
+                                          //     segments turned by 90 degrees
+        public double heading;            // heading of segSum == approximate heading of the way
+        public WayData(Way pWay) {
+            way = pWay;
+            nNode = way.getNodes().size();
+            nSeg = nNode - 1;
+        }
+        /**
+         * Estimate the direction of the segments, given the first segment points in the
+         * direction <code>pInitialDirection</code>.
+         * Then sum up all horizontal / vertical segments to have a good guess for the
+         * heading of the entire way.
+         */
+        public void calcDirections(Direction pInitialDirection) throws InvalidUserInputException {
+            final EastNorth[] en = new EastNorth[nNode]; // alias: way.getNodes().get(i).getEastNorth() ---> en[i]
+            for (int i=0; i < nNode; i++) {
+                en[i] = new EastNorth(way.getNodes().get(i).getEastNorth().east(), way.getNodes().get(i).getEastNorth().north());
+            }
+            segDirections = new Direction[nSeg];
+            Direction direction = pInitialDirection;
+            segDirections[0] = direction;
+            for (int i=0; i < nSeg - 1; i++) {
+                double h1 = EN.polar(en[i],en[i+1]);
+                double h2 = EN.polar(en[i+1],en[i+2]);
+                try {
+                    direction = direction.changeBy(angleToDirectionChange(h2 - h1, TOLERANCE1));
+                } catch (RejectedAngleException ex) {
+                    throw new InvalidUserInputException("Please select ways with angles of approximately 90 or 180 degrees.");
+                }
+                segDirections[i+1] = direction;
+            }
+
+            // sum up segments
+            EastNorth h = new EastNorth(0.,0.);
+            double lh = EN.abs(h);
+            EastNorth v = new EastNorth(0.,0.);
+            double lv = EN.abs(v);
+            for (int i = 0; i < nSeg; ++i) {
+                EastNorth segment = EN.diff(en[i+1], en[i]);
+                if      (segDirections[i] == Direction.RIGHT) h = EN.sum(h,segment);
+                else if (segDirections[i] == Direction.UP)    v = EN.sum(v,segment);
+                else if (segDirections[i] == Direction.LEFT)  h = EN.diff(h,segment);
+                else if (segDirections[i] == Direction.DOWN)  v = EN.diff(v,segment);
+                else throw new IllegalStateException();
+                /**
+                 * When summing up the length of the sum vector should increase.
+                 * However, it is possible to construct ways, such that this assertion fails.
+                 * So only uncomment this for testing
+                 **/
+//                if (segDirections[i].ordinal() % 2 == 0) {
+//                    if (EN.abs(h) < lh) throw new AssertionError();
+//                    lh = EN.abs(h);
+//                } else {
+//                    if (EN.abs(v) < lv) throw new AssertionError();
+//                    lv = EN.abs(v);
+//                }
+            }
+            // rotate the vertical vector by 90 degrees (clockwise) and add it to the horizontal vector
+            segSum = EN.sum(h, new EastNorth(v.north(), - v.east()));
+//            if (EN.abs(segSum) < lh) throw new AssertionError();
+            this.heading = EN.polar(new EastNorth(0.,0.), segSum);
+        }
+    }
+
+    private enum Direction {
+        RIGHT, UP, LEFT, DOWN;
+        public Direction changeBy(int directionChange) {
+            int tmp = (this.ordinal() + directionChange) % 4;
+            if (tmp < 0) tmp += 4;          // the % operator can return negative value
+            return Direction.values()[tmp];
+        }
+    }
+
+    /**
+     * Make sure angle (up to 2*Pi) is in interval [ 0, 2*Pi ).
+     */
+    private static double standard_angle_0_to_2PI(double a) {
+        while (a >= 2 * Math.PI) a -= 2 * Math.PI;
+        while (a < 0)            a += 2 * Math.PI;
+        return a;
+    }
+
+    /**
+     * Make sure angle (up to 2*Pi) is in interval ( -Pi, Pi ].
+     */
+    private static double standard_angle_mPI_to_PI(double a) {
+        while (a > Math.PI)    a -= 2 * Math.PI;
+        while (a <= - Math.PI) a += 2 * Math.PI;
+        return a;
+    }
+
+    /**
+     * Class contains some auxiliary functions
+     */
+    private static class EN {
+        // rotate counter-clock-wise
+        public static EastNorth rotate_cc(EastNorth pivot, EastNorth en, double angle) {
+            double cosPhi = Math.cos(angle);
+            double sinPhi = Math.sin(angle);
+            double x = en.east() - pivot.east();
+            double y = en.north() - pivot.north();
+            double nx =  cosPhi * x - sinPhi * y + pivot.east();
+            double ny =  sinPhi * x + cosPhi * y + pivot.north();
+            return new EastNorth(nx, ny);
+        }
+        public static EastNorth sum(EastNorth en1, EastNorth en2) {
+            return new EastNorth(en1.east() + en2.east(), en1.north() + en2.north());
+        }
+        public static EastNorth diff(EastNorth en1, EastNorth en2) {
+            return new EastNorth(en1.east() - en2.east(), en1.north() - en2.north());
+        }
+        public static double abs(EastNorth en) {
+            return Math.sqrt(en.east() * en.east() + en.north() * en.north());
+        }
+        public static String toString(EastNorth en) {
+            return "["+u(en.east())+","+u(en.north())+"]";
+        }
+        public static long u(double d) {
+            return Math.round(d * 1000000.);
+        }
+        public static double polar(EastNorth en1, EastNorth en2) {
+            return Math.atan2(en2.north() - en1.north(), en2.east() -  en1.east());
+        }
+    }
+
+    /**
+     * Recognize angle to be approximately 0, 90, 180 or 270 degrees.
+     * returns a integral value, corresponding to a counter clockwise turn:
+     */
+    private static int angleToDirectionChange(double a, double deltaMax) throws RejectedAngleException {
+        a = standard_angle_mPI_to_PI(a);
+        double d0    = Math.abs(a);
+        double d90   = Math.abs(a - Math.PI / 2);
+        double d_m90 = Math.abs(a + Math.PI / 2);
+        int dirChange;
+        if (d0 < deltaMax)         dirChange =  0;
+        else if (d90 < deltaMax)   dirChange =  1;
+        else if (d_m90 < deltaMax) dirChange = -1;
+        else {
+            a = standard_angle_0_to_2PI(a);
+            double d180 = Math.abs(a - Math.PI);
+            if (d180 < deltaMax)   dirChange = 2;
+            else {
+                throw new RejectedAngleException();
+            }
+        }
+        return dirChange;
+    }
+
+    /**
+     * Exception: unsuited user input
+     */
+    private static class InvalidUserInputException extends Exception {       
+        InvalidUserInputException(String message) {
+            super(message);
+        }
+        InvalidUserInputException() {
+            super();
+        }
+    }
+    /**
+     * Exception: angle cannot be recognized as 0, 90, 180 or 270 degrees
+     */
+    private static class RejectedAngleException extends Exception {
+        RejectedAngleException() {
+            super();
+        }
+    }
+
+    /**
+     * Don't check, if the current selection is suited for orthogonalization.
+     * Instead, show a usage dialog, that explains, why it cannot be done.
+     */
     @Override
     protected void updateEnabledState() {
-        if (getCurrentDataSet() == null) {
-            setEnabled(false);
-        } else {
-            updateEnabledState(getCurrentDataSet().getSelected());
-        }
-    }
-
-    @Override
-    protected void updateEnabledState(Collection<? extends OsmPrimitive> selection) {
-        setEnabled(selection != null && !selection.isEmpty());
+        setEnabled(getCurrentDataSet() != null);
     }
 }

trunk/src/org/openstreetmap/josm/gui/MainMenu.java

@@ -134 +134 @@
     public final JosmAction alignInLine = new AlignInLineAction();
     public final JosmAction distribute = new DistributeAction();
-    public final JosmAction ortho = new OrthogonalizeAction();
+    public final OrthogonalizeAction ortho = new OrthogonalizeAction();
+    public final JosmAction orthoUndo = ortho.new Undo();  // action is not shown in the menu. Only triggered by shortcut
     public final JosmAction mirror = new MirrorAction();
     public final AddNodeAction addnode = new AddNodeAction();