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

Contents of /navit/navit/coord.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 48 - (hide 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 zoff99 2 /**
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 zoff99 28
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 zoff99 2 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 zoff99 31 //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 zoff99 2
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 zoff99 41
211 zoff99 2 if (c->x > r->rl.x)
212     r->rl.x=c->x;
213 zoff99 41
214 zoff99 2 if (c->y < r->rl.y)
215     r->rl.y=c->y;
216 zoff99 41
217 zoff99 2 if (c->y > r->lu.y)
218     r->lu.y=c->y;
219 zoff99 41
220 zoff99 2 }
221    
222 zoff99 41 /*
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 zoff99 48 // 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 zoff99 41 r->rl.x = (int)((float)r->rl.x + dx2);
236     r->lu.x = (int)((float)r->lu.x - dx2);
237 zoff99 48 // 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 zoff99 41 //
240     // y
241     int dy = abs(r->rl.y - r->lu.y);
242     float dy2 = ((float)dy * percent)/2.0f;
243 zoff99 48 // 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 zoff99 41 }
248    
249    
250 zoff99 2 /**
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 zoff99 31 //dbg(1,"enter('%s',%d,%p)\n", c_str, pro, c_ret);
277 zoff99 2 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 zoff99 31 //dbg(1,"projection=%s\n", proj);
284 zoff99 2 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 zoff99 31 //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 zoff99 2
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 zoff99 31 //dbg(1,"str='%s'\n", str);
322 zoff99 2 args=sscanf(str, "%lf %c %lf %c%n", &lat, &ns, &lng, &ew, &ret);
323 zoff99 31 //dbg(1,"args=%d\n", args);
324     //dbg(1,"lat=%f %c lon=%f %c\n", lat, ns, lng, ew);
325 zoff99 2 if (args < 4)
326     goto out;
327 zoff99 31 //dbg(1,"projection=%d str_pro=%d projection_none=%d\n", pro, str_pro, projection_none);
328 zoff99 2 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 zoff99 31 //dbg(1,"transform_from_geo(%f,%f)",g.lat,g.lng);
340 zoff99 2 transform_from_geo(pro, &g, c_ret);
341 zoff99 31 //dbg(1,"result 0x%x,0x%x\n", c_ret->x,c_ret->y);
342 zoff99 2 }
343 zoff99 31 //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 zoff99 2 } else {
346     double lng, lat;
347     args=sscanf(str, "%lf %lf%n", &lng, &lat, &ret);
348     if (args < 2)
349     goto out;
350 zoff99 31 //dbg(1,"str='%s' x=%f y=%f c=%d\n", str, lng, lat, ret);
351     //dbg(1,"rest='%s'\n", str+ret);
352 zoff99 2 g.lng=lng;
353     g.lat=lat;
354     transform_from_geo(pro, &g, c_ret);
355     }
356 zoff99 31 //if (debug)
357     // printf("rest='%s'\n", str+ret);
358 zoff99 2 ret+=str-c_str;
359 zoff99 31 //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 zoff99 2 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 zoff99 31 const struct coord *c=key;
488 zoff99 2 return c->x^c->y;
489     }
490    
491     int
492     coord_equal(const void *a, const void *b)
493     {
494 zoff99 31 const struct coord *c_a=a;
495     const struct coord *c_b=b;
496    
497 zoff99 2 if (c_a->x == c_b->x && c_a->y == c_b->y)
498 zoff99 31 {
499     return TRUE;
500     }
501     return FALSE;
502 zoff99 2 }
503 zoff99 41
504    
505 zoff99 2 /** @} */
506 zoff99 41
507    

   
Visit the ZANavi Wiki