Rev 135 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 135 | Rev 149 | ||
---|---|---|---|
1 | /********************************************* |
1 | /********************************************* |
2 | * |
2 | * |
3 | * class representing the DebugData Structure |
3 | * class representing the DebugData Structure |
4 | * |
4 | * |
5 | * Author: Marcus -LiGi- Bueschleb |
5 | * Author: Marcus -LiGi- Bueschleb |
6 | * |
6 | * |
7 | * see README for further Infos |
7 | * see README for further Infos |
8 | * |
8 | * |
- | 9 | * Some code taken from here: |
|
- | 10 | * http://www.koders.com/java/fidFC75A641A87B51BB757E9CD3136C7886C491487F.aspx |
|
- | 11 | * |
|
- | 12 | * and |
|
- | 13 | * http://www.movable-type.co.uk/scripts/latlong.html |
|
- | 14 | * |
|
- | 15 | * thanx a lot for sharing! |
|
- | 16 | * |
|
9 | ********************************************/ |
17 | ********************************************/ |
10 | - | ||
- | 18 | ||
- | 19 | ||
- | 20 | ||
11 | public class MKGPSPosition |
21 | import java.lang.Math; |
- | 22 | public class MKGPSPosition |
|
- | 23 | { |
|
- | 24 | public final byte GPS_FORMAT_DECIMAL=0; |
|
- | 25 | public final byte GPS_FORMAT_MINSEC=1; |
|
12 | 26 | public final byte GPS_FORMAT_COUNT=2; |
|
13 | { |
27 | byte act_gps_format=GPS_FORMAT_DECIMAL; |
14 | 28 | ||
15 | public final static int MAX_WAYPOINTS=100; |
29 | public final static int MAX_WAYPOINTS=100; |
16 | 30 | ||
17 | int[] LongWP; |
31 | int[] LongWP; |
18 | int[] LatWP; |
32 | int[] LatWP; |
- | 33 | String[] NameWP; |
|
19 | 34 | ||
20 | int last_wp=0; |
35 | int last_wp=0; |
21 | 36 | ||
22 | 37 | ||
23 | int Longitude; |
38 | int Longitude; |
24 | int Latitude; |
39 | int Latitude; |
25 | 40 | ||
26 | int TargetLongitude; |
41 | int TargetLongitude; |
27 | int TargetLatitude; |
42 | int TargetLatitude; |
28 | 43 | ||
29 | int Distance2Target; |
44 | int Distance2Target; |
30 | int Angle2Target; |
45 | int Angle2Target; |
31 | 46 | ||
32 | byte Used_Sat; |
47 | byte Used_Sat; |
- | 48 | /** |
|
- | 49 | * Holds value Math.PI. |
|
- | 50 | */ |
|
- | 51 | public static final double PI = Math.PI; |
|
- | 52 | /** |
|
- | 53 | * Holds value PI / 2.0. |
|
- | 54 | */ |
|
- | 55 | public static final double PI_div2 = PI / 2.0; |
|
- | 56 | /** |
|
- | 57 | * Holds value PI / 4.0. |
|
- | 58 | */ |
|
- | 59 | public static final double PI_div4 = PI / 4.0; |
|
- | 60 | ||
- | 61 | ||
- | 62 | public static final double RADIANS = PI / 180.0; |
|
- | 63 | public static final double DEGREES = 180.0 / PI; |
|
- | 64 | ||
- | 65 | ||
- | 66 | private static final double p4 = 0.161536412982230228262e2; |
|
- | 67 | /** |
|
- | 68 | * Holds value 0.26842548195503973794141e3. |
|
- | 69 | */ |
|
- | 70 | private static final double p3 = 0.26842548195503973794141e3; |
|
- | 71 | /** |
|
- | 72 | * Holds value 0.11530293515404850115428136e4. |
|
- | 73 | */ |
|
- | 74 | private static final double p2 = 0.11530293515404850115428136e4; |
|
- | 75 | /** |
|
- | 76 | * Holds value 0.178040631643319697105464587e4. |
|
- | 77 | */ |
|
- | 78 | private static final double p1 = 0.178040631643319697105464587e4; |
|
- | 79 | /** |
|
- | 80 | * Holds value 0.89678597403663861959987488e3. |
|
- | 81 | */ |
|
- | 82 | private static final double p0 = 0.89678597403663861959987488e3; |
|
- | 83 | /** |
|
- | 84 | * Holds value 0.5895697050844462222791e2. |
|
- | 85 | */ |
|
- | 86 | private static final double q4 = 0.5895697050844462222791e2; |
|
- | 87 | /** |
|
- | 88 | * Holds value 0.536265374031215315104235e3. |
|
- | 89 | */ |
|
- | 90 | private static final double q3 = 0.536265374031215315104235e3; |
|
- | 91 | /** |
|
- | 92 | * Holds value 0.16667838148816337184521798e4. |
|
- | 93 | */ |
|
- | 94 | private static final double q2 = 0.16667838148816337184521798e4; |
|
- | 95 | /** |
|
- | 96 | * Holds value 0.207933497444540981287275926e4. |
|
- | 97 | */ |
|
- | 98 | private static final double q1 = 0.207933497444540981287275926e4; |
|
- | 99 | /** |
|
- | 100 | * Holds value 0.89678597403663861962481162e3. |
|
- | 101 | */ |
|
- | 102 | private static final double q0 = 0.89678597403663861962481162e3; |
|
33 | - | ||
34 | - | ||
35 | public void push_wp() |
- | |
36 | { |
- | |
37 | LongWP[last_wp]=Longitude; |
- | |
38 | LatWP[last_wp]=Latitude; |
- | |
- | 103 | ||
- | 104 | ||
- | 105 | ||
- | 106 | ||
- | 107 | ||
- | 108 | private static double _ATAN(double X) { |
|
- | 109 | if (X < 0.414213562373095048802) { /* tan(PI/8) */ |
|
- | 110 | return _ATANX(X); |
|
- | 111 | } |
|
- | 112 | else if (X > 2.414213562373095048802) { /* tan(3*PI/8) */ |
|
- | 113 | return PI_div2 - _ATANX(1.0 / X); |
|
- | 114 | } |
|
- | 115 | else { |
|
- | 116 | return PI_div4 + _ATANX((X - 1.0) / (X + 1.0)); |
|
- | 117 | } |
|
- | 118 | } |
|
- | 119 | ||
- | 120 | ||
- | 121 | private static double _ATANX(double X) { |
|
- | 122 | double XX = X * X; |
|
- | 123 | return X * ((((p4 * XX + p3) * XX + p2) * XX + p1) * XX + p0) |
|
- | 124 | / (((((XX + q4) * XX + q3) * XX + q2) * XX + q1) * XX + q0); |
|
- | 125 | } |
|
- | 126 | ||
- | 127 | ||
- | 128 | ||
- | 129 | public double aTan2(double Y, double X) { |
|
- | 130 | ||
- | 131 | //private static double ATAN2(double Y, double X) { |
|
- | 132 | // return Math.atan2(Y,X); // not in CLDC 1.1 |
|
- | 133 | ||
- | 134 | // X=0 |
|
- | 135 | if (X == 0.0) { |
|
- | 136 | if (Y > 0.0) { |
|
- | 137 | // mid Q1/Q2 |
|
- | 138 | return PI_div2; |
|
- | 139 | } |
|
- | 140 | else if (Y < 0.0) { |
|
- | 141 | // mid Q3/Q4 |
|
- | 142 | return -PI_div2; |
|
- | 143 | } |
|
- | 144 | else { |
|
- | 145 | // undefined |
|
- | 146 | return 0.0; |
|
- | 147 | } |
|
- | 148 | } |
|
- | 149 | ||
- | 150 | // X<0 |
|
- | 151 | if (X < 0.0) { |
|
- | 152 | if (Y >= 0.0) { |
|
- | 153 | // Q2 |
|
- | 154 | return (PI - _ATAN(Y / -X)); // Y>=0,X<0 |Y/X| |
|
- | 155 | } |
|
- | 156 | else { |
|
- | 157 | // Q3 |
|
- | 158 | return -(PI - _ATAN(Y / X)); // Y<0,X<0 |Y/X| |
|
- | 159 | } |
|
- | 160 | } |
|
- | 161 | ||
- | 162 | // X>0 |
|
- | 163 | if (X > 0.0) { |
|
- | 164 | // Q1/A4 |
|
- | 165 | //return ATAN( Y / X); |
|
- | 166 | if (Y > 0.0) { |
|
- | 167 | // Q1 |
|
- | 168 | return _ATAN(Y / X); |
|
- | 169 | } |
|
- | 170 | else { |
|
- | 171 | // Q4 |
|
- | 172 | return -_ATAN(-Y / X); |
|
- | 173 | } |
|
- | 174 | } |
|
- | 175 | ||
- | 176 | /* will never reach here */ |
|
- | 177 | return 0.0; |
|
- | 178 | ||
- | 179 | } |
|
- | 180 | /* |
|
- | 181 | ||
- | 182 | ||
- | 183 | ||
- | 184 | double coeff_1 = Math.PI / 4d; |
|
- | 185 | double coeff_2 = 3d * coeff_1; |
|
- | 186 | double abs_y = Math.abs(y); |
|
- | 187 | double angle; |
|
- | 188 | if (x >= 0d) { |
|
- | 189 | double r = (x - abs_y) / (x + abs_y); |
|
- | 190 | angle = coeff_1 - coeff_1 * r; |
|
- | 191 | } else { |
|
- | 192 | double r = (x + abs_y) / (abs_y - x); |
|
- | 193 | angle = coeff_2 - coeff_1 * r; |
|
- | 194 | } |
|
39 | last_wp++; |
195 | return y < 0d ? -angle : angle; |
40 | } |
196 | } |
- | 197 | ||
- | 198 | */ |
|
- | 199 | public int distance2wp(int id) |
|
- | 200 | { |
|
- | 201 | double lat1=(Latitude/10000000.0)*RADIANS; |
|
- | 202 | double long1=(Longitude/10000000.0)*RADIANS; |
|
- | 203 | ||
- | 204 | double lat2=(LatWP[id]/10000000.0)*RADIANS; |
|
- | 205 | double long2=(LongWP[id]/10000000.0)*RADIANS; |
|
- | 206 | ||
- | 207 | ||
- | 208 | double dLat= (lat2-lat1); |
|
- | 209 | double dLon= (long2-long1); |
|
- | 210 | ||
41 | 211 | double a = Math.sin(dLat/2.0) * Math.sin(dLat/2.0) + |
|
42 | 212 | Math.cos(lat1) * Math.cos(lat2) * |
|
43 | public String dec_to_min_sec(int val) |
213 | Math.sin(dLon/2.0) * Math.sin(dLon/2.0); |
44 | { |
214 | |
45 | return "" + val/10000000 + "^" + ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 + "." + ((((val%10000000)*60)%10000000)*60)%10000000; |
215 | return (int)(( 2.0 * aTan2(Math.sqrt(a), Math.sqrt(1.0-a)) )*6371008.8); |
46 | } |
216 | } |
47 | 217 | ||
- | 218 | ||
- | 219 | ||
48 | 220 | ||
49 | public String WP_Latitude_min_sec(int id) |
221 | public int angle2wp(int id) |
- | 222 | { |
|
- | 223 | // TODO reuse from distance |
|
- | 224 | double lat1=(Latitude/10000000.0)*RADIANS; |
|
- | 225 | double long1=(Longitude/10000000.0)*RADIANS; |
|
- | 226 | ||
- | 227 | double lat2=(LatWP[id]/10000000.0)*RADIANS; |
|
- | 228 | double long2=(LongWP[id]/10000000.0)*RADIANS; |
|
- | 229 | ||
- | 230 | ||
- | 231 | double dLat= (lat2-lat1); |
|
- | 232 | double dLon= (long2-long1); |
|
- | 233 | ||
- | 234 | ||
- | 235 | ||
50 | { |
236 | double y = Math.sin(dLon) * Math.cos(lat2); |
- | 237 | double x = Math.cos(lat1)*Math.sin(lat2) - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); |
|
51 | 238 | return ((int)(aTan2(y, x)*DEGREES)+360)%360; |
|
- | 239 | ||
- | 240 | } |
|
52 | return "" + dec_to_min_sec(LatWP[id])+ "''N" ; |
241 | |
53 | } |
242 | |
54 | 243 | ||
- | 244 | public void push_wp() |
|
- | 245 | { |
|
55 | public String WP_Longitude_min_sec(int id) |
246 | LongWP[last_wp]=Longitude; |
- | 247 | LatWP[last_wp]=Latitude; |
|
- | 248 | ||
- | 249 | last_wp++; |
|
- | 250 | } |
|
- | 251 | ||
- | 252 | public void next_gps_format() |
|
- | 253 | { |
|
- | 254 | act_gps_format=(byte)((act_gps_format+1)%GPS_FORMAT_COUNT); |
|
- | 255 | } |
|
- | 256 | ||
- | 257 | public String act_gps_format_str(int val) |
|
- | 258 | { |
|
- | 259 | switch(act_gps_format) |
|
- | 260 | { |
|
- | 261 | case GPS_FORMAT_DECIMAL: |
|
- | 262 | return "" + val/10000000 + "." +val%10000000 ; |
|
56 | { |
263 | case GPS_FORMAT_MINSEC: |
57 | return "" + dec_to_min_sec(LongWP[id])+ "''E" ; |
264 | return "" + val/10000000 + "^" + ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 + "." + ((((val%10000000)*60)%10000000)*60)%10000000; |
58 | 265 | default: |
|
59 | } |
266 | return "invalid format"; |
60 | 267 | } |
|
61 | 268 | } |
|
62 | 269 | ||
63 | public String Latitude_min_sec() |
270 | public String WP_Latitude_str(int id) |
64 | { |
271 | { |
65 | 272 | ||
66 | return "" + dec_to_min_sec(Latitude)+ "''N" ; |
273 | return "" + act_gps_format_str(LatWP[id]); //+ "''N" ; |
67 | } |
274 | } |
68 | 275 | ||
69 | public String Longitude_min_sec() |
276 | public String WP_Longitude_str(int id) |
70 | { |
277 | { |
71 | return "" + dec_to_min_sec(Longitude)+ "''E" ; |
278 | return "" + act_gps_format_str(LongWP[id]); //+ "''E" ; |
72 | 279 | ||
73 | } |
280 | } |
74 | 281 | ||
75 | public String Latitude_str() |
282 | public String Latitude_str() |
76 | { |
283 | { |
- | 284 | ||
77 | return "" + Latitude/10000000 + "." +Latitude%10000000 ; |
285 | return "" + act_gps_format_str(Latitude) ; |
78 | } |
286 | } |
79 | 287 | ||
80 | public String Longitude_str() |
288 | public String Longitude_str() |
81 | { |
289 | { |
82 | return "" + Longitude/10000000 + "." +Longitude%10000000 ; |
290 | return "" + act_gps_format_str(Longitude) ; |
- | 291 | ||
83 | } |
292 | } |
- | 293 | ||
- | 294 | ||
84 | 295 | // Constructor |
|
85 | public MKGPSPosition() |
296 | public MKGPSPosition() |
86 | { |
297 | { |
87 | 298 | ||
88 | LongWP=new int[MAX_WAYPOINTS]; |
299 | LongWP=new int[MAX_WAYPOINTS]; |
89 | LatWP=new int[MAX_WAYPOINTS]; |
300 | LatWP=new int[MAX_WAYPOINTS]; |
- | 301 | ||
- | 302 | ||
- | 303 | NameWP=new String[MAX_WAYPOINTS]; |
|
- | 304 | // predefined waypoints |
|
- | 305 | ||
- | 306 | ||
- | 307 | LongWP[0]=123230170; |
|
- | 308 | LatWP[0]= 513600170 ; |
|
- | 309 | NameWP[0]="Sicherer PC1"; |
|
- | 310 | ||
- | 311 | LongWP[1]=123269000; |
|
- | 312 | LatWP[1]= 513662670; |
|
- | 313 | NameWP[1]="Sicherer PC2"; |
|
- | 314 | ||
- | 315 | LongWP[2]=123475570; |
|
- | 316 | LatWP[2]= 513569750 ; |
|
- | 317 | NameWP[2]="Treffpunkt Seba"; |
|
- | 318 | ||
- | 319 | ||
90 | 320 | last_wp=2; |
|
91 | } |
321 | } |
92 | 322 | ||
93 | private int parse_arr(int offset,int[] in_arr) |
323 | private int parse_arr(int offset,int[] in_arr) |
94 | { |
324 | { |
95 | return ((in_arr[offset+3]<<24) | |
325 | return ((in_arr[offset+3]<<24) | |
96 | (in_arr[offset+2]<<16) | |
326 | (in_arr[offset+2]<<16) | |
97 | (in_arr[offset+1]<<8) | |
327 | (in_arr[offset+1]<<8) | |
98 | (in_arr[offset+0])); |
328 | (in_arr[offset+0])); |
99 | } |
329 | } |
100 | 330 | ||
101 | public void set_by_mk_data(int[] in_arr,MKVersion version) |
331 | public void set_by_mk_data(int[] in_arr,MKVersion version) |
102 | { |
332 | { |
103 | Longitude=parse_arr(0,in_arr); |
333 | Longitude=parse_arr(0,in_arr); |
104 | Latitude=parse_arr(4,in_arr); |
334 | Latitude=parse_arr(4,in_arr); |
105 | - | ||
106 | TargetLongitude=parse_arr(8,in_arr); |
335 | TargetLongitude=parse_arr(8,in_arr); |
107 | TargetLatitude=parse_arr(12,in_arr); |
336 | TargetLatitude=parse_arr(12,in_arr); |
108 | - | ||
109 | Distance2Target=parse_arr(16,in_arr); |
337 | Distance2Target=parse_arr(16,in_arr); |
110 | Angle2Target=parse_arr(20,in_arr); |
338 | Angle2Target=parse_arr(20,in_arr); |
111 | Used_Sat=(byte)in_arr[24]; |
339 | Used_Sat=(byte)in_arr[24]; |
112 | 340 | ||
113 | } |
341 | } |
114 | - | ||
115 | - | ||
116 | - | ||
117 | } |
342 | } |
118 | 343 |