Subversion Repositories Projects

/*********************************************
 *                                            
 * class representing the DebugData Structure
 *                                            
 * Author:        Marcus -LiGi- Bueschleb    
 *
 * see README for further Infos
 *
 * Some code taken from here:
 * http://www.koders.com/java/fidFC75A641A87B51BB757E9CD3136C7886C491487F.aspx
 *
 * and
 * http://www.movable-type.co.uk/scripts/latlong.html
 *
 * thanx a lot for sharing!
 *
 ********************************************/



import java.lang.Math;
public class MKGPSPosition
{
    public final byte GPS_FORMAT_DECIMAL=0;
    public final byte GPS_FORMAT_MINSEC=1;
    public final byte GPS_FORMAT_COUNT=2;
    byte act_gps_format=GPS_FORMAT_DECIMAL;

    public final static int MAX_WAYPOINTS=100;

    int[] LongWP;
    int[] LatWP;
    String[] NameWP;
   
    int last_wp=0;


    int Longitude;
    int Latitude;

    int TargetLongitude;
    int TargetLatitude;

    int Distance2Target;
    int Angle2Target;

    byte Used_Sat;
  /**
   * Holds value Math.PI.
   */
  public static final double PI = Math.PI;
  /**
   * Holds value PI / 2.0.
   */
  public static final double PI_div2 = PI / 2.0;
  /**
   * Holds value PI / 4.0.
   */
  public static final double PI_div4 = PI / 4.0;


 public static final double RADIANS = PI / 180.0;
 public static final double DEGREES = 180.0 / PI;


  private static final double p4 = 0.161536412982230228262e2;
  /**
   * Holds value 0.26842548195503973794141e3.
   */
  private static final double p3 = 0.26842548195503973794141e3;
  /**
   * Holds value 0.11530293515404850115428136e4.
   */
  private static final double p2 = 0.11530293515404850115428136e4;
  /**
   * Holds value 0.178040631643319697105464587e4.
   */
  private static final double p1 = 0.178040631643319697105464587e4;
  /**
   * Holds value 0.89678597403663861959987488e3.
   */
  private static final double p0 = 0.89678597403663861959987488e3;
  /**
   * Holds value 0.5895697050844462222791e2.
   */
  private static final double q4 = 0.5895697050844462222791e2;
  /**
   * Holds value 0.536265374031215315104235e3.
   */
  private static final double q3 = 0.536265374031215315104235e3;
  /**
   * Holds value 0.16667838148816337184521798e4.
   */
  private static final double q2 = 0.16667838148816337184521798e4;
  /**
   * Holds value 0.207933497444540981287275926e4.
   */
  private static final double q1 = 0.207933497444540981287275926e4;
  /**
   * Holds value 0.89678597403663861962481162e3.
   */
  private static final double q0 = 0.89678597403663861962481162e3;





  private static double _ATAN(double X) {
    if (X < 0.414213562373095048802) { /* tan(PI/8) */
      return _ATANX(X);
    }
    else if (X > 2.414213562373095048802) { /* tan(3*PI/8) */
      return PI_div2 - _ATANX(1.0 / X);
    }
    else {
      return PI_div4 + _ATANX((X - 1.0) / (X + 1.0));
    }
  }


  private static double _ATANX(double X) {
    double XX = X * X;
    return X * ((((p4 * XX + p3) * XX + p2) * XX + p1) * XX + p0)
        / (((((XX + q4) * XX + q3) * XX + q2) * XX + q1) * XX + q0);
  }



