Seguir una ruta de un GPS

public class Pricipal {
public static void main(String []args){

        Vector<Integer> coordenadas = new Vector();
        coordenadas.add(400);
        coordenadas.add(100);
        coordenadas.add(450);
        coordenadas.add(150);
        coordenadas.add(200);
        coordenadas.add(200);
        coordenadas.add(250);
        coordenadas.add(250);
        coordenadas.add(350);
        coordenadas.add(350);
        coordenadas.add(400);
        coordenadas.add(400);
       
       
        SeguirRuta ruta = new SeguirRuta(coordenadas);
        ruta.printAngulos();
        ruta.printRecorrido();
 }
}

public class SeguirRuta{
       
        public final double sigueRecto = Math.PI/6;
       
        public final int kDIRECCIONNORTE = 0;
        public final int kDIRECCIONSUR = 1;
        public final int kDIRECCIONESTE = 2;
        public final int kDIRECCIONOESTE = 3;
        public final int kINDEFINIDO = -1;
       
        public final int kPREDOMINAVERTICAL = 1;
        public final int kPREDOMINAHORIZONTAL = 2;
       
        public final String kGIRAIZQUIERDA = "Gira a la izquierda";
        public final String kGIRADERECHA = "Gira a la derecha";
        public final String kSIGUERECTO = "Siga recto hasta nueva orden";
       
       
        public Vector vangulos;
        public Vector acciones;
       

        public  SeguirRuta(Vector<Integer> coordenadas){
                int x1,y1,x2,y2, direccionVertical, direccionHorizontal, direccionPredomina;
                double m;
                vangulos = new Vector();
                acciones = new Vector();
               
                for (int i=0;i<coordenadas.size()-2;i=i+2){
                        x1 = ((Integer)coordenadas.elementAt(i)).intValue();
                        y1 = ((Integer)coordenadas.elementAt(i+1)).intValue();
                        x2 = ((Integer)coordenadas.elementAt(i+2)).intValue();
                        y2 = ((Integer)coordenadas.elementAt(i+3)).intValue();                                                         
                       
                        m = calculaPendiente(x1,y1,x2,y2);
                        direccionVertical = calculaDireccionVertical(y1,y2);
                        direccionHorizontal = calculaDireccionHorizontal(x1,x2);
                        direccionPredomina = calculaDireccionPredominante(x1,y1,x2,y2);
                       
                        vangulos.add(new RectaRuta(m, direccionVertical, direccionHorizontal, direccionPredomina));
                }
               
                analizaRuta();
        }

       
        public double calculaPendiente(int x1, int y1, int x2, int y2){
                if ((x1==x2) && (y1==y2)){
                        return calculaPendiente(x1,y1,x2+1,y2);
                }
                else{
                        if (x1==x2)//la recta es perpendicular
                                return Math.PI/2;
                        else{
                                if (y1==y2)//la recta es horizontal
                                        return 0.0;
                                else
                                        return (Math.atan(((double)y1-(double)y2)/((double)x1-(double)x2)));
                        }
                }
        }
       
        public int calculaDireccionVertical(int y1, int y2){
                if (y1==y2)
                        return kINDEFINIDO;
                else
                        if (y1<y2)
                                return kDIRECCIONSUR;
                        else
                                return kDIRECCIONNORTE;
        }

        public int calculaDireccionHorizontal(int x1, int x2){
                if (x1==x2)
                        return kINDEFINIDO;
                else
                        if (x1<x2)
                                return kDIRECCIONESTE;
                        else
                                return kDIRECCIONOESTE;
        }      
       
         public int calculaDireccionPredominante(int x1, int y1, int x2, int y2){
                int diferenciaVertical, diferenciaHorizontal;
               
                diferenciaVertical = Math.abs(y1-y2);
                diferenciaHorizontal = Math.abs(x1-x2);
               
                if (diferenciaVertical==diferenciaHorizontal)
                        return kINDEFINIDO;
                else
                        if (diferenciaVertical>diferenciaHorizontal)
                                return kPREDOMINAVERTICAL;
                        else
                                return kPREDOMINAHORIZONTAL;
         }
       
