/[zanavi_public1]/navit/navit/coord.c
ZANavi

Contents of /navit/navit/coord.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 31 - (show annotations) (download)
Mon Feb 4 17:41:59 2013 UTC (11 years, 1 month ago) by zoff99
File MIME type: text/plain
File size: 11060 byte(s)
new map version, lots of fixes and experimental new features
1 /**
2 * Navit, a modular navigation system.
3 * Copyright (C) 2005-2008 Navit Team
4 *
5 * This program is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License
7 * version 2 as published by the Free Software Foundation.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the
16 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
17 * Boston, MA 02110-1301, USA.
18 */
19
20 #include <stdlib.h>
21 #include <glib.h>
22 #include <stdio.h>
23 #include <string.h>
24 #include <math.h>
25 #include "debug.h"
26 #include "item.h"
27 #include "coord.h"
28 #include "transform.h"
29 #include "projection.h"
30 /**
31 * @defgroup coord Coordinate handling functions
32 * @{
33 */
34
35 /**
36 * Get a coordinate
37 *
38 * @param p Pointer to the coordinate
39 * @returns the coordinate
40 */
41
42
43
44 /*
45 ================
46 sine lookup table (input in degress)
47 ================
48 */
49 /*
50 #define SINMAX 9000
51 double sinlut[SINMAX + 1];
52 void sincreate(void)
53 {
54 int i;
55 double angle, angleinc;
56
57 angleinc = 3.1415926535 / 2.0 / (SINMAX);
58 for (i = 0, angle = 0.0; i <= SINMAX; ++i, angle += angleinc)
59 {
60 sinlut[i] = sin(angle);
61 }
62 }
63
64 double sin_lut(double degrees)
65 {
66 int ix;
67 ix = (int)(degrees * 100.0);
68 return sinlut[ix];
69 }
70 */
71
72 // very fast, but can be a bit off the real value!! so be careful!
73 float sqrtf_fast2(float x2)
74 {
75 unsigned int i = *(unsigned int*) &x2;
76 // adjust bias
77 i += 127 << 23;
78 // approximation of square root
79 i >>= 1;
80 return *(float*) &i;
81 }
82
83 /*
84 ================
85 SquareRootFloat (rather accurate)
86 ================
87 */
88 float sqrtf_fast(float number2)
89 {
90 long i;
91 float x, y;
92 const float f = 1.5F;
93
94 x = number2 * 0.5F;
95 y = number2;
96
97 i = * ( long * ) &y;
98 i = 0x5f3759df - ( i >> 1 );
99 y = * ( float * ) &i;
100 y = y * ( f - ( x * y * y ) );
101 y = y * ( f - ( x * y * y ) );
102 return number2 * y;
103 }
104
105
106
107 struct coord *
108 coord_get(unsigned char **p)
109 {
110 struct coord *ret=(struct coord *)(*p);
111 *p += sizeof(*ret);
112 return ret;
113 }
114
115 struct coord *
116 coord_new(int x, int y)
117 {
118 struct coord *c=g_new(struct coord, 1);
119
120 c->x=x;
121 c->y=y;
122
123 return c;
124 }
125
126 struct coord *
127 coord_new_from_attrs(struct attr *parent, struct attr **attrs)
128 {
129 struct attr *x,*y;
130 x=attr_search(attrs, NULL, attr_x);
131 y=attr_search(attrs, NULL, attr_y);
132 if (!x || !y)
133 return NULL;
134 return coord_new(x->u.num, y->u.num);
135 }
136
137
138 void
139 coord_destroy(struct coord *c)
140 {
141 g_free(c);
142 }
143
144 struct coord_rect *
145 coord_rect_new(struct coord *lu, struct coord *rl)
146 {
147 struct coord_rect *r=g_new(struct coord_rect, 1);
148
149 dbg_assert(lu->x <= rl->x);
150 dbg_assert(lu->y >= rl->y);
151
152 r->lu=*lu;
153 r->rl=*rl;
154
155 return r;
156
157 }
158
159 void
160 coord_rect_destroy(struct coord_rect *r)
161 {
162 g_free(r);
163 }
164
165 int
166 coord_rect_overlap(struct coord_rect *r1, struct coord_rect *r2)
167 {
168 //dbg(1,"0x%x,0x%x - 0x%x,0x%x vs 0x%x,0x%x - 0x%x,0x%x\n", r1->lu.x, r1->lu.y, r1->rl.x, r1->rl.y, r2->lu.x, r2->lu.y, r2->rl.x, r2->rl.y);
169
170 // ****** this sometimes causes a crash at startup!! ********
171 // ****** this sometimes causes a crash at startup!! ********
172 // dbg_assert(r1->lu.x <= r1->rl.x);
173 // dbg_assert(r1->lu.y >= r1->rl.y);
174 // dbg_assert(r2->lu.x <= r2->rl.x);
175 // dbg_assert(r2->lu.y >= r2->rl.y);
176 // ****** this sometimes causes a crash at startup!! ********
177 // ****** this sometimes causes a crash at startup!! ********
178 if (r1->lu.x > r2->rl.x)
179 return 0;
180 if (r1->rl.x < r2->lu.x)
181 return 0;
182 if (r1->lu.y < r2->rl.y)
183 return 0;
184 if (r1->rl.y > r2->lu.y)
185 return 0;
186 return 1;
187 }
188
189 int
190 coord_rect_contains(struct coord_rect *r, struct coord *c)
191 {
192 dbg_assert(r->lu.x <= r->rl.x);
193 dbg_assert(r->lu.y >= r->rl.y);
194 if (c->x < r->lu.x)
195 return 0;
196 if (c->x > r->rl.x)
197 return 0;
198 if (c->y < r->rl.y)
199 return 0;
200 if (c->y > r->lu.y)
201 return 0;
202 return 1;
203 }
204
205 void
206 coord_rect_extend(struct coord_rect *r, struct coord *c)
207 {
208 if (c->x < r->lu.x)
209 r->lu.x=c->x;
210 if (c->x > r->rl.x)
211 r->rl.x=c->x;
212 if (c->y < r->rl.y)
213 r->rl.y=c->y;
214 if (c->y > r->lu.y)
215 r->lu.y=c->y;
216 }
217
218 /**
219 * Parses \c char \a *c_str and writes back the coordinates to \c coord \a *c_ret. Uses \c projection \a pro if no projection is given in \c char \a *c_str.
220 * The format for \a *c_str can be:
221 * \li [Proj:]-0xX [-]0xX
222 * - where Proj can be mg/garmin, defaults to mg
223 * \li [Proj:][D][D]Dmm.ss[S][S] N/S [D][D]DMM.ss[S][S]... E/W
224 * \li [Proj:][-][D]D.d[d]... [-][D][D]D.d[d]
225 * - where Proj can be geo
226 *
227 * @param *c_str String to be parsed
228 * @param pro Projection of the string
229 * @param *pc_ret Where the \a pcoord should get stored
230 * @returns The lenght of the parsed string
231 */
232
233 int
234 coord_parse(const char *c_str, enum projection pro, struct coord *c_ret)
235 {
236 int debug=0;
237 char *proj=NULL,*s,*co;
238 const char *str=c_str;
239 int args,ret = 0;
240 struct coord_geo g;
241 struct coord c;
242 enum projection str_pro=projection_none;
243
244 //dbg(1,"enter('%s',%d,%p)\n", c_str, pro, c_ret);
245 s=strchr(str,' ');
246 co=strchr(str,':');
247 if (co && co < s) {
248 proj=malloc(co-str+1);
249 strncpy(proj, str, co-str);
250 proj[co-str]='\0';
251 //dbg(1,"projection=%s\n", proj);
252 str=co+1;
253 s=strchr(str,' ');
254 if (!strcmp(proj, "mg"))
255 str_pro = projection_mg;
256 else if (!strcmp(proj, "garmin"))
257 str_pro = projection_garmin;
258 else if (!strcmp(proj, "geo"))
259 str_pro = projection_none;
260 else {
261 dbg(0, "Unknown projection: %s\n", proj);
262 goto out;
263 }
264 }
265 if (! s) {
266 ret=0;
267 goto out;
268 }
269 while (*s == ' ') {
270 s++;
271 }
272 if (!strncmp(s, "0x", 2) || !strncmp(s, "-0x", 3)) {
273 args=sscanf(str, "%i %i%n",&c.x, &c.y, &ret);
274 if (args < 2)
275 goto out;
276 //dbg(1,"str='%s' x=0x%x y=0x%x c=%d\n", str, c.x, c.y, ret);
277 //dbg(1,"rest='%s'\n", str+ret);
278
279 if (str_pro == projection_none)
280 str_pro=projection_mg;
281 if (str_pro != pro) {
282 transform_to_geo(str_pro, &c, &g);
283 transform_from_geo(pro, &g, &c);
284 }
285 *c_ret=c;
286 } else if (*s == 'N' || *s == 'n' || *s == 'S' || *s == 's') {
287 double lng, lat;
288 char ns, ew;
289 //dbg(1,"str='%s'\n", str);
290 args=sscanf(str, "%lf %c %lf %c%n", &lat, &ns, &lng, &ew, &ret);
291 //dbg(1,"args=%d\n", args);
292 //dbg(1,"lat=%f %c lon=%f %c\n", lat, ns, lng, ew);
293 if (args < 4)
294 goto out;
295 //dbg(1,"projection=%d str_pro=%d projection_none=%d\n", pro, str_pro, projection_none);
296 if (str_pro == projection_none) {
297 g.lat=floor(lat/100);
298 lat-=g.lat*100;
299 g.lat+=lat/60;
300 g.lng=floor(lng/100);
301 lng-=g.lng*100;
302 g.lng+=lng/60;
303 if (ns == 's' || ns == 'S')
304 g.lat=-g.lat;
305 if (ew == 'w' || ew == 'W')
306 g.lng=-g.lng;
307 //dbg(1,"transform_from_geo(%f,%f)",g.lat,g.lng);
308 transform_from_geo(pro, &g, c_ret);
309 //dbg(1,"result 0x%x,0x%x\n", c_ret->x,c_ret->y);
310 }
311 //dbg(3,"str='%s' x=%f ns=%c y=%f ew=%c c=%d\n", str, lng, ns, lat, ew, ret);
312 //dbg(3,"rest='%s'\n", str+ret);
313 } else {
314 double lng, lat;
315 args=sscanf(str, "%lf %lf%n", &lng, &lat, &ret);
316 if (args < 2)
317 goto out;
318 //dbg(1,"str='%s' x=%f y=%f c=%d\n", str, lng, lat, ret);
319 //dbg(1,"rest='%s'\n", str+ret);
320 g.lng=lng;
321 g.lat=lat;
322 transform_from_geo(pro, &g, c_ret);
323 }
324 //if (debug)
325 // printf("rest='%s'\n", str+ret);
326 ret+=str-c_str;
327 //if (debug) {
328 // printf("args=%d\n", args);
329 // printf("ret=%d delta=%d ret_str='%s'\n", ret, GPOINTER_TO_INT(str-c_str), c_str+ret);
330 //}
331 out:
332 free(proj);
333 return ret;
334 }
335
336 /**
337 * A wrapper for pcoord_parse that also return the projection
338 * @param *c_str String to be parsed
339 * @param pro Projection of the string
340 * @param *pc_ret Where the \a pcoord should get stored
341 * @returns The lenght of the parsed string
342 */
343
344 int
345 pcoord_parse(const char *c_str, enum projection pro, struct pcoord *pc_ret)
346 {
347 struct coord c;
348 int ret;
349 ret = coord_parse(c_str, pro, &c);
350 pc_ret->x = c.x;
351 pc_ret->y = c.y;
352 pc_ret->pro = pro;
353 return ret;
354 }
355
356 void
357 coord_print(enum projection pro, struct coord *c, FILE *out) {
358 unsigned int x;
359 unsigned int y;
360 char *sign_x = "";
361 char *sign_y = "";
362
363 if ( c->x < 0 ) {
364 x = -c->x;
365 sign_x = "-";
366 } else {
367 x = c->x;
368 }
369 if ( c->y < 0 ) {
370 y = -c->y;
371 sign_y = "-";
372 } else {
373 y = c->y;
374 }
375 fprintf( out, "%s: %s0x%x %s0x%x\n",
376 projection_to_name( pro , NULL),
377 sign_x, x,
378 sign_y, y );
379 return;
380 }
381
382 /**
383 * @brief Converts a lat/lon into a text formatted text string.
384 * @param lat The latitude (if lat is 360 or greater, the latitude will be omitted)
385 * @param lng The longitude (if lng is 360 or greater, the longitude will be omitted)
386 * @param fmt The format to use.
387 * @li DEGREES_DECIMAL=>Degrees with decimal places, i.e. 20.5000°N 110.5000°E
388 * @li DEGREES_MINUTES=>Degrees and minutes, i.e. 20°30.00'N 110°30.00'E
389 * @li DEGREES_MINUTES_SECONDS=>Degrees, minutes and seconds, i.e. 20°30'30.00"N 110°30'30"E
390 *
391 *
392 * @param buffer A buffer large enough to hold the output + a terminating NULL (up to 31 bytes)
393 * @param size The size of the buffer
394 *
395 */
396 void coord_format(float lat,float lng, enum coord_format fmt, char * buffer, int size)
397 {
398
399 char lat_c='N';
400 char lng_c='E';
401 float lat_deg,lat_min,lat_sec;
402 float lng_deg,lng_min,lng_sec;
403 int size_used=0;
404
405 if (lng < 0) {
406 lng=-lng;
407 lng_c='W';
408 }
409 if (lat < 0) {
410 lat=-lat;
411 lat_c='S';
412 }
413 lat_deg=lat;
414 lat_min=(lat-floor(lat_deg))*60;
415 lat_sec=fmod(lat*3600,60);
416 lng_deg=lng;
417 lng_min=(lng-floor(lng_deg))*60;
418 lng_sec=fmod(lng*3600,60);
419 switch(fmt)
420 {
421
422 case DEGREES_DECIMAL:
423 if (lat<360)
424 size_used+=g_snprintf(buffer+size_used,size-size_used,"%02.6f°%c",lat,lat_c);
425 if ((lat<360)&&(lng<360))
426 size_used+=g_snprintf(buffer+size_used,size-size_used," ");
427 if (lng<360)
428 size_used+=g_snprintf(buffer+size_used,size-size_used,"%03.7f°%c",lng,lng_c);
429 break;
430 case DEGREES_MINUTES:
431 if (lat<360)
432 size_used+=g_snprintf(buffer+size_used,size-size_used,"%02.0f°%07.4f' %c",floor(lat_deg),lat_min,lat_c);
433 if ((lat<360)&&(lng<360))
434 size_used+=g_snprintf(buffer+size_used,size-size_used," ");
435 if (lng<360)
436 size_used+=g_snprintf(buffer+size_used,size-size_used,"%03.0f°%07.4f' %c",floor(lng_deg),lng_min,lng_c);
437 break;
438 case DEGREES_MINUTES_SECONDS:
439 if (lat<360)
440 size_used+=g_snprintf(buffer+size_used,size-size_used,"%02.0f°%02.0f'%05.2f\" %c",floor(lat_deg),floor(lat_min),lat_sec,lat_c);
441 if ((lat<360)&&(lng<360))
442 size_used+=g_snprintf(buffer+size_used,size-size_used," ");
443 if (lng<360)
444 size_used+=g_snprintf(buffer+size_used,size-size_used,"%03.0f°%02.0f'%05.2f\" %c",floor(lng_deg),floor(lng_min),lng_sec,lng_c);
445 break;
446
447
448 }
449
450 }
451
452 unsigned int
453 coord_hash(const void *key)
454 {
455 const struct coord *c=key;
456 return c->x^c->y;
457 }
458
459 int
460 coord_equal(const void *a, const void *b)
461 {
462 const struct coord *c_a=a;
463 const struct coord *c_b=b;
464
465 if (c_a->x == c_b->x && c_a->y == c_b->y)
466 {
467 return TRUE;
468 }
469 return FALSE;
470 }
471 /** @} */

   
Visit the ZANavi Wiki