      public double aTan2(double Y, double X) {

          //private static double ATAN2(double Y, double X) {
    // return Math.atan2(Y,X);  // not in CLDC 1.1

    // X=0
    if (X == 0.0) {
      if (Y > 0.0) {
        // mid Q1/Q2
        return PI_div2;
      }
      else if (Y < 0.0) {
        // mid Q3/Q4
        return -PI_div2;
      }
      else {
        // undefined
        return 0.0;
      }
    }

    // X<0
    if (X < 0.0) {
      if (Y >= 0.0) {
        // Q2
        return (PI - _ATAN(Y / -X)); // Y>=0,X<0 |Y/X|
      }
      else {
        // Q3
        return -(PI - _ATAN(Y / X)); // Y<0,X<0 |Y/X|
      }
    }

    // X>0
    if (X > 0.0) {
      // Q1/A4
      //return  ATAN( Y / X);
      if (Y > 0.0) {
        // Q1
        return _ATAN(Y / X);
      }
      else {
        // Q4
        return -_ATAN(-Y / X);
      }
    }

    /* will never reach here */
    return 0.0;

  }
/*



        double coeff_1 = Math.PI / 4d;
        double coeff_2 = 3d * coeff_1;
        double abs_y = Math.abs(y);
        double angle;
        if (x >= 0d) {
                double r = (x - abs_y) / (x + abs_y);
                angle = coeff_1 - coeff_1 * r;
        } else {
                double r = (x + abs_y) / (abs_y - x);
                angle = coeff_2 - coeff_1 * r;
        }
        return y < 0d ? -angle : angle;
    }

*/
    public int distance2wp(int id)
    {
        double lat1=(Latitude/10000000.0)*RADIANS;
        double long1=(Longitude/10000000.0)*RADIANS;

        double lat2=(LatWP[id]/10000000.0)*RADIANS;
        double long2=(LongWP[id]/10000000.0)*RADIANS;


        double dLat= (lat2-lat1);
        double dLon= (long2-long1);

        double a = Math.sin(dLat/2.0) * Math.sin(dLat/2.0) +
        Math.cos(lat1) * Math.cos(lat2) *
        Math.sin(dLon/2.0) * Math.sin(dLon/2.0);

        return (int)(( 2.0 * aTan2(Math.sqrt(a), Math.sqrt(1.0-a)) )*6371008.8);
    }




    public int angle2wp(int id)
    {
        // TODO reuse from distance
        double lat1=(Latitude/10000000.0)*RADIANS;
        double long1=(Longitude/10000000.0)*RADIANS;

        double lat2=(LatWP[id]/10000000.0)*RADIANS;
        double long2=(LongWP[id]/10000000.0)*RADIANS;


        double dLat= (lat2-lat1);
        double dLon= (long2-long1);

       

        double y = Math.sin(dLon) * Math.cos(lat2);
        double x = Math.cos(lat1)*Math.sin(lat2) -   Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
        return ((int)(aTan2(y, x)*DEGREES)+360)%360;

    }



    public void push_wp()
    {
        LongWP[last_wp]=Longitude;
        LatWP[last_wp]=Latitude;

        last_wp++;
    }

    public void next_gps_format()
    {
        act_gps_format=(byte)((act_gps_format+1)%GPS_FORMAT_COUNT);
    }

    public String act_gps_format_str(int val)
    {
        switch(act_gps_format)
            {
            case GPS_FORMAT_DECIMAL:
                return "" + val/10000000 + "." +val%10000000  ;
            case GPS_FORMAT_MINSEC:
                return "" +  val/10000000 + "^" +  ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 +  "." + ((((val%10000000)*60)%10000000)*60)%10000000;
            default:
                return "invalid format";
            }
    }

    public String WP_Latitude_str(int id)
    {
       
        return "" +  act_gps_format_str(LatWP[id]); //+ "''N"  ;
    }

    public String WP_Longitude_str(int id)
    {
        return "" +  act_gps_format_str(LongWP[id]); //+ "''E"  ;

    }

    public String Latitude_str()
    {
       
        return "" +  act_gps_format_str(Latitude) ;
    }

    public String Longitude_str()
    {
        return "" +  act_gps_format_str(Longitude)  ;

    }


    // Constructor
    public MKGPSPosition()
    {

        LongWP=new int[MAX_WAYPOINTS];
        LatWP=new int[MAX_WAYPOINTS];
       

        NameWP=new String[MAX_WAYPOINTS];
        // predefined waypoints


        LongWP[0]=123230170;
        LatWP[0]= 513600170 ;
        NameWP[0]="Sicherer PC1";

        LongWP[1]=123269000;
        LatWP[1]= 513662670;
        NameWP[1]="Sicherer PC2";

        LongWP[2]=123475570;
        LatWP[2]= 513569750 ;
        NameWP[2]="Treffpunkt Seba";


        last_wp=2;
    }

    private int parse_arr(int offset,int[] in_arr)
    {
        return ((in_arr[offset+3]<<24) |
                (in_arr[offset+2]<<16) |
                (in_arr[offset+1]<<8)  |
                (in_arr[offset+0]));
    }

    public void set_by_mk_data(int[] in_arr,MKVersion version)
    {
        Longitude=parse_arr(0,in_arr);
        Latitude=parse_arr(4,in_arr);
        TargetLongitude=parse_arr(8,in_arr);
        TargetLatitude=parse_arr(12,in_arr);
        Distance2Target=parse_arr(16,in_arr);
        Angle2Target=parse_arr(20,in_arr);
        Used_Sat=(byte)in_arr[24];

    }
}