Seguir una ruta de un GPS

Hola a todos he estado desarrollando un proyecto que usa un gps el cual tiene que moverse ala izquierda, derecha o avanzar derecho dependiendo de 2 puntos en x e y. (x1,y1 y x2,y2) si alguien tiene algunas mejoras son bienvenidas.

Principal.java

import java.util.Vector;

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();
 }
}

SeguirRuta.java

import java.util.*;
import java.lang.*;
import java.io.*;

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));
        }

RectaRuta.java

import java.util.*;
import java.lang.*;
import java.io.*;

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;
        }      
}

Saludos.

Comentarios

Opciones de visualización de comentarios

Seleccione la forma que prefiera para mostrar los comentarios y haga clic en «Guardar las opciones» para activar los cambios.
Imagen de arterzatij

Bueno

Yo agregaria una clase punto para mejorar la legibilidad... recordemos que hay que manejar objetos.

public class Punto {
       
        private int x, y;
       
       //Contructor con campos
   
       //Getters y Setters

       //Override de equals

}