        public void analizaRuta(){
                double angulo1, angulo2, diferencia;
                boolean sigoRectoAnterior = false;
                RectaRuta rectaRutaAux1, rectaRutaAux2;
               
                for (int i=0;i<vangulos.size()-1;i++){
                        rectaRutaAux1 = (RectaRuta)(vangulos.elementAt(i));
                        rectaRutaAux2 = (RectaRuta)(vangulos.elementAt(i+1));
                       
                        angulo1 = rectaRutaAux1.getPendiente();
                        angulo2 = rectaRutaAux2.getPendiente();
                        diferencia = angulo1-angulo2;
                       
                        if (Math.abs(diferencia)<sigueRecto){
                                acciones.add(new String(kSIGUERECTO));
                        }      
                        else{
                                sigoRectoAnterior = false;
                                if (rectaRutaAux1.getDireccionPredominante()==kPREDOMINAVERTICAL){
                                        if (rectaRutaAux1.getDireccionVertical()==kDIRECCIONNORTE){
                                                if (rectaRutaAux2.getDireccionHorizontal()==kDIRECCIONESTE){
                                                        acciones.add(new String(kGIRADERECHA));
                                                }
                                                else{
                                                        acciones.add(new String(kGIRAIZQUIERDA));
                                                }
                                        }
                                        else{
                                                if (rectaRutaAux2.getDireccionHorizontal()==kDIRECCIONESTE){
                                                        acciones.add(new String(kGIRAIZQUIERDA));
                                                }
                                                else{
                                                        acciones.add(new String(kGIRADERECHA));
                                                }
                                        }
                                }
                                else{//Predomina la horizontal
                                        if (rectaRutaAux1.getDireccionHorizontal()==kDIRECCIONESTE){
                                                if (rectaRutaAux2.getDireccionVertical()==kDIRECCIONNORTE){
                                                        acciones.add(new String(kGIRAIZQUIERDA));
                                                }
                                                else{
                                                        acciones.add(new String(kGIRADERECHA));
                                                }
                                        }
                                        else{
                                                if (rectaRutaAux2.getDireccionVertical()==kDIRECCIONNORTE){
                                                        acciones.add(new String(kGIRADERECHA));
                                                }
                                                else{
                                                        acciones.add(new String(kGIRAIZQUIERDA));                                              
                                                }
                                        }      
                                }      
                        }
                }
        }

        public void printAngulos(){
                int i;
                RectaRuta rectaRutaAux;
               
                System.out.println("RectasRuta=> (");
                for (i=0;i<vangulos.size();i++){
                        rectaRutaAux = (RectaRuta)(vangulos.elementAt(i));
                        System.out.println("Recta["+(i+1)+"]");
                        System.out.println("Pendiente = "+rectaRutaAux.getPendiente());
                        System.out.println("DirecciÛn vertical = "+rectaRutaAux.getDireccionVertical());
                        System.out.println("Direccion horizontal = "+rectaRutaAux.getDireccionHorizontal());
                        System.out.println("Direccion predominante = "+rectaRutaAux.getDireccionPredominante());
                }
        }

        public void printRecorrido(){
                String salida = new String();
               
                for (int i=0;i<acciones.size();i++){
                        System.out.println(""+(String)(acciones.elementAt(i))+"\n");   
                }
        }
       
        public String accionNodo(int nodo){
                return (String)(acciones.elementAt(nodo));
        }

public class RectaRuta{
        public double pendiente;
        public int direccionVertical;
        public int direccionHorizontal;
        public int direccionPredominante;
       
        public RectaRuta(double m, int dv, int dh, int dp){
                pendiente = m;
                direccionVertical = dv;
                direccionHorizontal = dh;
                direccionPredominante = dp;
        }
       
        public double getPendiente(){
                return pendiente;
        }
       
        public int getDireccionVertical(){
                return direccionVertical;
        }
       
        public int getDireccionHorizontal(){
                return direccionHorizontal;    
        }

        public int getDireccionPredominante(){
                return direccionPredominante;
        }      
       
        public void setPendiente(double m){
                pendiente = m;
        }

        public void setDireccionVertical(int dv){
                direccionVertical = dv;
        }
       

        public void setDireccionHorizontal(int dh){
                direccionHorizontal = dh;
        }

        public void setDireccionPredominante(int dp){
                direccionPredominante = dp;
        }      
}