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

    }

}