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