WebSVN - Projects - Diff - Rev 135 and 149 - /DUBwise/src/J2ME/src/MKGPSPosition.java

 /*********************************************
+ *
  * 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!
+ *
  ********************************************/
-public class MKGPSPosition
+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;
-    public void push_wp()
-    {
-        LongWP[last_wp]=Longitude;
-        LatWP[last_wp]=Latitude;
+  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;
+        }
-        last_wp++;
+        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) *
-    public String dec_to_min_sec(int val)
+        Math.sin(dLon/2.0) * Math.sin(dLon/2.0);
-        return "" +  val/10000000 + "^" +  ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 +  "." + ((((val%10000000)*60)%10000000)*60)%10000000;
+        return (int)(( 2.0 * aTan2(Math.sqrt(a), Math.sqrt(1.0-a)) )*6371008.8);
+    }
-    public String WP_Latitude_min_sec(int id)
+    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;
+    }
-        return "" +  dec_to_min_sec(LatWP[id])+ "''N"  ;
+    public void push_wp()
+    {
-    public String WP_Longitude_min_sec(int id)
+        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 "" +  dec_to_min_sec(LongWP[id])+ "''E"  ;
+                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 Latitude_min_sec()
+    public String WP_Latitude_str(int id)
+    {
-        return "" +  dec_to_min_sec(Latitude)+ "''N"  ;
+        return "" +  act_gps_format_str(LatWP[id]); //+ "''N"  ;
+    }
-    public String Longitude_min_sec()
+    public String WP_Longitude_str(int id)
+    {
-        return "" +  dec_to_min_sec(Longitude)+ "''E"  ;
+        return "" +  act_gps_format_str(LongWP[id]); //+ "''E"  ;
+    }
     public String Latitude_str()
+    {
-        return "" + Latitude/10000000 + "." +Latitude%10000000  ;
+        return "" +  act_gps_format_str(Latitude) ;
+    }
     public String Longitude_str()
+    {
-        return "" + Longitude/10000000 + "." +Longitude%10000000  ;
+        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];
+    }
+}

Subversion Repositories Projects

(root)/DUBwise/src/J2ME/src/MKGPSPosition.java @ 149 - Rev 135 → 149