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

Contents of /navit/navit/coord.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 48 - (show annotations) (download)
Sat Oct 24 13:12:03 2015 UTC (8 years, 5 months ago) by zoff99
File MIME type: text/plain
File size: 11973 byte(s)
v2.0.49
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
211 if (c->x > r->rl.x)
212 r->rl.x=c->x;
213
214 if (c->y < r->rl.y)
215 r->rl.y=c->y;
216
217 if (c->y > r->lu.y)
218 r->lu.y=c->y;
219
220 }
221
222 /*
223 *
224 * extend rect by "percent" (e.g.: 0.2f) --> means: add left 10% and add right 10% and so on ...
225 *
226 */
227 void coord_rect_extend_by_percent(struct coord_rect *r, float percent)
228 {
229 // extend rectangle by factor
230 //
231 // x
232 int dx = abs(r->rl.x - r->lu.x);
233 float dx2 = ((float)dx * percent)/2.0f;
234 // dbg(0, "EXTEND_RECT:01a:dx=%d dx2=%f lu.x=%d rl.x=%d\n", dx, dx2, r->lu.x, r->rl.x);
235 r->rl.x = (int)((float)r->rl.x + dx2);
236 r->lu.x = (int)((float)r->lu.x - dx2);
237 // dbg(0, "EXTEND_RECT:01b:dx=%d dx2=%f lu.x=%d rl.x=%d\n", dx, dx2, r->lu.x, r->rl.x);
238
239 //
240 // y
241 int dy = abs(r->rl.y - r->lu.y);
242 float dy2 = ((float)dy * percent)/2.0f;
243 // dbg(0, "EXTEND_RECT:02a:dy=%d dy2=%f rl.y=%d lu.y=%d\n", dy, dy2, r->rl.y, r->lu.y);
244 r->rl.y = (int)((float)r->rl.y - dy2);
245 r->lu.y = (int)((float)r->lu.y + dy2);
246 // dbg(0, "EXTEND_RECT:02b:dy=%d dy2=%f rl.y=%d lu.y=%d\n", dy, dy2, r->rl.y, r->lu.y);
247 }
248
249
250 /**
251 * 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.
252 * The format for \a *c_str can be:
253 * \li [Proj:]-0xX [-]0xX
254 * - where Proj can be mg/garmin, defaults to mg
255 * \li [Proj:][D][D]Dmm.ss[S][S] N/S [D][D]DMM.ss[S][S]... E/W
256 * \li [Proj:][-][D]D.d[d]... [-][D][D]D.d[d]
257 * - where Proj can be geo
258 *
259 * @param *c_str String to be parsed
260 * @param pro Projection of the string
261 * @param *pc_ret Where the \a pcoord should get stored
262 * @returns The lenght of the parsed string
263 */
264
265 int
266 coord_parse(const char *c_str, enum projection pro, struct coord *c_ret)
267 {
268 int debug=0;
269 char *proj=NULL,*s,*co;
270 const char *str=c_str;
271 int args,ret = 0;
272 struct coord_geo g;
273 struct coord c;
274 enum projection str_pro=projection_none;
275
276 //dbg(1,"enter('%s',%d,%p)\n", c_str, pro, c_ret);
277 s=strchr(str,' ');
278 co=strchr(str,':');
279 if (co && co < s) {
280 proj=malloc(co-str+1);
281 strncpy(proj, str, co-str);
282 proj[co-str]='\0';
283 //dbg(1,"projection=%s\n", proj);
284 str=co+1;
285 s=strchr(str,' ');
286 if (!strcmp(proj, "mg"))
287 str_pro = projection_mg;
288 else if (!strcmp(proj, "garmin"))
289 str_pro = projection_garmin;
290 else if (!strcmp(proj, "geo"))
291 str_pro = projection_none;
292 else {
293 dbg(0, "Unknown projection: %s\n", proj);
294 goto out;
295 }
296 }
297 if (! s) {
298 ret=0;
299 goto out;
300 }
301 while (*s == ' ') {
302 s++;
303 }
304 if (!strncmp(s, "0x", 2) || !strncmp(s, "-0x", 3)) {
305 args=sscanf(str, "%i %i%n",&c.x, &c.y, &ret);
306 if (args < 2)
307 goto out;
308 //dbg(1,"str='%s' x=0x%x y=0x%x c=%d\n", str, c.x, c.y, ret);
309 //dbg(1,"rest='%s'\n", str+ret);
310
311 if (str_pro == projection_none)
312 str_pro=projection_mg;
313 if (str_pro != pro) {
314 transform_to_geo(str_pro, &c, &g);
315 transform_from_geo(pro, &g, &c);
316 }
317 *c_ret=c;
318 } else if (*s == 'N' || *s == 'n' || *s == 'S' || *s == 's') {
319 double lng, lat;
320 char ns, ew;
321 //dbg(1,"str='%s'\n", str);
322 args=sscanf(str, "%lf %c %lf %c%n", &lat, &ns, &lng, &ew, &ret);
323 //dbg(1,"args=%d\n", args);
324 //dbg(1,"lat=%f %c lon=%f %c\n", lat, ns, lng, ew);
325 if (args < 4)
326 goto out;
327 //dbg(1,"projection=%d str_pro=%d projection_none=%d\n", pro, str_pro, projection_none);
328 if (str_pro == projection_none) {
329 g.lat=floor(lat/100);
330 lat-=g.lat*100;
331 g.lat+=lat/60;
332 g.lng=floor(lng/100);
333 lng-=g.lng*100;
334 g.lng+=lng/60;
335 if (ns == 's' || ns == 'S')
336 g.lat=-g.lat;
337 if (ew == 'w' || ew == 'W')
338 g.lng=-g.lng;
339 //dbg(1,"transform_from_geo(%f,%f)",g.lat,g.lng);
340 transform_from_geo(pro, &g, c_ret);
341 //dbg(1,"result 0x%x,0x%x\n", c_ret->x,c_ret->y);
342 }
343 //dbg(3,"str='%s' x=%f ns=%c y=%f ew=%c c=%d\n", str, lng, ns, lat, ew, ret);
344 //dbg(3,"rest='%s'\n", str+ret);
345 } else {
346 double lng, lat;
347 args=sscanf(str, "%lf %lf%n", &lng, &lat, &ret);
348 if (args < 2)
349 goto out;
350 //dbg(1,"str='%s' x=%f y=%f c=%d\n", str, lng, lat, ret);
351 //dbg(1,"rest='%s'\n", str+ret);
352 g.lng=lng;
353 g.lat=lat;
354 transform_from_geo(pro, &g, c_ret);
355 }
356 //if (debug)
357 // printf("rest='%s'\n", str+ret);
358 ret+=str-c_str;
359 //if (debug) {
360 // printf("args=%d\n", args);
361 // printf("ret=%d delta=%d ret_str='%s'\n", ret, GPOINTER_TO_INT(str-c_str), c_str+ret);
362 //}
363 out:
364 free(proj);
365 return ret;
366 }
367
368 /**
369 * A wrapper for pcoord_parse that also return the projection
370 * @param *c_str String to be parsed
371 * @param pro Projection of the string
372 * @param *pc_ret Where the \a pcoord should get stored
373 * @returns The lenght of the parsed string
374 */
375
376 int
377 pcoord_parse(const char *c_str, enum projection pro, struct pcoord *pc_ret)
378 {
379 struct coord c;
380 int ret;
381 ret = coord_parse(c_str, pro, &c);
382 pc_ret->x = c.x;
383 pc_ret->y = c.y;
384 pc_ret->pro = pro;
385 return ret;
386 }
387
388 void
389 coord_print(enum projection pro, struct coord *c, FILE *out) {
390 unsigned int x;
391 unsigned int y;
392 char *sign_x = "";
393 char *sign_y = "";
394
395 if ( c->x < 0 ) {
396 x = -c->x;
397 sign_x = "-";
398 } else {
399 x = c->x;
400 }
401 if ( c->y < 0 ) {
402 y = -c->y;
403 sign_y = "-";
404 } else {
405 y = c->y;
406 }
407 fprintf( out, "%s: %s0x%x %s0x%x\n",
408 projection_to_name( pro , NULL),
409 sign_x, x,
410 sign_y, y );
411 return;
412 }
413
414 /**
415 * @brief Converts a lat/lon into a text formatted text string.
416 * @param lat The latitude (if lat is 360 or greater, the latitude will be omitted)
417 * @param lng The longitude (if lng is 360 or greater, the longitude will be omitted)
418 * @param fmt The format to use.
419 * @li DEGREES_DECIMAL=>Degrees with decimal places, i.e. 20.5000°N 110.5000°E
420 * @li DEGREES_MINUTES=>Degrees and minutes, i.e. 20°30.00'N 110°30.00'E
421 * @li DEGREES_MINUTES_SECONDS=>Degrees, minutes and seconds, i.e. 20°30'30.00"N 110°30'30"E
422 *
423 *
424 * @param buffer A buffer large enough to hold the output + a terminating NULL (up to 31 bytes)
425 * @param size The size of the buffer
426 *
427 */
428 void coord_format(float lat,float lng, enum coord_format fmt, char * buffer, int size)
429 {
430
431 char lat_c='N';
432 char lng_c='E';
433 float lat_deg,lat_min,lat_sec;
434 float lng_deg,lng_min,lng_sec;
435 int size_used=0;
436
437 if (lng < 0) {
438 lng=-lng;
439 lng_c='W';
440 }
441 if (lat < 0) {
442 lat=-lat;
443 lat_c='S';
444 }
445 lat_deg=lat;
446 lat_min=(lat-floor(lat_deg))*60;
447 lat_sec=fmod(lat*3600,60);
448 lng_deg=lng;
449 lng_min=(lng-floor(lng_deg))*60;
450 lng_sec=fmod(lng*3600,60);
451 switch(fmt)
452 {
453
454 case DEGREES_DECIMAL:
455 if (lat<360)
456 size_used+=g_snprintf(buffer+size_used,size-size_used,"%02.6f°%c",lat,lat_c);
457 if ((lat<360)&&(lng<360))
458 size_used+=g_snprintf(buffer+size_used,size-size_used," ");
459 if (lng<360)
460 size_used+=g_snprintf(buffer+size_used,size-size_used,"%03.7f°%c",lng,lng_c);
461 break;
462 case DEGREES_MINUTES:
463 if (lat<360)
464 size_used+=g_snprintf(buffer+size_used,size-size_used,"%02.0f°%07.4f' %c",floor(lat_deg),lat_min,lat_c);
465 if ((lat<360)&&(lng<360))
466 size_used+=g_snprintf(buffer+size_used,size-size_used," ");
467 if (lng<360)
468 size_used+=g_snprintf(buffer+size_used,size-size_used,"%03.0f°%07.4f' %c",floor(lng_deg),lng_min,lng_c);
469 break;
470 case DEGREES_MINUTES_SECONDS:
471 if (lat<360)
472 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);
473 if ((lat<360)&&(lng<360))
474 size_used+=g_snprintf(buffer+size_used,size-size_used," ");
475 if (lng<360)
476 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);
477 break;
478
479
480 }
481
482 }
483
484 unsigned int
485 coord_hash(const void *key)
486 {
487 const struct coord *c=key;
488 return c->x^c->y;
489 }
490
491 int
492 coord_equal(const void *a, const void *b)
493 {
494 const struct coord *c_a=a;
495 const struct coord *c_b=b;
496
497 if (c_a->x == c_b->x && c_a->y == c_b->y)
498 {
499 return TRUE;
500 }
501 return FALSE;
502 }
503
504
505 /** @} */
506
507

   
Visit the ZANavi Wiki