Ticket #1621: AlignOrthogonallyAction.java

// License: GPL. See LICENSE file for details.
//
package org.openstreetmap.josm.actions;

import static org.openstreetmap.josm.tools.I18n.tr;

import java.awt.List;
import java.awt.event.ActionEvent;
import java.awt.event.KeyEvent;
import java.util.ArrayList;
import java.util.Collection;
import java.util.LinkedList;

import javax.swing.JOptionPane;

import org.openstreetmap.josm.Main;
import org.openstreetmap.josm.command.AddCommand;
import org.openstreetmap.josm.command.Command;
import org.openstreetmap.josm.command.MoveCommand;
import org.openstreetmap.josm.command.SequenceCommand;
import org.openstreetmap.josm.data.coor.EastNorth;
import org.openstreetmap.josm.data.osm.Node;
import org.openstreetmap.josm.data.osm.OsmPrimitive;
import org.openstreetmap.josm.data.osm.Way;
import org.openstreetmap.josm.tools.ShortCut;

/**
 * Align edges of a way so all angles are right angles. 
 * 
 * 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
 */
public final class AlignOrthogonallyAction extends JosmAction {

        public AlignOrthogonallyAction() {
        super(tr("Align Nodes to make shape orthogonally"), "alignortho", tr("Move the selected nodes so all angles are orthogonally."),
                ShortCut.registerShortCut("tools:alignortho", tr("Tool: {0}", tr("Align orthonormal")), KeyEvent.VK_T, ShortCut.GROUP_EDIT), true);
        }

        public void actionPerformed(ActionEvent e) {
        
                Collection<OsmPrimitive> sel = Main.ds.getSelected();
        
        ArrayList<Node> dirnodes = new ArrayList<Node>();
                
        // Check the selection if it is suitible for the orthogonalization
                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"));
                    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."));
                        return;
            } 
            
            // Check if every way is made of at least four segments and closed
            Way way = (Way)osm;
            if ((way.nodes.size() < 5) || (!way.nodes.get(0).equals(way.nodes.get(way.nodes.size() - 1)))) {
                JOptionPane.showMessageDialog(Main.parent, tr("Please select closed way(s) of at least four nodes."));
                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.nodes.size()-1; i1++) {    
               int i2 = (i1+1) % (way.nodes.size()-1);
               int i3 = (i1+2) % (way.nodes.size()-1);
               double angle1  =Math.abs(way.nodes.get(i1).eastNorth.heading(way.nodes.get(i2).eastNorth));
               double angle2 = Math.abs(way.nodes.get(i2).eastNorth.heading(way.nodes.get(i3).eastNorth));
               double delta = Math.abs(angle2 - angle1);
               while(delta > Math.PI) delta -= Math.PI;
               if(delta < Math.PI/4) {
                   JOptionPane.showMessageDialog(Main.parent, tr("Please select ways with edges close to right angles."));
                   return;
               }
            }
        }
        // Check, if selection held neither none nor two nodes
        if(dirnodes.size() == 1) {
            JOptionPane.showMessageDialog(Main.parent, tr("Only one node selected"));
            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;
        
        
        if(dirnodes.size() == 2) { // When selection contained two nodes, use the nodes to compute a direction to align to
            double heading;        
            heading = dirnodes.get(0).eastNorth.heading(dirnodes.get(1).eastNorth);
            while(heading > Math.PI/4) heading -= Math.PI/2;
            align_to_heading=heading;
        } else {   // Otherwise compute the alignment direction from the ways in the collection
            // First, compute the weighted average of the headings of all segments
            double sum_weighted_headings = 0.0;
            double sum_weights = 0.0;
            for (OsmPrimitive osm : sel) {
                if(!(osm instanceof Way)) 
                    continue;
                Way way = (Way)osm;
                int nodes = way.nodes.size();
                        int sides = nodes - 1;            
                        // To find orientation of all segments, compute weighted average of all segment's headings
                // all headings are mapped into [0, 3*4*PI) 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.
                        for (int i=0; i < sides; i++) {
                    double heading;        
                    double weight;
                    heading = way.nodes.get(i).eastNorth.heading(way.nodes.get(i+1).eastNorth);
                    //Put into [0, PI/4) to find main direction
                    while(heading > Math.PI/4) heading -= Math.PI/2;
                    weight = way.nodes.get(i).eastNorth.distance(way.nodes.get(i+1).eastNorth);
                    sum_weighted_headings += heading*weight;
                                sum_weights += weight;
                }
             }
            align_to_heading = sum_weighted_headings/sum_weights;         
        }
        
        for (OsmPrimitive osm : sel) {  
            if(!(osm instanceof Way)) 
                continue;
            Way myWay = (Way)osm;
            int nodes = myWay.nodes.size();
            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(myWay.nodes.get(i).eastNorth.east(), myWay.nodes.get(i).eastNorth.north());
            }
 
            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 = en[i1].heading(en[i2]);
                // Put into [-PI/4, PI/4) because we want a minimum of rotation so we don't swap node positions
                while(heading1 - align_to_heading > Math.PI/4) heading1 -= Math.PI/2;
                while(heading1 - align_to_heading < -Math.PI/4) heading1 += Math.PI/2;
                delta1 = align_to_heading - heading1;
                // Compute neccessary rotation of second segment to align it with main orientation
                heading2 = en[i2].heading(en[i3]);
                // Put into [-PI/4, PI/4) because we want a minimum of rotation so we don't swap node positions
                while(heading2 - align_to_heading > Math.PI/4) heading2 -= Math.PI/2;
                while(heading2 - align_to_heading < -Math.PI/4) heading2 += Math.PI/2;
                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 lenght 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 = myWay.nodes.get(i2);
                double dx = intersection.east()-n.eastNorth.east();
                double dy = intersection.north()-n.eastNorth.north();
                cmds.add(new MoveCommand(n, dx, dy));        
            }      
        }
        
                Main.main.undoRedo.add(new SequenceCommand(tr("Align Segments orthogonally"), cmds));
                Main.map.repaint();
        }
    
    static double det(double a, double b, double c, double d)
    {
        return a * d - b * c;
    }

}