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

Contents of /navit/navit/routech.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: 14299 byte(s)
new map version, lots of fixes and experimental new features
1 zoff99 2 #include <glib.h>
2     #include <stdio.h>
3     #include "item.h"
4     #include "coord.h"
5     #include "navit.h"
6     #include "transform.h"
7     #include "profile.h"
8     #include "mapset.h"
9     #include "map.h"
10    
11     FILE *routefile;
12    
13     void routech_test(struct navit *navit);
14    
15     struct ch_edge {
16     int flags;
17     int weight;
18     struct item_id target,middle;
19     };
20    
21     struct routech_search {
22     struct pq *pq;
23     GHashTable *hash;
24     int finished;
25     int dir;
26     unsigned int upper;
27     struct item_id *via;
28     };
29    
30    
31     struct pq_element {
32     struct item_id *node_id;
33     struct item_id *parent_node_id;
34     int stalled;
35     int key;
36     int heap_element;
37     };
38    
39     struct pq_heap_element {
40     int key;
41     int element;
42     };
43    
44     struct pq {
45     int capacity;
46     int size;
47     int step;
48     int elements_capacity;
49     int elements_size;
50     int elements_step;
51     struct pq_element *elements;
52     struct pq_heap_element *heap_elements;
53     };
54    
55     static struct pq *
56     pq_new(void)
57     {
58     struct pq *ret=g_new(struct pq, 1);
59     ret->step=10;
60     ret->capacity=0;
61     ret->size=1;
62     ret->elements_step=10;
63     ret->elements_capacity=0;
64     ret->elements_size=1;
65     ret->elements=NULL;
66     ret->heap_elements=NULL;
67     return ret;
68     }
69    
70     static int
71     pq_insert(struct pq *pq, int key, struct item_id *node_id)
72     {
73     int element,i;
74     if (pq->size >= pq->capacity) {
75     pq->capacity += pq->step;
76     pq->heap_elements=g_renew(struct pq_heap_element, pq->heap_elements, pq->capacity);
77     }
78     if (pq->elements_size >= pq->elements_capacity) {
79     pq->elements_capacity += pq->elements_step;
80     pq->elements=g_renew(struct pq_element, pq->elements, pq->elements_capacity);
81     }
82     element=pq->elements_size++;
83     pq->elements[element].node_id=node_id;
84     i=pq->size++;
85     while (i > 1 && pq->heap_elements[i/2].key > key) {
86     pq->heap_elements[i]=pq->heap_elements[i/2];
87     pq->elements[pq->heap_elements[i].element].heap_element=i;
88     i/=2;
89     }
90     pq->heap_elements[i].key=key;
91     pq->heap_elements[i].element=element;
92     pq->elements[element].heap_element=i;
93     pq->elements[element].key=key;
94     return element;
95     }
96    
97     static int
98     pq_get_key(struct pq *pq, int element, int *key)
99     {
100     *key=pq->elements[element].key;
101     return 1;
102     }
103    
104     static void
105     pq_set_parent(struct pq *pq, int element, struct item_id *node_id, int stalled)
106     {
107     pq->elements[element].parent_node_id=node_id;
108     pq->elements[element].stalled=stalled;
109     }
110    
111     static struct item_id *
112     pq_get_parent_node_id(struct pq *pq, int element)
113     {
114     return pq->elements[element].parent_node_id;
115     }
116    
117     static void
118     pq_set_stalled(struct pq *pq, int element, int stalled)
119     {
120     pq->elements[element].stalled=stalled;
121     }
122    
123     static int
124     pq_get_stalled(struct pq *pq, int element)
125     {
126     return pq->elements[element].stalled;
127     }
128    
129     static int
130     pq_is_deleted(struct pq *pq, int element)
131     {
132     return (pq->elements[element].heap_element == 0);
133     }
134    
135     static int
136     pq_min(struct pq *pq)
137     {
138     return (pq->heap_elements[1].key);
139     }
140    
141     static void
142     pq_decrease_key(struct pq *pq, int element, int key)
143     {
144     int i=pq->elements[element].heap_element;
145     while (i > 1 && pq->heap_elements[i/2].key > key) {
146     pq->heap_elements[i]=pq->heap_elements[i/2];
147     pq->elements[pq->heap_elements[i].element].heap_element=i;
148     i/=2;
149     }
150     pq->heap_elements[i].element=element;
151     pq->heap_elements[i].key=key;
152     pq->elements[element].heap_element=i;
153     pq->elements[element].key=key;
154     }
155    
156     static int
157     pq_delete_min(struct pq *pq, struct item_id **node_id, int *key, int *element)
158     {
159     struct pq_heap_element min, last;
160     int i=1,j;
161     if (pq->size <= 1)
162     return 0;
163     min=pq->heap_elements[1];
164     if (node_id)
165     *node_id=pq->elements[min.element].node_id;
166     if (key)
167     *key=min.key;
168     if (element)
169     *element=min.element;
170     pq->elements[min.element].heap_element=0;
171     min.element=0;
172     last=pq->heap_elements[--pq->size];
173     while (i <= pq->size / 2) {
174     j=2*i;
175     if (j < pq->size && pq->heap_elements[j].key > pq->heap_elements[j+1].key)
176     j++;
177     if (pq->heap_elements[j].key >= last.key)
178     break;
179     pq->heap_elements[i]=pq->heap_elements[j];
180     pq->elements[pq->heap_elements[i].element].heap_element=i;
181     i=j;
182     }
183     pq->heap_elements[i]=last;
184     pq->elements[last.element].heap_element=i;
185     return 1;
186     }
187    
188     static int
189     pq_is_empty(struct pq *pq)
190     {
191     return pq->size <= 1;
192     }
193    
194     static void
195     pq_check(struct pq *pq)
196     {
197     int i;
198     for (i = 2 ; i < pq->size ; i++) {
199     if (pq->heap_elements[i/2].key > pq->heap_elements[i].key) {
200     printf("%d vs %d\n", pq->heap_elements[i/2].key, pq->heap_elements[i].key);
201     return;
202     }
203     }
204     for (i = 1 ; i < pq->size ; i++) {
205     if (i != pq->elements[pq->heap_elements[i].element].heap_element) {
206     printf("Error: heap_element %d points to element %d, but that points to %d\n",i,pq->heap_elements[i].element,pq->elements[pq->heap_elements[i].element].heap_element);
207     }
208     }
209     }
210    
211     static struct routech_search *
212     routech_search_new(int dir)
213     {
214     struct routech_search *ret=g_new0(struct routech_search, 1);
215     ret->pq=pq_new();
216 zoff99 31 ret->hash=g_hash_table_new_full(item_id_hash, item_id_equal, g_free_func, NULL);
217 zoff99 2 ret->upper=UINT_MAX;
218     ret->dir=dir;
219    
220     return ret;
221     }
222    
223     static int
224     routech_insert_node(struct routech_search *search, struct item_id **id, int val)
225     {
226     struct item_id *ret;
227     int e;
228     if (g_hash_table_lookup_extended(search->hash, *id, (gpointer)&ret, (gpointer)&e)) {
229     int oldval;
230     pq_get_key(search->pq, e, &oldval);
231     // printf("Node = %d\n",node);
232     if (oldval > val) {
233     pq_decrease_key(search->pq, e, val);
234     *id=ret;
235     return e;
236     }
237     return 0;
238     }
239     ret=g_new(struct item_id, 1);
240     *ret=**id;
241     e=pq_insert(search->pq, val, ret);
242     g_hash_table_insert(search->hash, ret, GINT_TO_POINTER(e));
243     *id=ret;
244     return e;
245     }
246    
247    
248     static int
249     routech_find_nearest(struct mapset *ms, struct coord *c, struct item_id *id, struct map **map_ret)
250     {
251     int dst=50;
252     int dstsq=dst*dst;
253     int ret=0;
254     struct map_selection sel;
255     struct map_rect *mr;
256     struct mapset_handle *msh;
257     struct map *map;
258     struct item *item;
259     sel.next=NULL;
260     sel.order=18;
261     sel.range.min=type_ch_node;
262     sel.range.max=type_ch_node;
263     sel.u.c_rect.lu.x=c->x-dst;
264     sel.u.c_rect.lu.y=c->y+dst;
265     sel.u.c_rect.rl.x=c->x+dst;
266     sel.u.c_rect.rl.y=c->y-dst;
267     printf("0x%x,0x%x-0x%x,0x%x\n",sel.u.c_rect.lu.x,sel.u.c_rect.lu.y,sel.u.c_rect.rl.x,sel.u.c_rect.rl.y);
268     msh=mapset_open(ms);
269     while ((map=mapset_next(msh, 1))) {
270     mr=map_rect_new(map, &sel);
271     if (!mr)
272     continue;
273     while ((item=map_rect_get_item(mr))) {
274     struct coord cn;
275     if (item->type == type_ch_node && item_coord_get(item, &cn, 1)) {
276     int dist=transform_distance_sq(&cn, c);
277     if (dist < dstsq) {
278     dstsq=dist;
279     id->id_hi=item->id_hi;
280     id->id_lo=item->id_lo;
281     *map_ret=map;
282     ret=1;
283     }
284     }
285     }
286     map_rect_destroy(mr);
287     }
288     mapset_close(msh);
289     dbg_assert(ret==1);
290     return ret;
291     }
292    
293     static int
294     routech_edge_valid(struct ch_edge *edge, int dir)
295     {
296     if (edge->flags & (1 << dir))
297     return 1;
298     return 0;
299     }
300    
301     static void
302     routech_stall(struct map_rect *mr, struct routech_search *curr, struct item_id *id, int key)
303     {
304     struct stall_element {
305     struct item_id id;
306     int key;
307     } *se;
308     GList *list=NULL;
309     struct item *item;
310     struct attr edge_attr;
311    
312     int index=GPOINTER_TO_INT(g_hash_table_lookup(curr->hash, id));
313     pq_set_stalled(curr->pq, index, key);
314     se=g_new(struct stall_element, 1);
315     se->id=*id;
316     se->key=key;
317     list=g_list_append(list, se);
318     while (list) {
319     se=list->data;
320     key=se->key;
321     item=map_rect_get_item_byid(mr, se->id.id_hi, se->id.id_lo);
322     while (item_attr_get(item, attr_ch_edge, &edge_attr)) {
323     struct ch_edge *edge=edge_attr.u.data;
324     if (routech_edge_valid(edge, curr->dir)) {
325     int index=GPOINTER_TO_INT(g_hash_table_lookup(curr->hash, &edge->target));
326     if (index) {
327     int newkey=key+edge->weight;
328     int target_key;
329     pq_get_key(curr->pq, index, &target_key);
330     if (newkey < target_key) {
331     if (!pq_get_stalled(curr->pq, index)) {
332     pq_set_stalled(curr->pq, index, newkey);
333     se=g_new(struct stall_element, 1);
334     se->id=edge->target;
335     se->key=newkey;
336     list=g_list_append(list, se);
337     }
338     }
339     }
340     }
341     }
342     list=g_list_remove(list, se);
343     g_free(se);
344     }
345     }
346    
347     static void
348     routech_relax(struct map_rect **mr, struct routech_search *curr, struct routech_search *opposite)
349     {
350     int val,element;
351     struct item_id *id;
352     struct item *item;
353     struct attr edge_attr;
354     int opposite_element;
355    
356     if (!pq_delete_min(curr->pq, &id, &val, &element)) {
357     return;
358     }
359     pq_check(curr->pq);
360     opposite_element=GPOINTER_TO_INT(g_hash_table_lookup(opposite->hash, id));
361     if (opposite_element && pq_is_deleted(opposite->pq, opposite_element)) {
362     int opposite_val;
363     pq_get_key(opposite->pq, opposite_element, &opposite_val);
364     if (val+opposite_val < curr->upper) {
365     curr->upper=opposite->upper=val+opposite_val;
366     printf("%d path found: 0x%x,0x%x ub = %d\n",curr->dir,id->id_hi,id->id_lo,curr->upper);
367     curr->via=opposite->via=id;
368     }
369     }
370     if (pq_get_stalled(curr->pq, element))
371     return;
372     item=map_rect_get_item_byid(mr[0], id->id_hi, id->id_lo);
373     while (item_attr_get(item, attr_ch_edge, &edge_attr)) {
374     struct ch_edge *edge=edge_attr.u.data;
375     struct item_id *target_id=&edge->target;
376     int element;
377     if (routech_edge_valid(edge, curr->dir)) {
378     int index=GPOINTER_TO_INT(g_hash_table_lookup(curr->hash, target_id));
379     if (index && routech_edge_valid(edge, 1-curr->dir)) {
380     int newkey,stallkey;
381     stallkey=pq_get_stalled(curr->pq, index);
382     if (stallkey)
383     newkey=stallkey;
384     else
385     pq_get_key(curr->pq, index, &newkey);
386     newkey+=edge->weight;
387     if (newkey < val) {
388     routech_stall(mr[1], curr, id, newkey);
389     return;
390     }
391     }
392     element=routech_insert_node(curr, &target_id, edge->weight+val);
393     if (element) {
394     pq_set_parent(curr->pq, element, id, 0);
395     }
396     }
397     }
398     }
399    
400     static void
401     routech_print_coord(struct coord *c, FILE *out)
402     {
403     int x=c->x;
404     int y=c->y;
405     char *sx="";
406     char *sy="";
407     if (x < 0) {
408     sx="-";
409     x=-x;
410     }
411     if (y < 0) {
412     sy="-";
413     y=-y;
414     }
415     fprintf(out,"%s0x%x %s0x%x\n",sx,x,sy,y);
416     }
417    
418     static void
419     routech_resolve_route(struct map_rect *mr, struct item_id *id, int flags, int dir)
420     {
421     int i,count,max=16384;
422     struct coord *ca=g_alloca(sizeof(struct coord)*(max));
423     struct item *item;
424     int rev=0;
425     if (!(flags & 8) == dir)
426     rev=1;
427     item=map_rect_get_item_byid(mr, id->id_hi, id->id_lo);
428     dbg_assert(item->type >= type_line && item->type < type_area);
429     item->type=type_street_route;
430    
431     count=item_coord_get(item, ca, max);
432     item_dump_attr(item, item->map, routefile);
433     fprintf(routefile,"debug=\"flags=%d dir=%d rev=%d\"",flags,dir,rev);
434     fprintf(routefile,"\n");
435     if (rev) {
436     for (i = count-1 ; i >= 0 ; i--)
437     routech_print_coord(&ca[i], routefile);
438     } else {
439     for (i = 0 ; i < count ; i++)
440     routech_print_coord(&ca[i], routefile);
441     }
442     }
443    
444     static int
445     routech_find_edge(struct map_rect *mr, struct item_id *from, struct item_id *to, struct item_id *middle)
446     {
447     struct item *item=map_rect_get_item_byid(mr, from->id_hi, from->id_lo);
448     struct attr edge_attr;
449     dbg_assert(item->type == type_ch_node);
450     dbg(1,"type %s\n",item_to_name(item->type));
451     dbg(1,"segment item=%p\n",item);
452     while (item_attr_get(item, attr_ch_edge, &edge_attr)) {
453     struct ch_edge *edge=edge_attr.u.data;
454     dbg(1,"flags=%d\n",edge->flags);
455     if (edge->target.id_hi == to->id_hi && edge->target.id_lo == to->id_lo) {
456     *middle=edge->middle;
457     return edge->flags;
458     }
459     }
460     dbg(0,"** not found\n");
461     return 0;
462     }
463    
464     static void
465     routech_resolve(struct map_rect *mr, struct item_id *from, struct item_id *to, int dir)
466     {
467     struct item_id middle_node;
468     int res;
469     if (dir)
470     res=routech_find_edge(mr, to, from, &middle_node);
471     else
472     res=routech_find_edge(mr, from, to, &middle_node);
473     dbg(1,"res=%d\n",res);
474     if (res & 4) {
475     routech_resolve(mr, from, &middle_node, 1);
476     routech_resolve(mr, &middle_node, to, 0);
477     } else
478     routech_resolve_route(mr, &middle_node, res, dir);
479     }
480    
481     static void
482     routech_find_path(struct map_rect *mr, struct routech_search *search)
483     {
484     struct item_id *curr_node=search->via;
485     GList *i,*n,*list=NULL;
486     dbg(1,"node %p\n",curr_node);
487     for (;;) {
488     int element=GPOINTER_TO_INT(g_hash_table_lookup(search->hash, curr_node));
489     struct item_id *next_node=pq_get_parent_node_id(search->pq,element);
490     if (search->dir)
491     list=g_list_append(list, curr_node);
492     else
493     list=g_list_prepend(list, curr_node);
494     dbg(1,"element %d\n",element);
495     dbg(1,"next node %p\n",next_node);
496     if (!next_node)
497     break;
498     curr_node=next_node;
499     }
500     i=list;
501     while (i && (n=g_list_next(i))) {
502     routech_resolve(mr, i->data, n->data, search->dir);
503     i=n;
504     }
505     }
506    
507     void
508     routech_test(struct navit *navit)
509     {
510     struct attr mapset;
511     struct coord src={0x3fd661,0x727146};
512     struct coord dst={0xfff07fc2,0x4754c9};
513     struct item_id id[2],*id_ptr;
514     struct routech_search *search[2],*curr,*opposite;
515     struct map *map[2];
516     struct map_rect *mr[2];
517     int element;
518     int k;
519     int search_id=0;
520     int i;
521    
522     navit_get_attr(navit, attr_mapset, &mapset, NULL);
523     routech_find_nearest(mapset.u.mapset, &src, &id[0], &map[0]);
524     routech_find_nearest(mapset.u.mapset, &dst, &id[1], &map[1]);
525     for (k = 0 ; k < 2 ; k++) {
526     profile(0,"start\n");
527     search[0]=routech_search_new(0);
528     search[1]=routech_search_new(1);
529     printf("Start 0x%x,0x%x\n",id[0].id_hi,id[0].id_lo);
530     printf("End 0x%x,0x%x\n",id[1].id_hi,id[1].id_lo);
531     id_ptr=&id[0];
532     element=routech_insert_node(search[0], &id_ptr, 0);
533     pq_set_parent(search[0]->pq, element, NULL, 0);
534    
535     id_ptr=&id[1];
536     element=routech_insert_node(search[1], &id_ptr, 0);
537     pq_set_parent(search[1]->pq, element, NULL, 0);
538    
539     mr[0]=map_rect_new(map[0], NULL);
540     mr[1]=map_rect_new(map[0], NULL);
541     for (i=0 ; i < 5000 ; i++) {
542     if (pq_is_empty(search[0]->pq) && pq_is_empty(search[1]->pq))
543     break;
544     if (!pq_is_empty(search[1-search_id]->pq)) {
545     search_id=1-search_id;
546     }
547     if (search[0]->finished)
548     search_id=1;
549     if (search[1]->finished)
550     search_id=0;
551     curr=search[search_id];
552     opposite=search[1-search_id];
553     if (pq_is_empty(curr->pq)) {
554     dbg(0,"empty\n");
555     break;
556     }
557     routech_relax(mr, curr, opposite);
558     if (pq_min(curr->pq) > curr->upper) {
559     dbg(0,"min %d upper %d\n",pq_min(curr->pq), curr->upper);
560     curr->finished=1;
561     }
562     if (curr->finished && opposite->finished) {
563     dbg(0,"finished\n");
564     break;
565     }
566     }
567    
568     printf("finished %d\n",search[0]->upper);
569     profile(0,"finished\n");
570     }
571     routefile=fopen("route.txt","w");
572     routech_find_path(mr[0], search[0]);
573     routech_find_path(mr[0], search[1]);
574     fclose(routefile);
575     printf("Heap size %d vs %d\n",search[0]->pq->size,search[1]->pq->size);
576     printf("Element size %d vs %d\n",search[0]->pq->elements_size,search[1]->pq->elements_size);
577    
578     }

   
Visit the ZANavi Wiki