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

Contents of /navit/navit/coord.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 31 - (hide 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 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     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 zoff99 31 //dbg(1,"enter('%s',%d,%p)\n", c_str, pro, c_ret);
245 zoff99 2 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 zoff99 31 //dbg(1,"projection=%s\n", proj);
252 zoff99 2 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 zoff99 31 //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 zoff99 2
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 zoff99 31 //dbg(1,"str='%s'\n", str);
290 zoff99 2 args=sscanf(str, "%lf %c %lf %c%n", &lat, &ns, &lng, &ew, &ret);
291 zoff99 31 //dbg(1,"args=%d\n", args);
292     //dbg(1,"lat=%f %c lon=%f %c\n", lat, ns, lng, ew);
293 zoff99 2 if (args < 4)
294     goto out;
295 zoff99 31 //dbg(1,"projection=%d str_pro=%d projection_none=%d\n", pro, str_pro, projection_none);
296 zoff99 2 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 zoff99 31 //dbg(1,"transform_from_geo(%f,%f)",g.lat,g.lng);
308 zoff99 2 transform_from_geo(pro, &g, c_ret);
309 zoff99 31 //dbg(1,"result 0x%x,0x%x\n", c_ret->x,c_ret->y);
310 zoff99 2 }
311 zoff99 31 //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 zoff99 2 } else {
314     double lng, lat;
315     args=sscanf(str, "%lf %lf%n", &lng, &lat, &ret);
316     if (args < 2)
317     goto out;
318 zoff99 31 //dbg(1,"str='%s' x=%f y=%f c=%d\n", str, lng, lat, ret);
319     //dbg(1,"rest='%s'\n", str+ret);
320 zoff99 2 g.lng=lng;
321     g.lat=lat;
322     transform_from_geo(pro, &g, c_ret);
323     }
324 zoff99 31 //if (debug)
325     // printf("rest='%s'\n", str+ret);
326 zoff99 2 ret+=str-c_str;
327 zoff99 31 //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 zoff99 2 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 zoff99 31 const struct coord *c=key;
456 zoff99 2 return c->x^c->y;
457     }
458    
459     int
460     coord_equal(const void *a, const void *b)
461     {
462 zoff99 31 const struct coord *c_a=a;
463     const struct coord *c_b=b;
464    
465 zoff99 2 if (c_a->x == c_b->x && c_a->y == c_b->y)
466 zoff99 31 {
467     return TRUE;
468     }
469     return FALSE;
470 zoff99 2 }
471     /** @} */

   
Visit the ZANavi Wiki