git.maemo.org Git - navit-package/blob - navit/routech.c

   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 struct ch_edge {
  14         int flags;
  15         int weight;
  16         struct item_id target,middle;
  17 };
  18
  19 struct routech_search {
  20         struct pq *pq;
  21         GHashTable *hash;
  22         int finished;
  23         int dir;
  24         unsigned int upper;
  25         struct item_id *via;
  26 };
  27
  28
  29 struct pq_element {
  30         struct item_id *node_id;
  31         struct item_id *parent_node_id;
  32         int stalled;
  33         int key;
  34         int heap_element;
  35 };
  36
  37 struct pq_heap_element {
  38         int key;
  39         int element;
  40 };
  41
  42 struct pq {
  43         int capacity;
  44         int size;
  45         int step;
  46         int elements_capacity;
  47         int elements_size;
  48         int elements_step;
  49         struct pq_element *elements;
  50         struct pq_heap_element *heap_elements;
  51 };
  52
  53 static struct pq *
  54 pq_new(void)
  55 {
  56         struct pq *ret=g_new(struct pq, 1);
  57         ret->step=10;
  58         ret->capacity=0;
  59         ret->size=1;
  60         ret->elements_step=10;
  61         ret->elements_capacity=0;
  62         ret->elements_size=1;
  63         ret->elements=NULL;
  64         ret->heap_elements=NULL;
  65         return ret;
  66 }
  67
  68 static int
  69 pq_insert(struct pq *pq, int key, struct item_id *node_id)
  70 {
  71         int element,i;
  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);
  75         }
  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);
  79         }
  80         element=pq->elements_size++;
  81         pq->elements[element].node_id=node_id;
  82         i=pq->size++;
  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;
  86                 i/=2;
  87         }
  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;
  92         return element;
  93 }
  94
  95 static int
  96 pq_get_key(struct pq *pq, int element, int *key)
  97 {
  98         *key=pq->elements[element].key;
  99         return 1;
 100 }
 101
 102 static void
 103 pq_set_parent(struct pq *pq, int element, struct item_id *node_id, int stalled)
 104 {
 105         pq->elements[element].parent_node_id=node_id;
 106         pq->elements[element].stalled=stalled;
 107 }
 108
 109 static struct item_id *
 110 pq_get_parent_node_id(struct pq *pq, int element)
 111 {
 112         return pq->elements[element].parent_node_id;
 113 }
 114
 115 static void
 116 pq_set_stalled(struct pq *pq, int element, int stalled)
 117 {
 118         pq->elements[element].stalled=stalled;
 119 }
 120
 121 static int
 122 pq_get_stalled(struct pq *pq, int element)
 123 {
 124         return pq->elements[element].stalled;
 125 }
 126
 127 static int
 128 pq_is_deleted(struct pq *pq, int element)
 129 {
 130         return (pq->elements[element].heap_element == 0);
 131 }
 132
 133 static int
 134 pq_min(struct pq *pq)
 135 {
 136         return (pq->heap_elements[1].key);
 137 }
 138
 139 static void
 140 pq_decrease_key(struct pq *pq, int element, int key)
 141 {
 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;
 146                 i/=2;
 147         }
 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;
 152 }
 153
 154 static int
 155 pq_delete_min(struct pq *pq, struct item_id **node_id, int *key, int *element)
 156 {
 157         struct pq_heap_element min, last;
 158         int i=1,j;
 159         if (pq->size <= 1)
 160                 return 0;
 161         min=pq->heap_elements[1];
 162         if (node_id)
 163                 *node_id=pq->elements[min.element].node_id;
 164         if (key)
 165                 *key=min.key;
 166         if (element)
 167                 *element=min.element;
 168         pq->elements[min.element].heap_element=0;
 169         min.element=0;
 170         last=pq->heap_elements[--pq->size];
 171         while (i <= pq->size / 2) {
 172                 j=2*i;
 173                 if (j < pq->size && pq->heap_elements[j].key > pq->heap_elements[j+1].key)
 174                         j++;
 175                 if (pq->heap_elements[j].key >= last.key)
 176                         break;
 177                 pq->heap_elements[i]=pq->heap_elements[j];
 178                 pq->elements[pq->heap_elements[i].element].heap_element=i;
 179                 i=j;
 180         }
 181         pq->heap_elements[i]=last;
 182         pq->elements[last.element].heap_element=i;
 183         return 1;
 184 }
 185
 186 static int
 187 pq_is_empty(struct pq *pq)
 188 {
 189         return pq->size <= 1;
 190 }
 191
 192 static void
 193 pq_check(struct pq *pq)
 194 {
 195         int i;
 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);
 199                         return;
 200                 }
 201         }
 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);
 205                 }
 206         }
 207 }
 208
 209 struct routech_search *
 210 routech_search_new(int dir)
 211 {
 212         struct routech_search *ret=g_new0(struct routech_search, 1);
 213         ret->pq=pq_new();
 214         ret->hash=g_hash_table_new_full(item_id_hash, item_id_equal, g_free, NULL);
 215         ret->upper=UINT_MAX;
 216         ret->dir=dir;
 217
 218         return ret;
 219 }
 220
 221 static int
 222 routech_insert_node(struct routech_search *search, struct item_id **id, int val)
 223 {
 224         struct item_id *ret;
 225         int e;
 226         if (g_hash_table_lookup_extended(search->hash, *id, (gpointer)&ret, (gpointer)&e)) {
 227                 int oldval;
 228                 pq_get_key(search->pq, e, &oldval);
 229                 // printf("Node = %d\n",node);
 230                 if (oldval > val) {
 231                         pq_decrease_key(search->pq, e, val);
 232                         *id=ret;
 233                         return e;
 234                 }
 235                 return 0;
 236         }
 237         ret=g_new(struct item_id, 1);
 238         *ret=**id;
 239         e=pq_insert(search->pq, val, ret);
 240         g_hash_table_insert(search->hash, ret, GINT_TO_POINTER(e));
 241         *id=ret;
 242         return e;
 243 }
 244
 245
 246 static int
 247 routech_find_nearest(struct mapset *ms, struct coord *c, struct item_id *id, struct map **map_ret)
 248 {
 249         int dst=50;
 250         int dstsq=dst*dst;
 251         int ret=0;
 252         struct map_selection sel;
 253         struct map_rect *mr;
 254         struct mapset_handle *msh;
 255         struct map *map;
 256         struct item *item;
 257         sel.next=NULL;
 258         sel.order=18;
 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);
 266         msh=mapset_open(ms);
 267         while ((map=mapset_next(msh, 1))) {
 268                 mr=map_rect_new(map, &sel);
 269                 if (!mr)
 270                         continue;
 271                 while ((item=map_rect_get_item(mr))) {
 272                         struct coord cn;
 273                         if (item->type == type_ch_node && item_coord_get(item, &cn, 1)) {
 274                                 int dist=transform_distance_sq(&cn, c);
 275                                 if (dist < dstsq) {
 276                                         dstsq=dist;
 277                                         id->id_hi=item->id_hi;
 278                                         id->id_lo=item->id_lo;
 279                                         *map_ret=map;
 280                                         ret=1;
 281                                 }
 282                         }
 283                 }
 284                 map_rect_destroy(mr);
 285         }
 286         mapset_close(msh);
 287         dbg_assert(ret==1);
 288         return ret;
 289 }
 290
 291 static int
 292 routech_edge_valid(struct ch_edge *edge, int dir)
 293 {
 294         if (edge->flags & (1 << dir))
 295                 return 1;
 296         return 0;
 297 }
 298
 299 static void
 300 routech_stall(struct map_rect *mr, struct routech_search *curr, struct item_id *id, int key)
 301 {
 302         struct stall_element {
 303                 struct item_id id;
 304                 int key;
 305         } *se;
 306         GList *list=NULL;
 307         struct item *item;
 308         struct attr edge_attr;
 309
 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);
 313         se->id=*id;
 314         se->key=key;
 315         list=g_list_append(list, se);
 316         while (list) {
 317                 se=list->data;
 318                 key=se->key;
 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));
 324                                 if (index) {
 325                                         int newkey=key+edge->weight;
 326                                         int target_key;
 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);
 332                                                         se->id=edge->target;
 333                                                         se->key=newkey;
 334                                                         list=g_list_append(list, se);
 335                                                 }
 336                                         }
 337                                 }
 338                         }
 339                 }
 340                 list=g_list_remove(list, se);
 341                 g_free(se);
 342         }
 343 }
 344
 345 static void
 346 routech_relax(struct map_rect **mr, struct routech_search *curr, struct routech_search *opposite)
 347 {
 348         int val,element;
 349         struct item_id *id;
 350         struct item *item;
 351         struct attr edge_attr;
 352
 353         if (!pq_delete_min(curr->pq, &id, &val, &element)) {
 354                 return;
 355         }
 356         pq_check(curr->pq);
 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)) {
 359                 int opposite_val;
 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;
 365                 }
 366         }
 367         if (pq_get_stalled(curr->pq, element))
 368                 return;
 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)) {
 376                                 int newkey,stallkey;
 377                                 stallkey=pq_get_stalled(curr->pq, index);
 378                                 if (stallkey)
 379                                         newkey=stallkey;
 380                                 else
 381                                         pq_get_key(curr->pq, index, &newkey);
 382                                 newkey+=edge->weight;
 383                                 if (newkey < val) {
 384                                         routech_stall(mr[1], curr, id, newkey);
 385                                         return;
 386                                 }
 387                         }
 388                         int element=routech_insert_node(curr, &target_id, edge->weight+val);
 389                         if (element) {
 390                                 pq_set_parent(curr->pq, element, id, 0);
 391                         }
 392                 }
 393         }
 394 }
 395
 396 void
 397 routech_print_coord(struct coord *c, FILE *out)
 398 {
 399         int x=c->x;
 400         int y=c->y;
 401         char *sx="";
 402         char *sy="";
 403         if (x < 0) {
 404                 sx="-";
 405                 x=-x;
 406         }
 407         if (y < 0) {
 408                 sy="-";
 409                 y=-y;
 410         }
 411         fprintf(out,"%s0x%x %s0x%x\n",sx,x,sy,y);
 412 }
 413
 414 void
 415 routech_resolve_route(struct map_rect *mr, struct item_id *id, int flags, int dir)
 416 {
 417         int i,count,max=16384;
 418         int rev=0;
 419         if (!(flags & 8) == dir)
 420                 rev=1;
 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;
 425
 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");
 430         if (rev) {
 431                 for (i = count-1 ; i >= 0 ; i--)
 432                         routech_print_coord(&ca[i], routefile);
 433         } else {
 434                 for (i = 0 ; i < count ; i++)
 435                         routech_print_coord(&ca[i], routefile);
 436         }
 437 }
 438
 439 int
 440 routech_find_edge(struct map_rect *mr, struct item_id *from, struct item_id *to, struct item_id *middle)
 441 {
 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;
 452                         return edge->flags;
 453                 }
 454         }
 455         dbg(0,"** not found\n");
 456         return 0;
 457 }
 458
 459 void
 460 routech_resolve(struct map_rect *mr, struct item_id *from, struct item_id *to, int dir)
 461 {
 462         struct item_id middle_node;
 463         int res;
 464         if (dir)
 465                 res=routech_find_edge(mr, to, from, &middle_node);
 466         else
 467                 res=routech_find_edge(mr, from, to, &middle_node);
 468         dbg(1,"res=%d\n",res);
 469         if (res & 4) {
 470                 routech_resolve(mr, from, &middle_node, 1);
 471                 routech_resolve(mr, &middle_node, to, 0);
 472         } else
 473                 routech_resolve_route(mr, &middle_node, res, dir);
 474 }
 475
 476 void
 477 routech_find_path(struct map_rect *mr, struct routech_search *search)
 478 {
 479         struct item_id *curr_node=search->via;
 480         GList *i,*n,*list=NULL;
 481         dbg(1,"node %p\n",curr_node);
 482         for (;;) {
 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);
 485                 if (search->dir)
 486                         list=g_list_append(list, curr_node);
 487                 else
 488                         list=g_list_prepend(list, curr_node);
 489                 dbg(1,"element %d\n",element);
 490                 dbg(1,"next node %p\n",next_node);
 491                 if (!next_node)
 492                         break;
 493                 curr_node=next_node;
 494         }
 495         i=list;
 496         while (i && (n=g_list_next(i))) {
 497                 routech_resolve(mr, i->data, n->data, search->dir);
 498                 i=n;
 499         }
 500 }
 501
 502 void
 503 routech_test(struct navit *navit)
 504 {
 505         struct attr mapset;
 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;
 510         struct map *map[2];
 511         struct map_rect *mr[2];
 512         int element;
 513         int k;
 514
 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);
 524         id_ptr=&id[0];
 525         element=routech_insert_node(search[0], &id_ptr, 0);
 526         pq_set_parent(search[0]->pq, element, NULL, 0);
 527
 528         id_ptr=&id[1];
 529         element=routech_insert_node(search[1], &id_ptr, 0);
 530         pq_set_parent(search[1]->pq, element, NULL, 0);
 531
 532         mr[0]=map_rect_new(map[0], NULL);
 533         mr[1]=map_rect_new(map[0], NULL);
 534         int search_id=0;
 535         int i;
 536         for (i=0 ; i < 5000 ; i++) {
 537                 if (pq_is_empty(search[0]->pq) && pq_is_empty(search[1]->pq))
 538                         break;
 539                 if (!pq_is_empty(search[1-search_id]->pq)) {
 540                         search_id=1-search_id;
 541                 }
 542                 if (search[0]->finished)
 543                         search_id=1;
 544                 if (search[1]->finished)
 545                         search_id=0;
 546                 curr=search[search_id];
 547                 opposite=search[1-search_id];
 548                 if (pq_is_empty(curr->pq)) {
 549                         dbg(0,"empty\n");
 550                         break;
 551                 }
 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);
 555                         curr->finished=1;
 556                 }
 557                 if (curr->finished && opposite->finished) {
 558                         dbg(0,"finished\n");
 559                         break;
 560                 }
 561         }
 562
 563         printf("finished %d\n",search[0]->upper);
 564         profile(0,"finished\n");
 565         }
 566         routefile=fopen("route.txt","w");
 567         routech_find_path(mr[0], search[0]);
 568         routech_find_path(mr[0], search[1]);
 569         fclose(routefile);
 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);
 572
 573 }