16 struct item_id target,middle;
19 struct routech_search {
30 struct item_id *node_id;
31 struct item_id *parent_node_id;
37 struct pq_heap_element {
46 int elements_capacity;
49 struct pq_element *elements;
50 struct pq_heap_element *heap_elements;
56 struct pq *ret=g_new(struct pq, 1);
60 ret->elements_step=10;
61 ret->elements_capacity=0;
64 ret->heap_elements=NULL;
69 pq_insert(struct pq *pq, int key, struct item_id *node_id)
72 if (pq->size >= pq->capacity) {
73 pq->capacity += pq->step;
74 pq->heap_elements=g_renew(struct pq_heap_element, pq->heap_elements, pq->capacity);
76 if (pq->elements_size >= pq->elements_capacity) {
77 pq->elements_capacity += pq->elements_step;
78 pq->elements=g_renew(struct pq_element, pq->elements, pq->elements_capacity);
80 element=pq->elements_size++;
81 pq->elements[element].node_id=node_id;
83 while (i > 1 && pq->heap_elements[i/2].key > key) {
84 pq->heap_elements[i]=pq->heap_elements[i/2];
85 pq->elements[pq->heap_elements[i].element].heap_element=i;
88 pq->heap_elements[i].key=key;
89 pq->heap_elements[i].element=element;
90 pq->elements[element].heap_element=i;
91 pq->elements[element].key=key;
96 pq_get_key(struct pq *pq, int element, int *key)
98 *key=pq->elements[element].key;
103 pq_set_parent(struct pq *pq, int element, struct item_id *node_id, int stalled)
105 pq->elements[element].parent_node_id=node_id;
106 pq->elements[element].stalled=stalled;
109 static struct item_id *
110 pq_get_parent_node_id(struct pq *pq, int element)
112 return pq->elements[element].parent_node_id;
116 pq_set_stalled(struct pq *pq, int element, int stalled)
118 pq->elements[element].stalled=stalled;
122 pq_get_stalled(struct pq *pq, int element)
124 return pq->elements[element].stalled;
128 pq_is_deleted(struct pq *pq, int element)
130 return (pq->elements[element].heap_element == 0);
134 pq_min(struct pq *pq)
136 return (pq->heap_elements[1].key);
140 pq_decrease_key(struct pq *pq, int element, int key)
142 int i=pq->elements[element].heap_element;
143 while (i > 1 && pq->heap_elements[i/2].key > key) {
144 pq->heap_elements[i]=pq->heap_elements[i/2];
145 pq->elements[pq->heap_elements[i].element].heap_element=i;
148 pq->heap_elements[i].element=element;
149 pq->heap_elements[i].key=key;
150 pq->elements[element].heap_element=i;
151 pq->elements[element].key=key;
155 pq_delete_min(struct pq *pq, struct item_id **node_id, int *key, int *element)
157 struct pq_heap_element min, last;
161 min=pq->heap_elements[1];
163 *node_id=pq->elements[min.element].node_id;
167 *element=min.element;
168 pq->elements[min.element].heap_element=0;
170 last=pq->heap_elements[--pq->size];
171 while (i <= pq->size / 2) {
173 if (j < pq->size && pq->heap_elements[j].key > pq->heap_elements[j+1].key)
175 if (pq->heap_elements[j].key >= last.key)
177 pq->heap_elements[i]=pq->heap_elements[j];
178 pq->elements[pq->heap_elements[i].element].heap_element=i;
181 pq->heap_elements[i]=last;
182 pq->elements[last.element].heap_element=i;
187 pq_is_empty(struct pq *pq)
189 return pq->size <= 1;
193 pq_check(struct pq *pq)
196 for (i = 2 ; i < pq->size ; i++) {
197 if (pq->heap_elements[i/2].key > pq->heap_elements[i].key) {
198 printf("%d vs %d\n", pq->heap_elements[i/2].key, pq->heap_elements[i].key);
202 for (i = 1 ; i < pq->size ; i++) {
203 if (i != pq->elements[pq->heap_elements[i].element].heap_element) {
204 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);
209 struct routech_search *
210 routech_search_new(int dir)
212 struct routech_search *ret=g_new0(struct routech_search, 1);
214 ret->hash=g_hash_table_new_full(item_id_hash, item_id_equal, g_free, NULL);
222 routech_insert_node(struct routech_search *search, struct item_id **id, int val)
226 if (g_hash_table_lookup_extended(search->hash, *id, (gpointer)&ret, (gpointer)&e)) {
228 pq_get_key(search->pq, e, &oldval);
229 // printf("Node = %d\n",node);
231 pq_decrease_key(search->pq, e, val);
237 ret=g_new(struct item_id, 1);
239 e=pq_insert(search->pq, val, ret);
240 g_hash_table_insert(search->hash, ret, GINT_TO_POINTER(e));
247 routech_find_nearest(struct mapset *ms, struct coord *c, struct item_id *id, struct map **map_ret)
252 struct map_selection sel;
254 struct mapset_handle *msh;
259 sel.range.min=type_ch_node;
260 sel.range.max=type_ch_node;
261 sel.u.c_rect.lu.x=c->x-dst;
262 sel.u.c_rect.lu.y=c->y+dst;
263 sel.u.c_rect.rl.x=c->x+dst;
264 sel.u.c_rect.rl.y=c->y-dst;
265 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);
267 while ((map=mapset_next(msh, 1))) {
268 mr=map_rect_new(map, &sel);
271 while ((item=map_rect_get_item(mr))) {
273 if (item->type == type_ch_node && item_coord_get(item, &cn, 1)) {
274 int dist=transform_distance_sq(&cn, c);
277 id->id_hi=item->id_hi;
278 id->id_lo=item->id_lo;
284 map_rect_destroy(mr);
292 routech_edge_valid(struct ch_edge *edge, int dir)
294 if (edge->flags & (1 << dir))
300 routech_stall(struct map_rect *mr, struct routech_search *curr, struct item_id *id, int key)
302 struct stall_element {
308 struct attr edge_attr;
310 int index=GPOINTER_TO_INT(g_hash_table_lookup(curr->hash, id));
311 pq_set_stalled(curr->pq, index, key);
312 se=g_new(struct stall_element, 1);
315 list=g_list_append(list, se);
319 item=map_rect_get_item_byid(mr, se->id.id_hi, se->id.id_lo);
320 while (item_attr_get(item, attr_ch_edge, &edge_attr)) {
321 struct ch_edge *edge=edge_attr.u.data;
322 if (routech_edge_valid(edge, curr->dir)) {
323 int index=GPOINTER_TO_INT(g_hash_table_lookup(curr->hash, &edge->target));
325 int newkey=key+edge->weight;
327 pq_get_key(curr->pq, index, &target_key);
328 if (newkey < target_key) {
329 if (!pq_get_stalled(curr->pq, index)) {
330 pq_set_stalled(curr->pq, index, newkey);
331 se=g_new(struct stall_element, 1);
334 list=g_list_append(list, se);
340 list=g_list_remove(list, se);
346 routech_relax(struct map_rect **mr, struct routech_search *curr, struct routech_search *opposite)
351 struct attr edge_attr;
353 if (!pq_delete_min(curr->pq, &id, &val, &element)) {
357 int opposite_element=GPOINTER_TO_INT(g_hash_table_lookup(opposite->hash, id));
358 if (opposite_element && pq_is_deleted(opposite->pq, opposite_element)) {
360 pq_get_key(opposite->pq, opposite_element, &opposite_val);
361 if (val+opposite_val < curr->upper) {
362 curr->upper=opposite->upper=val+opposite_val;
363 printf("%d path found: 0x%x,0x%x ub = %d\n",curr->dir,id->id_hi,id->id_lo,curr->upper);
364 curr->via=opposite->via=id;
367 if (pq_get_stalled(curr->pq, element))
369 item=map_rect_get_item_byid(mr[0], id->id_hi, id->id_lo);
370 while (item_attr_get(item, attr_ch_edge, &edge_attr)) {
371 struct ch_edge *edge=edge_attr.u.data;
372 struct item_id *target_id=&edge->target;
373 if (routech_edge_valid(edge, curr->dir)) {
374 int index=GPOINTER_TO_INT(g_hash_table_lookup(curr->hash, target_id));
375 if (index && routech_edge_valid(edge, 1-curr->dir)) {
377 stallkey=pq_get_stalled(curr->pq, index);
381 pq_get_key(curr->pq, index, &newkey);
382 newkey+=edge->weight;
384 routech_stall(mr[1], curr, id, newkey);
388 int element=routech_insert_node(curr, &target_id, edge->weight+val);
390 pq_set_parent(curr->pq, element, id, 0);
397 routech_print_coord(struct coord *c, FILE *out)
411 fprintf(out,"%s0x%x %s0x%x\n",sx,x,sy,y);
415 routech_resolve_route(struct map_rect *mr, struct item_id *id, int flags, int dir)
417 int i,count,max=16384;
419 if (!(flags & 8) == dir)
421 struct coord ca[max];
422 struct item *item=map_rect_get_item_byid(mr, id->id_hi, id->id_lo);
423 dbg_assert(item->type >= type_line && item->type < type_area);
424 item->type=type_street_route;
426 count=item_coord_get(item, ca, max);
427 item_dump_attr(item, item->map, routefile);
428 fprintf(routefile,"debug=\"flags=%d dir=%d rev=%d\"",flags,dir,rev);
429 fprintf(routefile,"\n");
431 for (i = count-1 ; i >= 0 ; i--)
432 routech_print_coord(&ca[i], routefile);
434 for (i = 0 ; i < count ; i++)
435 routech_print_coord(&ca[i], routefile);
440 routech_find_edge(struct map_rect *mr, struct item_id *from, struct item_id *to, struct item_id *middle)
442 struct item *item=map_rect_get_item_byid(mr, from->id_hi, from->id_lo);
443 struct attr edge_attr;
444 dbg_assert(item->type == type_ch_node);
445 dbg(1,"type %s\n",item_to_name(item->type));
446 dbg(1,"segment item=%p\n",item);
447 while (item_attr_get(item, attr_ch_edge, &edge_attr)) {
448 struct ch_edge *edge=edge_attr.u.data;
449 dbg(1,"flags=%d\n",edge->flags);
450 if (edge->target.id_hi == to->id_hi && edge->target.id_lo == to->id_lo) {
451 *middle=edge->middle;
455 dbg(0,"** not found\n");
460 routech_resolve(struct map_rect *mr, struct item_id *from, struct item_id *to, int dir)
462 struct item_id middle_node;
465 res=routech_find_edge(mr, to, from, &middle_node);
467 res=routech_find_edge(mr, from, to, &middle_node);
468 dbg(1,"res=%d\n",res);
470 routech_resolve(mr, from, &middle_node, 1);
471 routech_resolve(mr, &middle_node, to, 0);
473 routech_resolve_route(mr, &middle_node, res, dir);
477 routech_find_path(struct map_rect *mr, struct routech_search *search)
479 struct item_id *curr_node=search->via;
480 GList *i,*n,*list=NULL;
481 dbg(1,"node %p\n",curr_node);
483 int element=GPOINTER_TO_INT(g_hash_table_lookup(search->hash, curr_node));
484 struct item_id *next_node=pq_get_parent_node_id(search->pq,element);
486 list=g_list_append(list, curr_node);
488 list=g_list_prepend(list, curr_node);
489 dbg(1,"element %d\n",element);
490 dbg(1,"next node %p\n",next_node);
496 while (i && (n=g_list_next(i))) {
497 routech_resolve(mr, i->data, n->data, search->dir);
503 routech_test(struct navit *navit)
506 struct coord src={0x3fd661,0x727146};
507 struct coord dst={0xfff07fc2,0x4754c9};
508 struct item_id id[2],*id_ptr;
509 struct routech_search *search[2],*curr,*opposite;
511 struct map_rect *mr[2];
515 navit_get_attr(navit, attr_mapset, &mapset, NULL);
516 routech_find_nearest(mapset.u.mapset, &src, &id[0], &map[0]);
517 routech_find_nearest(mapset.u.mapset, &dst, &id[1], &map[1]);
518 for (k = 0 ; k < 2 ; k++) {
519 profile(0,"start\n");
520 search[0]=routech_search_new(0);
521 search[1]=routech_search_new(1);
522 printf("Start 0x%x,0x%x\n",id[0].id_hi,id[0].id_lo);
523 printf("End 0x%x,0x%x\n",id[1].id_hi,id[1].id_lo);
525 element=routech_insert_node(search[0], &id_ptr, 0);
526 pq_set_parent(search[0]->pq, element, NULL, 0);
529 element=routech_insert_node(search[1], &id_ptr, 0);
530 pq_set_parent(search[1]->pq, element, NULL, 0);
532 mr[0]=map_rect_new(map[0], NULL);
533 mr[1]=map_rect_new(map[0], NULL);
536 for (i=0 ; i < 5000 ; i++) {
537 if (pq_is_empty(search[0]->pq) && pq_is_empty(search[1]->pq))
539 if (!pq_is_empty(search[1-search_id]->pq)) {
540 search_id=1-search_id;
542 if (search[0]->finished)
544 if (search[1]->finished)
546 curr=search[search_id];
547 opposite=search[1-search_id];
548 if (pq_is_empty(curr->pq)) {
552 routech_relax(mr, curr, opposite);
553 if (pq_min(curr->pq) > curr->upper) {
554 dbg(0,"min %d upper %d\n",pq_min(curr->pq), curr->upper);
557 if (curr->finished && opposite->finished) {
563 printf("finished %d\n",search[0]->upper);
564 profile(0,"finished\n");
566 routefile=fopen("route.txt","w");
567 routech_find_path(mr[0], search[0]);
568 routech_find_path(mr[0], search[1]);
570 printf("Heap size %d vs %d\n",search[0]->pq->size,search[1]->pq->size);
571 printf("Element size %d vs %d\n",search[0]->pq->elements_size,search[1]->pq->elements_size);