Rev 135 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 135 | Rev 149 | ||
---|---|---|---|
Line 4... | Line 4... | ||
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 | ********************************************/ |
Line 10... | Line -... | ||
10 | - | ||
Line -... | Line 18... | ||
- | 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; |
|
Line 12... | Line 26... | ||
12 | 26 | public final byte GPS_FORMAT_COUNT=2; |
|
Line 13... | Line 27... | ||
13 | { |
27 | byte act_gps_format=GPS_FORMAT_DECIMAL; |
14 | 28 | ||
- | 29 | public final static int MAX_WAYPOINTS=100; |
|
15 | public final static int MAX_WAYPOINTS=100; |
30 | |
16 | 31 | int[] LongWP; |
|
Line 17... | Line 32... | ||
17 | int[] LongWP; |
32 | int[] LatWP; |
18 | int[] LatWP; |
33 | String[] NameWP; |
Line 28... | Line 43... | ||
28 | 43 | ||
29 | int Distance2Target; |
44 | int Distance2Target; |
Line 30... | Line 45... | ||
30 | int Angle2Target; |
45 | int Angle2Target; |
- | 46 | ||
- | 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. |
|
Line 31... | Line -... | ||
31 | - | ||
32 | byte Used_Sat; |
- | |
33 | - | ||
34 | - | ||
35 | public void push_wp() |
- | |
36 | { |
- | |
Line -... | Line 101... | ||
- | 101 | */ |
|
- | 102 | private static final double q0 = 0.89678597403663861962481162e3; |
|
- | 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); |
|
37 | LongWP[last_wp]=Longitude; |
193 | angle = coeff_2 - coeff_1 * r; |
38 | LatWP[last_wp]=Latitude; |
194 | } |
- | 195 | return y < 0d ? -angle : angle; |
|
- | 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); |
|
39 | last_wp++; |
209 | double dLon= (long2-long1); |
40 | } |
210 | |
Line -... | Line 211... | ||
- | 211 | double a = Math.sin(dLat/2.0) * Math.sin(dLat/2.0) + |
|
- | 212 | Math.cos(lat1) * Math.cos(lat2) * |
|
41 | 213 | Math.sin(dLon/2.0) * Math.sin(dLon/2.0); |
|
42 | 214 | ||
- | 215 | return (int)(( 2.0 * aTan2(Math.sqrt(a), Math.sqrt(1.0-a)) )*6371008.8); |
|
- | 216 | } |
|
- | 217 | ||
- | 218 | ||
- | 219 | ||
- | 220 | ||
- | 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; |
|
Line -... | Line 226... | ||
- | 226 | ||
- | 227 | double lat2=(LatWP[id]/10000000.0)*RADIANS; |
|
- | 228 | double long2=(LongWP[id]/10000000.0)*RADIANS; |
|
43 | public String dec_to_min_sec(int val) |
229 | |
- | 230 | ||
44 | { |
231 | double dLat= (lat2-lat1); |
Line -... | Line 232... | ||
- | 232 | double dLon= (long2-long1); |
|
- | 233 | ||
45 | return "" + val/10000000 + "^" + ((val%10000000)*60)/10000000 + "'" + ((((val%10000000)*60)%10000000)*60)/10000000 + "." + ((((val%10000000)*60)%10000000)*60)%10000000; |
234 | |
46 | } |
235 | |
47 | 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); |
|
Line -... | Line 238... | ||
- | 238 | return ((int)(aTan2(y, x)*DEGREES)+360)%360; |
|
48 | 239 | ||
Line -... | Line 240... | ||
- | 240 | } |
|
- | 241 | ||
- | 242 | ||
- | 243 | ||
Line -... | Line 244... | ||
- | 244 | public void push_wp() |
|
- | 245 | { |
|
- | 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 | } |
|
Line 49... | Line 256... | ||
49 | public String WP_Latitude_min_sec(int id) |
256 | |
50 | { |
257 | public String act_gps_format_str(int val) |
Line 51... | Line 258... | ||
51 | 258 | { |
|
52 | return "" + dec_to_min_sec(LatWP[id])+ "''N" ; |
259 | switch(act_gps_format) |
Line 53... | Line 260... | ||
53 | } |
260 | { |
54 | 261 | case GPS_FORMAT_DECIMAL: |
|
55 | public String WP_Longitude_min_sec(int id) |
262 | return "" + val/10000000 + "." +val%10000000 ; |
Line 56... | Line 263... | ||
56 | { |
263 | case GPS_FORMAT_MINSEC: |
Line 57... | Line 264... | ||
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: |
|
- | 266 | return "invalid format"; |
|
59 | } |
267 | } |
60 | 268 | } |
|
Line 61... | Line 269... | ||
61 | 269 | ||
62 | 270 | public String WP_Latitude_str(int id) |
|
63 | public String Latitude_min_sec() |
271 | { |
- | 272 | ||
64 | { |
273 | return "" + act_gps_format_str(LatWP[id]); //+ "''N" ; |
Line -... | Line 274... | ||
- | 274 | } |
|
- | 275 | ||
65 | 276 | public String WP_Longitude_str(int id) |
|
66 | return "" + dec_to_min_sec(Latitude)+ "''N" ; |
277 | { |
Line 67... | Line 278... | ||
67 | } |
278 | return "" + act_gps_format_str(LongWP[id]); //+ "''E" ; |
68 | 279 | ||
- | 280 | } |
|
- | 281 | ||
- | 282 | public String Latitude_str() |
|
- | 283 | { |
|
- | 284 | ||
- | 285 | return "" + act_gps_format_str(Latitude) ; |
|
- | 286 | } |
|
- | 287 | ||
- | 288 | public String Longitude_str() |
|
- | 289 | { |
|
- | 290 | return "" + act_gps_format_str(Longitude) ; |
|
- | 291 | ||
- | 292 | } |
|
- | 293 | ||
- | 294 | ||
- | 295 | // Constructor |
|
- | 296 | public MKGPSPosition() |
|
Line -... | Line 297... | ||
- | 297 | { |
|
- | 298 | ||
69 | public String Longitude_min_sec() |
299 | LongWP=new int[MAX_WAYPOINTS]; |
Line 70... | Line 300... | ||
70 | { |
300 | LatWP=new int[MAX_WAYPOINTS]; |
71 | return "" + dec_to_min_sec(Longitude)+ "''E" ; |
301 | |
72 | 302 | ||
Line 100... | Line 330... | ||
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); |
- | |
105 | 334 | Latitude=parse_arr(4,in_arr); |
|
106 | TargetLongitude=parse_arr(8,in_arr); |
335 | TargetLongitude=parse_arr(8,in_arr); |
107 | TargetLatitude=parse_arr(12,in_arr); |
- | |
108 | 336 | TargetLatitude=parse_arr(12,in_arr); |
|
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); |
Line 111... | Line 339... | ||
111 | Used_Sat=(byte)in_arr[24]; |
339 | Used_Sat=(byte)in_arr[24]; |
112 | - | ||
113 | } |
- | |
114 | - | ||
115 | 340 |