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

Contents of /navit/navit/routech.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 31 - (show annotations) (download)
Mon Feb 4 17:41:59 2013 UTC (6 years, 11 months ago) by zoff99
File MIME type: text/plain
File size: 14299 byte(s)
new map version, lots of fixes and experimental new features
1 #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 ret->hash=g_hash_table_new_full(item_id_hash, item_id_equal, g_free_func, NULL);
217 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