vcs.maemo.org Git - navit-package/blob - src/route.c

   1 #include <stdio.h>
   2 #include <string.h>
   3 #if 0
   4 #include <math.h>
   5 #include <assert.h>
   6 #include <unistd.h>
   7 #include <sys/time.h>
   8 #endif
   9 #include <glib.h>
  10 #include "config.h"
  11 #include "debug.h"
  12 #include "profile.h"
  13 #include "coord.h"
  14 #include "projection.h"
  15 #include "map.h"
  16 #include "mapset.h"
  17 #include "item.h"
  18 #include "route.h"
  19 #include "track.h"
  20 #include "point.h"
  21 #include "graphics.h"
  22 #include "transform.h"
  23 #include "fib.h"
  24
  25 #if 0
  26 static int speed_list[]={
  27         10, /* street_0 */
  28         10, /* street_1_city */
  29         30, /* street_2_city */
  30         40, /* street_3_city */
  31         50, /* street_4_city */
  32         80, /* highway_city */
  33         60, /* street_1_land */
  34         65, /* street_2_land */
  35         70, /* street_3_land */
  36         80, /* street_4_land */
  37         120, /* street_n_lanes */
  38         120, /* highway_land */
  39         40, /* ramp */
  40         30, /* ferry */
  41 };
  42 #endif
  43
  44 int debug_route=0;
  45
  46
  47 struct route_graph_point {
  48         struct route_graph_point *next;
  49         struct route_graph_point *hash_next;
  50         struct route_graph_segment *start;
  51         struct route_graph_segment *end;
  52         struct route_graph_segment *seg;
  53         struct fibheap_el *el;
  54         int value;
  55         struct coord c;
  56 };
  57
  58
  59 struct route_graph_segment {
  60         struct route_graph_segment *next;
  61         struct route_graph_segment *start_next;
  62         struct route_graph_segment *end_next;
  63         struct route_graph_point *start;
  64         struct route_graph_point *end;
  65         struct item item;
  66         int limit;
  67         int len;
  68 };
  69
  70 struct route_path_segment {
  71         struct item item;
  72         int time;
  73         int length;
  74         struct coord c[2];
  75         struct route_path_segment *next;
  76 };
  77
  78
  79 struct street_data {
  80         struct item item;
  81         int count;
  82         int limit;
  83         struct coord c[0];
  84 };
  85
  86 struct route_info {
  87         struct coord c;
  88         struct coord lp;
  89         int pos;
  90
  91         int dist;
  92         int dir;
  93
  94         struct street_data *street;
  95 };
  96
  97 struct route_path {
  98         struct route_path_segment *path;
  99         struct route_path_segment *path_last;
 100         struct item_hash *path_hash;
 101 };
 102
 103
 104 struct route {
 105         int version;
 106         struct mapset *ms;
 107         struct route_info *pos;
 108         struct route_info *dst;
 109
 110         struct route_graph *graph;
 111         struct route_path *path2;
 112         int speedlist[route_item_last-route_item_first+1];
 113 };
 114
 115 struct route_graph {
 116         struct route_graph_point *route_points;
 117         struct route_graph_segment *route_segments;
 118 #define HASH_SIZE 8192
 119         struct route_graph_point *hash[HASH_SIZE];
 120 };
 121
 122 static struct route_info * route_find_nearest_street(struct mapset *ms, struct coord *c);
 123 static struct route_graph_point *route_graph_get_point(struct route_graph *this, struct coord *c);
 124 static void route_graph_update(struct route *this);
 125 static struct route_path *route_path_new(struct route_graph *this, struct route_info *pos, struct route_info *dst, int *speedlist);
 126 static void route_process_street_graph(struct route_graph *this, struct item *item);
 127 static void route_graph_destroy(struct route_graph *this);
 128 static void route_path_update(struct route *this);
 129
 130
 131 static void
 132 route_path_destroy(struct route_path *this)
 133 {
 134         struct route_path_segment *c,*n;
 135         if (! this)
 136                 return;
 137         if (this->path_hash) {
 138                 item_hash_destroy(this->path_hash);
 139                 this->path_hash=NULL;
 140         }
 141         c=this->path;
 142         while (c) {
 143                 n=c->next;
 144                 g_free(c);
 145                 c=n;
 146         }
 147         g_free(this);
 148 }
 149
 150 struct route *
 151 route_new(struct mapset *ms)
 152 {
 153         struct route *this=g_new0(struct route, 1);
 154         this->ms=ms;
 155         return this;
 156 }
 157
 158 void
 159 route_set_mapset(struct route *this, struct mapset *ms)
 160 {
 161         this->ms=ms;
 162 }
 163
 164 struct mapset *
 165 route_get_mapset(struct route *this)
 166 {
 167         return this->ms;
 168 }
 169
 170 struct route_info *
 171 route_get_pos(struct route *this)
 172 {
 173         return this->pos;
 174 }
 175
 176 struct route_info *
 177 route_get_dst(struct route *this)
 178 {
 179         return this->dst;
 180 }
 181
 182 int *
 183 route_get_speedlist(struct route *this)
 184 {
 185         return this->speedlist;
 186 }
 187
 188 int
 189 route_set_speed(struct route *this, enum item_type type, int value)
 190 {
 191         if (type < route_item_first || type > route_item_last) {
 192                 dbg(0,"street type %d out of range [%d,%d]", type, route_item_first, route_item_last);
 193                 return 0;
 194         }
 195         this->speedlist[type-route_item_first]=value;
 196         return 1;
 197 }
 198
 199 int
 200 route_contains(struct route *this, struct item *item)
 201 {
 202         if (! this->path2 || !this->path2->path_hash || !item_hash_lookup(this->path2->path_hash, item))
 203                 return 0;
 204         return 1;
 205 }
 206
 207 static void
 208 route_path_update(struct route *this)
 209 {
 210         route_path_destroy(this->path2);
 211         if (! this->pos || ! this->dst)
 212                 return;
 213         if (! this->graph || !(this->path2=route_path_new(this->graph, this->pos, this->dst, this->speedlist))) {
 214                 profile(0,NULL);
 215                 route_graph_update(this);
 216                 this->path2=route_path_new(this->graph, this->pos, this->dst, this->speedlist);
 217                 profile(1,"route_path_new");
 218                 profile(0,"end");
 219         }
 220 }
 221
 222 void
 223 route_set_position(struct route *this, struct coord *pos)
 224 {
 225         if (this->pos)
 226                 route_info_free(this->pos);
 227         this->pos=route_find_nearest_street(this->ms, pos);
 228         dbg(1,"this->pos=%p\n", this->pos);
 229         if (! this->pos)
 230                 return;
 231         if (this->dst)
 232                 route_path_update(this);
 233 }
 234
 235 void
 236 route_set_position_from_tracking(struct route *this, struct tracking *tracking)
 237 {
 238         struct coord *c;
 239         struct route_info *ret;
 240
 241         dbg(2,"enter\n");
 242         c=tracking_get_pos(tracking);
 243         ret=g_new0(struct route_info, 1);
 244         if (this->pos)
 245                 route_info_free(this->pos);
 246         ret->c=*c;
 247         ret->lp=*c;
 248         ret->pos=tracking_get_segment_pos(tracking);
 249         ret->dist=0;
 250         ret->dir=0;
 251         ret->street=street_data_dup(tracking_get_street_data(tracking));
 252         dbg(3,"c->x=0x%x, c->y=0x%x pos=%d item=(0x%x,0x%x)\n", c->x, c->y, ret->pos, ret->street->item.id_hi, ret->street->item.id_lo);
 253         dbg(3,"street 0=(0x%x,0x%x) %d=(0x%x,0x%x)\n", ret->street->c[0].x, ret->street->c[0].y, ret->street->count-1, ret->street->c[ret->street->count-1].x, ret->street->c[ret->street->count-1].y);
 254         this->pos=ret;
 255         if (this->dst)
 256                 route_path_update(this);
 257         dbg(2,"ret\n");
 258 }
 259
 260 struct map_selection *route_selection;
 261
 262 struct map_selection *
 263 route_rect(int order, struct coord *c1, struct coord *c2, int rel, int abs)
 264 {
 265         int dx,dy,sx=1,sy=1,d,m;
 266         struct map_selection *sel=g_new(struct map_selection, 1);
 267         sel->order[layer_town]=0;
 268         sel->order[layer_poly]=0;
 269         sel->order[layer_street]=order;
 270         dbg(1,"%p %p\n", c1, c2);
 271         dx=c1->x-c2->x;
 272         dy=c1->y-c2->y;
 273         if (dx < 0) {
 274                 sx=-1;
 275                 sel->rect.lu.x=c1->x;
 276                 sel->rect.rl.x=c2->x;
 277         } else {
 278                 sel->rect.lu.x=c2->x;
 279                 sel->rect.rl.x=c1->x;
 280         }
 281         if (dy < 0) {
 282                 sy=-1;
 283                 sel->rect.lu.y=c2->y;
 284                 sel->rect.rl.y=c1->y;
 285         } else {
 286                 sel->rect.lu.y=c1->y;
 287                 sel->rect.rl.y=c2->y;
 288         }
 289         if (dx*sx > dy*sy)
 290                 d=dx*sx;
 291         else
 292                 d=dy*sy;
 293         m=d*rel/100+abs;
 294         sel->rect.lu.x-=m;
 295         sel->rect.rl.x+=m;
 296         sel->rect.lu.y+=m;
 297         sel->rect.rl.y-=m;
 298         sel->next=NULL;
 299         return sel;
 300 }
 301
 302 static struct map_selection *
 303 route_calc_selection(struct coord *c1, struct coord *c2)
 304 {
 305         struct map_selection *ret,*sel;
 306         sel=route_rect(4, c1, c2, 25, 0);
 307         ret=sel;
 308         sel->next=route_rect(8, c1, c1, 0, 40000);
 309         sel=sel->next;
 310         sel->next=route_rect(18, c1, c1, 0, 10000);
 311         sel=sel->next;
 312         sel->next=route_rect(8, c2, c2, 0, 40000);
 313         sel=sel->next;
 314         sel->next=route_rect(18, c2, c2, 0, 10000);
 315         /* route_selection=ret; */
 316         return ret;
 317 }
 318
 319 static void
 320 route_free_selection(struct map_selection *sel)
 321 {
 322         struct map_selection *next;
 323         while (sel) {
 324                 next=sel->next;
 325                 g_free(sel);
 326                 sel=next;
 327         }
 328 }
 329
 330
 331
 332 void
 333 route_set_destination(struct route *this, struct coord *dst)
 334 {
 335         profile(0,NULL);
 336         if (this->dst)
 337                 route_info_free(this->dst);
 338         this->dst=NULL;
 339         if (dst)
 340                 this->dst=route_find_nearest_street(this->ms, dst);
 341         profile(1,"find_nearest_street");
 342
 343         route_graph_destroy(this->graph);
 344         this->graph=NULL;
 345         route_path_update(this);
 346         profile(0,"end");
 347 }
 348
 349 static struct route_graph_point *
 350 route_graph_get_point(struct route_graph *this, struct coord *c)
 351 {
 352         struct route_graph_point *p=this->route_points;
 353         int hashval=(c->x +  c->y) & (HASH_SIZE-1);
 354         p=this->hash[hashval];
 355         while (p) {
 356                 if (p->c.x == c->x && p->c.y == c->y)
 357                         return p;
 358                 p=p->hash_next;
 359         }
 360         return NULL;
 361 }
 362
 363
 364 static struct route_graph_point *
 365 route_graph_add_point(struct route_graph *this, struct coord *f)
 366 {
 367         int hashval;
 368         struct route_graph_point *p;
 369
 370         p=route_graph_get_point(this,f);
 371         if (!p) {
 372                 hashval=(f->x +  f->y) & (HASH_SIZE-1);
 373                 if (debug_route)
 374                         printf("p (0x%x,0x%x)\n", f->x, f->y);
 375                 p=g_new(struct route_graph_point,1);
 376                 p->hash_next=this->hash[hashval];
 377                 this->hash[hashval]=p;
 378                 p->next=this->route_points;
 379                 p->el=NULL;
 380                 p->start=NULL;
 381                 p->end=NULL;
 382                 p->seg=NULL;
 383                 p->value=INT_MAX;
 384                 p->c=*f;
 385                 this->route_points=p;
 386         }
 387         return p;
 388 }
 389
 390
 391 static void
 392 route_graph_free_points(struct route_graph *this)
 393 {
 394         struct route_graph_point *curr,*next;
 395         curr=this->route_points;
 396         while (curr) {
 397                 next=curr->next;
 398                 g_free(curr);
 399                 curr=next;
 400         }
 401         this->route_points=NULL;
 402         memset(this->hash, 0, sizeof(this->hash));
 403 }
 404
 405 static void
 406 route_graph_add_segment(struct route_graph *this, struct route_graph_point *start, struct route_graph_point *end, int len, struct item *item, int limit)
 407 {
 408         struct route_graph_segment *s;
 409         s=g_new0(struct route_graph_segment,1);
 410         s->start=start;
 411         s->start_next=start->start;
 412         start->start=s;
 413         s->end=end;
 414         s->end_next=end->end;
 415         end->end=s;
 416         g_assert(len >= 0);
 417         s->len=len;
 418         s->item=*item;
 419         s->limit=limit;
 420         s->next=this->route_segments;
 421         this->route_segments=s;
 422         if (debug_route)
 423                 printf("l (0x%x,0x%x)-(0x%x,0x%x)\n", start->c.x, start->c.y, end->c.x, end->c.y);
 424
 425 }
 426
 427 static void
 428 route_path_add_item(struct route_path *this, struct item *itm, struct coord *start, struct coord *end, int len, int time)
 429 {
 430         struct route_path_segment *segment=g_new0(struct route_path_segment,1);
 431         item_hash_insert(this->path_hash,  itm, (void *)1);
 432         segment->item=*itm;
 433         segment->next=NULL;
 434         segment->length=len;
 435         segment->time=time;
 436         if (start)
 437                 segment->c[0]=*start;
 438         if (end)
 439                 segment->c[1]=*end;
 440         if (!this->path)
 441                 this->path=segment;
 442         if (this->path_last)
 443                 this->path_last->next=segment;
 444         this->path_last=segment;
 445 }
 446
 447
 448 struct route_path_handle {
 449         struct route_path_segment *s;
 450 };
 451
 452 struct route_path_handle *
 453 route_path_open(struct route *this)
 454 {
 455         struct route_path_handle *ret=g_new(struct route_path_handle, 1);
 456
 457         ret->s=this->path2->path;
 458         return ret;
 459 }
 460
 461 struct route_path_segment *
 462 route_path_get_segment(struct route_path_handle *h)
 463 {
 464         struct route_path_segment *ret=h->s;
 465
 466         if (ret)
 467                 h->s=ret->next;
 468
 469         return ret;
 470 }
 471
 472 struct coord *
 473 route_path_segment_get_start(struct route_path_segment *s)
 474 {
 475         return &s->c[0];
 476 }
 477
 478 struct coord *
 479 route_path_segment_get_end(struct route_path_segment *s)
 480 {
 481         return &s->c[1];
 482 }
 483
 484 struct item *
 485 route_path_segment_get_item(struct route_path_segment *s)
 486 {
 487         return &s->item;
 488 }
 489
 490 int
 491 route_path_segment_get_length(struct route_path_segment *s)
 492 {
 493         return s->length;
 494 }
 495
 496 int
 497 route_path_segment_get_time(struct route_path_segment *s)
 498 {
 499         return s->time;
 500 }
 501
 502 void
 503 route_path_close(struct route_path_handle *h)
 504 {
 505         g_free(h);
 506 }
 507
 508
 509 static void
 510 route_graph_free_segments(struct route_graph *this)
 511 {
 512         struct route_graph_segment *curr,*next;
 513         curr=this->route_segments;
 514         while (curr) {
 515                 next=curr->next;
 516                 g_free(curr);
 517                 curr=next;
 518         }
 519         this->route_segments=NULL;
 520 }
 521
 522 static void
 523 route_graph_destroy(struct route_graph *this)
 524 {
 525         if (this) {
 526                 route_graph_free_points(this);
 527                 route_graph_free_segments(this);
 528                 g_free(this);
 529         }
 530 }
 531
 532 int
 533 route_time(int *speedlist, struct item *item, int len)
 534 {
 535         if (item->type < route_item_first || item->type > route_item_last) {
 536                 dbg(0,"street type %d out of range [%d,%d]", item->type, route_item_first, route_item_last);
 537                 return len*36;
 538         }
 539         return len*36/speedlist[item->type-route_item_first];
 540 }
 541
 542
 543 static int
 544 route_value(int *speedlist, struct item *item, int len)
 545 {
 546         int ret;
 547         if (len < 0) {
 548                 printf("len=%d\n", len);
 549         }
 550         g_assert(len >= 0);
 551         ret=route_time(speedlist, item, len);
 552         dbg(1, "route_value(0x%x, %d)=%d\n", item->type, len, ret);
 553         return ret;
 554 }
 555
 556 static void
 557 route_process_street_graph(struct route_graph *this, struct item *item)
 558 {
 559 #ifdef AVOID_FLOAT
 560         int len=0;
 561 #else
 562         double len=0;
 563 #endif
 564         struct route_graph_point *s_pnt,*e_pnt;
 565         struct coord c,l;
 566         struct attr attr;
 567
 568
 569         if (item_coord_get(item, &l, 1)) {
 570                 s_pnt=route_graph_add_point(this,&l);
 571                 while (item_coord_get(item, &c, 1)) {
 572                         len+=transform_distance(&l, &c);
 573                         l=c;
 574                 }
 575                 e_pnt=route_graph_add_point(this,&l);
 576                 g_assert(len >= 0);
 577                 if (item_attr_get(item, attr_limit, &attr))
 578                         route_graph_add_segment(this, s_pnt, e_pnt, len, item, attr.u.num);
 579                 else
 580                         route_graph_add_segment(this, s_pnt, e_pnt, len, item, 0);
 581         }
 582 }
 583
 584 static int
 585 compare(void *v1, void *v2)
 586 {
 587         struct route_graph_point *p1=v1;
 588         struct route_graph_point *p2=v2;
 589 #if 0
 590         if (debug_route)
 591                 printf("compare %d (%p) vs %d (%p)\n", p1->value,p1,p2->value,p2);
 592 #endif
 593         return p1->value-p2->value;
 594 }
 595
 596 int
 597 route_info_length(struct route_info *pos, struct route_info *dst, int dir)
 598 {
 599         struct route_info_handle *h;
 600         struct coord *c,*l;
 601         int ret=0;
 602
 603         dbg(2,"enter pos=%p dst=%p dir=%d\n", pos, dst, dir);
 604         h=route_info_open(pos, dst, dir);
 605         if (! h) {
 606                 dbg(2,"ret=-1\n");
 607                 return -1;
 608         }
 609         l=route_info_get(h);
 610         while ((c=route_info_get(h))) {
 611                 dbg(3,"c=%p\n", c);
 612                 ret+=transform_distance(c, l);
 613                 l=c;
 614         }
 615         dbg(2,"ret=%d\n", ret);
 616         return ret;
 617 }
 618
 619 static void
 620 route_graph_flood(struct route_graph *this, struct route_info *dst, int *speedlist)
 621 {
 622         struct route_graph_point *p_min,*end=NULL;
 623         struct route_graph_segment *s;
 624         int min,new,old,val;
 625         struct fibheap *heap;
 626         struct street_data *sd=dst->street;
 627
 628         heap = fh_makeheap();
 629         fh_setcmp(heap, compare);
 630
 631         if (! (sd->limit & 2)) {
 632                 end=route_graph_get_point(this, &sd->c[0]);
 633                 g_assert(end != 0);
 634                 end->value=route_value(speedlist, &sd->item, route_info_length(NULL, dst, -1));
 635                 end->el=fh_insert(heap, end);
 636         }
 637
 638         if (! (sd->limit & 1)) {
 639                 end=route_graph_get_point(this, &sd->c[sd->count-1]);
 640                 g_assert(end != 0);
 641                 end->value=route_value(speedlist, &sd->item, route_info_length(NULL, dst, 1));
 642                 end->el=fh_insert(heap, end);
 643         }
 644
 645         dbg(1,"0x%x,0x%x\n", end->c.x, end->c.y);
 646         for (;;) {
 647                 p_min=fh_extractmin(heap);
 648                 if (! p_min)
 649                         break;
 650                 min=p_min->value;
 651                 if (debug_route)
 652                         printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
 653                 p_min->el=NULL;
 654                 s=p_min->start;
 655                 while (s) {
 656                         val=route_value(speedlist, &s->item, s->len);
 657 #if 0
 658                         val+=val*2*street_route_contained(s->str->segid);
 659 #endif
 660                         new=min+val;
 661                         if (debug_route)
 662                                 printf("begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
 663                         if (new < s->end->value && !(s->limit & 1)) {
 664                                 s->end->value=new;
 665                                 s->end->seg=s;
 666                                 if (! s->end->el) {
 667                                         if (debug_route)
 668                                                 printf("insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
 669                                         s->end->el=fh_insert(heap, s->end);
 670                                         if (debug_route)
 671                                                 printf("el new=%p\n", s->end->el);
 672                                 }
 673                                 else {
 674                                         if (debug_route)
 675                                                 printf("replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
 676                                         fh_replacedata(heap, s->end->el, s->end);
 677                                 }
 678                         }
 679                         if (debug_route)
 680                                 printf("\n");
 681                         s=s->start_next;
 682                 }
 683                 s=p_min->end;
 684                 while (s) {
 685                         val=route_value(speedlist, &s->item, s->len);
 686                         new=min+val;
 687                         if (debug_route)
 688                                 printf("end %d len %d vs %d (0x%x,0x%x)\n",new,val,s->start->value,s->start->c.x, s->start->c.y);
 689                         if (new < s->start->value && !(s->limit & 2)) {
 690                                 old=s->start->value;
 691                                 s->start->value=new;
 692                                 s->start->seg=s;
 693                                 if (! s->start->el) {
 694                                         if (debug_route)
 695                                                 printf("insert_start p=%p el=%p val=%d ", s->start, s->start->el, s->start->value);
 696                                         s->start->el=fh_insert(heap, s->start);
 697                                         if (debug_route)
 698                                                 printf("el new=%p\n", s->start->el);
 699                                 }
 700                                 else {
 701                                         if (debug_route)
 702                                                 printf("replace_start p=%p el=%p val=%d\n", s->start, s->start->el, s->start->value);
 703                                         fh_replacedata(heap, s->start->el, s->start);
 704                                 }
 705                         }
 706                         if (debug_route)
 707                                 printf("\n");
 708                         s=s->end_next;
 709                 }
 710         }
 711         fh_deleteheap(heap);
 712 }
 713
 714 static struct route_path *
 715 route_path_new(struct route_graph *this, struct route_info *pos, struct route_info *dst, int *speedlist)
 716 {
 717         struct route_graph_point *start1=NULL,*start2=NULL,*start;
 718         struct route_graph_segment *s=NULL;
 719         int len=0,segs=0;
 720         int ilen,seg_time,seg_len;
 721 #if 0
 722         int time=0,hr,min,sec
 723 #endif
 724         unsigned int val1=0xffffffff,val2=0xffffffff;
 725         struct street_data *sd=pos->street;
 726         struct route_path *ret;
 727
 728         if (! (sd->limit & 1)) {
 729                 start1=route_graph_get_point(this, &sd->c[0]);
 730                 if (! start1)
 731                         return NULL;
 732                 val1=start1->value+route_value(speedlist, &sd->item, route_info_length(pos, NULL, -1));
 733                 dbg(1,"start1: %d(route)+%d=%d\n", start1->value, val1-start1->value, val1);
 734         }
 735         if (! (sd->limit & 2)) {
 736                 start2=route_graph_get_point(this, &sd->c[sd->count-1]);
 737                 if (! start2)
 738                         return NULL;
 739                 val2=start2->value+route_value(speedlist, &sd->item, route_info_length(pos, NULL, 1));
 740                 dbg(1,"start2: %d(route)+%d=%d\n", start2->value, val2-start2->value, val2);
 741         }
 742         dbg(1,"val1=%d val2=%d\n", val1, val2);
 743         if (val1 == val2) {
 744                 val1=start1->start->start->value;
 745                 val2=start2->end->end->value;
 746         }
 747         if (start1 && (val1 < val2)) {
 748                 start=start1;
 749                 pos->dir=-1;
 750         } else {
 751                 if (start2) {
 752                         start=start2;
 753                         pos->dir=1;
 754                 } else {
 755                         printf("no route found, pos blocked\n");
 756                         return NULL;
 757                 }
 758         }
 759         ret=g_new0(struct route_path, 1);
 760         ret->path_hash=item_hash_new();
 761         dbg(1,"dir=%d\n", pos->dir);
 762         while ((s=start->seg)) {
 763                 segs++;
 764 #if 0
 765                 printf("start->value=%d 0x%x,0x%x\n", start->value, start->c.x, start->c.y);
 766 #endif
 767                 seg_len=s->len;
 768                 seg_time=route_time(speedlist, &s->item, seg_len);
 769 #if 0
 770                 time+=seg_time;
 771 #endif
 772                 len+=seg_len;
 773                 if (s->start == start) {
 774                         route_path_add_item(ret, &s->item, &s->start->c, &s->end->c, seg_len, seg_time);
 775                         start=s->end;
 776                 } else {
 777                         route_path_add_item(ret, &s->item, &s->end->c, &s->start->c, seg_len, seg_time);
 778                         start=s->start;
 779                 }
 780         }
 781         sd=dst->street;
 782         dbg(1,"start->value=%d 0x%x,0x%x\n", start->value, start->c.x, start->c.y);
 783         dbg(1,"dst sd->limit=%d sd->c[0]=0x%x,0x%x sd->c[sd->count-1]=0x%x,0x%x\n", sd->limit, sd->c[0].x,sd->c[0].y, sd->c[sd->count-1].x, sd->c[sd->count-1].y);
 784         if (start->c.x == sd->c[0].x && start->c.y == sd->c[0].y)
 785                 dst->dir=-1;
 786         else if (start->c.x == sd->c[sd->count-1].x && start->c.y == sd->c[sd->count-1].y)
 787                 dst->dir=1;
 788         else {
 789                 printf("no route found\n");
 790                 route_path_destroy(ret);
 791                 return NULL;
 792         }
 793         ilen=route_info_length(pos, NULL, 0);
 794 #if 0
 795         time+=route_time(&pos->street->item, ilen);
 796 #endif
 797         len+=ilen;
 798
 799         ilen=route_info_length(NULL, dst, 0);
 800 #if 0
 801         time+=route_time(&dst->street->item, ilen);
 802 #endif
 803         len+=ilen;
 804
 805         dbg(1, "%d segments\n", segs);
 806 #if 0
 807         dbg(1, "len %5.3f\n", len/1000.0);
 808         time/=10;
 809         sec=time;
 810         min=time/60;
 811         time-=min*60;
 812         hr=min/60;
 813         min-=hr*60;
 814         dbg(1, "time %02d:%02d:%02d (%d sec)\n", hr, min, time, sec);
 815         dbg(1, "speed %f km/h\n", len/sec*3.6);
 816 #endif
 817         return ret;
 818 }
 819
 820 static struct route_graph *
 821 route_graph_build(struct mapset *ms, struct coord *c1, struct coord *c2)
 822 {
 823         struct route_graph *ret=g_new0(struct route_graph, 1);
 824         struct map_selection *sel;
 825         struct mapset_handle *h;
 826         struct map_rect *mr;
 827         struct map *m;
 828         struct item *item;
 829         struct coord e;
 830
 831         sel=route_calc_selection(c1, c2);
 832         h=mapset_open(ms);
 833         while ((m=mapset_next(h,1))) {
 834                 mr=map_rect_new(m, sel);
 835                 while ((item=map_rect_get_item(mr))) {
 836                         if (item->type >= type_street_0 && item->type <= type_ferry) {
 837                                 route_process_street_graph(ret, item);
 838                         } else
 839                                 while (item_coord_get(item, &e, 1));
 840                 }
 841                 map_rect_destroy(mr);
 842         }
 843         mapset_close(h);
 844         route_free_selection(sel);
 845
 846         return ret;
 847 }
 848
 849 static void
 850 route_graph_update(struct route *this)
 851 {
 852         route_graph_destroy(this->graph);
 853         profile(1,"graph_free");
 854         this->graph=route_graph_build(this->ms, &this->pos->c, &this->dst->c);
 855         profile(1,"route_graph_build");
 856         route_graph_flood(this->graph, this->dst, this->speedlist);
 857         profile(1,"route_graph_flood");
 858         this->version++;
 859 }
 860
 861 struct street_data *
 862 street_get_data (struct item *item)
 863 {
 864         struct coord c[1000];
 865         int count=0;
 866         struct street_data *ret;
 867         struct attr attr;
 868
 869         while (count < 1000) {
 870                 if (!item_coord_get(item, &c[count], 1))
 871                         break;
 872                 count++;
 873         }
 874         g_assert(count < 1000);
 875         ret=g_malloc(sizeof(struct street_data)+count*sizeof(struct coord));
 876         ret->item=*item;
 877         ret->count=count;
 878         if (item_attr_get(item, attr_limit, &attr))
 879                 ret->limit=attr.u.num;
 880         else
 881                 ret->limit=0;
 882         memcpy(ret->c, c, count*sizeof(struct coord));
 883
 884         return ret;
 885
 886 }
 887
 888 struct street_data *
 889 street_data_dup(struct street_data *orig)
 890 {
 891         struct street_data *ret;
 892         int size=sizeof(struct street_data)+orig->count*sizeof(struct coord);
 893
 894         ret=g_malloc(size);
 895         memcpy(ret, orig, size);
 896
 897         return ret;
 898 }
 899
 900 void
 901 street_data_free(struct street_data *sd)
 902 {
 903         g_free(sd);
 904 }
 905
 906 static struct route_info *
 907 route_find_nearest_street(struct mapset *ms, struct coord *c)
 908 {
 909         struct route_info *ret=NULL;
 910         int max_dist=1000;
 911         struct map_selection *sel=route_rect(18, c, c, 0, max_dist);
 912         int dist,pos;
 913         struct mapset_handle *h;
 914         struct map *m;
 915         struct map_rect *mr;
 916         struct item *item;
 917         struct coord lp, sc[1000];
 918         struct street_data *sd;
 919
 920         h=mapset_open(ms);
 921         while ((m=mapset_next(h,1))) {
 922                 mr=map_rect_new(m, sel);
 923                while ((item=map_rect_get_item(mr))) {
 924                         if (item->type >= type_street_0 && item->type <= type_ferry) {
 925                                 sd=street_get_data(item);
 926                                 dist=transform_distance_polyline_sq(sd->c, sd->count, c, &lp, &pos);
 927                                 if (!ret || dist < ret->dist) {
 928                                         if (ret) {
 929                                                 street_data_free(ret->street);
 930                                                 g_free(ret);
 931                                         }
 932                                         ret=g_new(struct route_info, 1);
 933                                         ret->c=*c;
 934                                         ret->lp=lp;
 935                                         ret->pos=pos;
 936                                         ret->dist=dist;
 937                                         ret->dir=0;
 938                                         ret->street=sd;
 939                                         dbg(1,"dist=%d id 0x%x 0x%x pos=%d\n", dist, item->id_hi, item->id_lo, pos);
 940                                 } else
 941                                         street_data_free(sd);
 942                         } else
 943                                 while (item_coord_get(item, &sc[0], 1));
 944                 }
 945                 map_rect_destroy(mr);
 946         }
 947         mapset_close(h);
 948
 949         return ret;
 950 }
 951
 952 void
 953 route_info_free(struct route_info *inf)
 954 {
 955         if (inf->street)
 956                 street_data_free(inf->street);
 957         g_free(inf);
 958 }
 959
 960
 961 #include "point.h"
 962
 963 struct street_data *
 964 route_info_street(struct route_info *rinf)
 965 {
 966         return rinf->street;
 967 }
 968
 969 struct coord *
 970 route_info_point(struct route_info *rinf, int point)
 971 {
 972         struct street_data *sd=rinf->street;
 973         int dir;
 974
 975         switch(point) {
 976         case -1:
 977         case 2:
 978                 dir=point == 2 ? rinf->dir : -rinf->dir;
 979                 if (dir > 0)
 980                         return &sd->c[sd->count-1];
 981                 else
 982                         return &sd->c[0];
 983         case 0:
 984                 return &rinf->c;
 985         case 1:
 986                 return &rinf->lp;
 987         }
 988         return NULL;
 989
 990 }
 991
 992 struct route_info_handle {
 993         struct route_info *start;
 994         struct route_info *curr;
 995         struct route_info *end;
 996         struct coord *last;
 997         int count;
 998         int iter;
 999         int pos;
1000         int endpos;
1001         int dir;
1002 };
1003
1004 struct route_info_handle *
1005 route_info_open(struct route_info *start, struct route_info *end, int dir)
1006 {
1007         struct route_info_handle *ret=g_new0(struct route_info_handle, 1);
1008
1009         struct route_info *curr;
1010         dbg(2,"enter start=%p end=%p dir=%d\n", start, end, dir);
1011         ret->start=start;
1012         ret->end=end;
1013         if (start)
1014                 curr=start;
1015         else
1016                 curr=end;
1017         ret->endpos=-2;
1018         if (start && end) {
1019                 if (start->street->item.map != end->street->item.map || start->street->item.id_hi != end->street->item.id_hi || start->street->item.id_lo != end->street->item.id_lo) {
1020                         dbg(1,"return NULL\n");
1021                         return NULL;
1022                 }
1023                 printf("trivial start=%d end=%d start dir %d end dir %d\n", start->pos, end->pos, start->dir, end->dir);
1024                 if (start->pos == end->pos) {
1025                         printf("fixme\n");
1026                         start->dir=0;
1027                         end->dir=0;
1028                 }
1029                 if (start->pos > end->pos) {
1030                         printf("fixme\n");
1031                         start->dir=-1;
1032                         end->dir=1;
1033                 }
1034                 if (start->pos < end->pos) {
1035                         printf("fixme\n");
1036                         start->dir=1;
1037                         end->dir=-1;
1038                 }
1039                 printf("trivial now start=%d end=%d start dir %d end dir %d\n", start->pos, end->pos, start->dir, end->dir);
1040                 ret->endpos=end->pos;
1041         }
1042
1043         if (!dir)
1044                 dir=curr->dir;
1045         dbg(2,"dir=%d\n", dir);
1046         ret->dir=dir;
1047         ret->curr=curr;
1048         ret->pos=curr->pos;
1049         if (dir > 0) {
1050                 ret->pos++;
1051                 ret->endpos++;
1052         }
1053         dbg(2,"ret=%p\n",ret);
1054         return ret;
1055 }
1056
1057 struct coord *
1058 route_info_get(struct route_info_handle *h)
1059 {
1060         struct coord *new;
1061         for (;;) {
1062                 new=NULL;
1063                 dbg(1,"iter=%d\n", h->iter);
1064                 switch(h->iter) {
1065                 case 0:
1066                         if (h->start) {
1067                                 new=&h->start->c;
1068                                 h->iter++;
1069                                 break;
1070                         } else {
1071                                 h->iter=2;
1072                                 continue;
1073                         }
1074                 case 1:
1075                         new=&h->start->lp;
1076                         h->iter++;
1077                         break;
1078                 case 2:
1079                         dbg(1,"h->pos=%d\n", h->pos);
1080                         if (h->dir && h->pos >= 0 && h->pos < h->curr->street->count && (h->end == NULL || h->endpos!=h->pos)) {
1081                                 new=&h->curr->street->c[h->pos];
1082                                 h->pos+=h->dir;
1083                         } else {
1084                                 h->iter++;
1085                                 continue;
1086                         }
1087                         break;
1088                 case 3:
1089                         if (h->end) {
1090                                 new=&h->end->lp;
1091                                 h->iter++;
1092                                 break;
1093                         }
1094                         break;
1095                 case 4:
1096                         new=&h->end->c;
1097                         h->iter++;
1098                         break;
1099
1100                 }
1101                 if (new) {
1102                         dbg(1,"new=%p (0x%x,0x%x) last=%p\n", new, new->x, new->y, h->last);
1103                         if (h->last && new->x == h->last->x && new->y == h->last->y)
1104                                 continue;
1105                         h->last=new;
1106                         return new;
1107                 }
1108                 return NULL;
1109         }
1110 }
1111
1112 void
1113 route_info_close(struct route_info_handle *h)
1114 {
1115         g_free(h);
1116 }
1117
1118
1119
1120
1121 static int
1122 route_draw_route_info(struct route_info *pos, struct route_info *dst, struct transformation *t, struct displaylist *dsp)
1123 {
1124         struct route_info_handle *h;
1125         struct coord *c;
1126         struct coord_rect r;
1127         struct item item;
1128         struct point pnt[100];
1129         int count=0;
1130
1131         item.id_lo=0;
1132         item.id_hi=0;
1133         item.map=NULL;
1134         item.type=type_street_route;
1135
1136         dbg(1, "enter\n");
1137         h=route_info_open(pos, dst, 0);
1138         dbg(1,"h=%p\n", h);
1139         if (! h) {
1140                 dbg(1, "return 0\n");
1141                 return 0;
1142         }
1143         if (pos)
1144                 dbg(1, "pos=%p pos->dir=%d pos->pos=%d\n", pos, pos->dir, pos->pos);
1145         c=route_info_get(h);
1146         r.lu=*c;
1147         r.rl=*c;
1148         while (c && count < 100) {
1149                 dbg(1,"c=%p (0x%x,0x%x)\n", c, c->x, c->y);
1150                 transform(t, projection_mg, c, &pnt[count++]);
1151                 coord_rect_extend(&r, c);
1152                 c=route_info_get(h);
1153
1154         }
1155         if (count && transform_contains(t, projection_mg, &r))
1156                 display_add(dsp, &item, count, pnt, "Route");
1157         route_info_close(h);
1158         dbg(1, "return 1\n");
1159         return 1;
1160 }
1161
1162 void
1163 route_draw(struct route *this, struct transformation *t, struct displaylist *dsp)
1164 {
1165         dbg(1,"enter\n");
1166         if (! this->pos || ! this->dst)
1167                 return;
1168         if (! route_draw_route_info(this->pos, this->dst, t, dsp)) {
1169                 route_draw_route_info(this->pos, NULL, t, dsp);
1170                 route_draw_route_info(NULL, this->dst, t, dsp);
1171         }
1172         dbg(1,"exit\n");
1173 }
1174
1175 #if 0
1176 struct route_crossings *
1177 route_crossings_get(struct route *this, struct coord *c)
1178 {
1179       struct route_point *pnt;
1180       struct route_segment *seg;
1181       int crossings=0;
1182       struct route_crossings *ret;
1183
1184        pnt=route_graph_get_point(this, c);
1185        seg=pnt->start;
1186        while (seg) {
1187                 printf("start: 0x%x 0x%x\n", seg->item.id_hi, seg->item.id_lo);
1188                crossings++;
1189                seg=seg->start_next;
1190        }
1191        seg=pnt->end;
1192        while (seg) {
1193                 printf("end: 0x%x 0x%x\n", seg->item.id_hi, seg->item.id_lo);
1194                crossings++;
1195                seg=seg->end_next;
1196        }
1197        ret=g_malloc(sizeof(struct route_crossings)+crossings*sizeof(struct route_crossing));
1198        ret->count=crossings;
1199        return ret;
1200 }
1201 #endif