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

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <math.h>
   5 #include <glib.h>
   6 #include <libintl.h>
   7 #include "debug.h"
   8 #include "profile.h"
   9 #include "navigation.h"
  10 #include "coord.h"
  11 #include "item.h"
  12 #include "route.h"
  13 #include "transform.h"
  14 #include "mapset.h"
  15 #include "projection.h"
  16 #include "map.h"
  17 #include "navit.h"
  18 #include "callback.h"
  19
  20 #define _(STRING)    gettext(STRING)
  21
  22 struct navigation {
  23         struct mapset *ms;
  24         struct item_hash *hash;
  25         struct navigation_itm *first;
  26         struct navigation_itm *last;
  27         struct navigation_command *cmd_first;
  28         struct navigation_command *cmd_last;
  29         struct callback_list *callback_speech;
  30         struct callback_list *callback;
  31         int level_last;
  32         struct item item_last;
  33         int turn_around;
  34         int distance_turn;
  35         int distance_last;
  36         int announce[route_item_last-route_item_first+1][3];
  37 };
  38
  39
  40 struct navigation_command {
  41         struct navigation_itm *itm;
  42         struct navigation_command *next;
  43         int delta;
  44 };
  45
  46 struct navigation_list {
  47         struct navigation *nav;
  48         struct navigation_command *cmd;
  49         struct navigation_command *cmd_next;
  50         struct navigation_itm *itm;
  51         struct navigation_itm *itm_next;
  52         struct item item;
  53 #if 0
  54         char *str;
  55 #endif
  56 };
  57
  58 struct navigation *
  59 navigation_new(struct mapset *ms)
  60 {
  61         int i,j;
  62         struct navigation *ret=g_new0(struct navigation, 1);
  63         ret->ms=ms;
  64         ret->hash=item_hash_new();
  65         ret->callback=callback_list_new();
  66         ret->callback_speech=callback_list_new();
  67         ret->level_last=-2;
  68         ret->distance_last=-2;
  69         ret->distance_turn=50;
  70
  71         for (j = 0 ; j <= route_item_last-route_item_first ; j++) {
  72                 for (i = 0 ; i < 3 ; i++) {
  73                         ret->announce[j][i]=-1;
  74                 }
  75         }
  76
  77         return ret;
  78 }
  79
  80 void
  81 navigation_set_mapset(struct navigation *this_, struct mapset *ms)
  82 {
  83         this_->ms=ms;
  84 }
  85
  86 int
  87 navigation_set_announce(struct navigation *this_, enum item_type type, int *level)
  88 {
  89         int i;
  90         if (type < route_item_first || type > route_item_last) {
  91                 dbg(0,"street type %d out of range [%d,%d]", type, route_item_first, route_item_last);
  92                 return 0;
  93         }
  94         for (i = 0 ; i < 3 ; i++)
  95                 this_->announce[type-route_item_first][i]=level[i];
  96         return 1;
  97 }
  98
  99 static int
 100 navigation_get_announce_level(struct navigation *this_, enum item_type type, int dist)
 101 {
 102         int i;
 103
 104         if (type < route_item_first || type > route_item_last)
 105                 return -1;
 106         for (i = 0 ; i < 3 ; i++) {
 107                 if (dist <= this_->announce[type-route_item_first][i])
 108                         return i;
 109         }
 110         return i;
 111 }
 112
 113 struct navigation_itm {
 114         char *name1;
 115         char *name2;
 116         struct coord start;
 117         struct item item;
 118         int angle_start;
 119         int angle_end;
 120         int time;
 121         int length;
 122         int dest_time;
 123         int dest_length;
 124         struct navigation_itm *next;
 125         struct navigation_itm *prev;
 126 };
 127
 128 /* 0=N,90=E */
 129 static int
 130 road_angle(struct coord *c1, struct coord *c2, int dir)
 131 {
 132         int ret=transform_get_angle_delta(c1, c2, dir);
 133         dbg(1, "road_angle(0x%x,0x%x - 0x%x,0x%x)=%d\n", c1->x, c1->y, c2->x, c2->y, ret);
 134         return ret;
 135 }
 136
 137 static int
 138 round_distance(int dist)
 139 {
 140         if (dist < 100) {
 141                 dist=(dist+5)/10;
 142                 return dist*10;
 143         }
 144         if (dist < 250) {
 145                 dist=(dist+13)/25;
 146                 return dist*25;
 147         }
 148         if (dist < 500) {
 149                 dist=(dist+25)/50;
 150                 return dist*50;
 151         }
 152         if (dist < 1000) {
 153                 dist=(dist+50)/100;
 154                 return dist*100;
 155         }
 156         if (dist < 5000) {
 157                 dist=(dist+50)/100;
 158                 return dist*100;
 159         }
 160         if (dist < 100000) {
 161                 dist=(dist+500)/1000;
 162                 return dist*1000;
 163         }
 164         dist=(dist+5000)/10000;
 165         return dist*10000;
 166 }
 167
 168 static char *
 169 get_distance(int dist, enum attr_type type, int is_length)
 170 {
 171         if (type == attr_navigation_long) {
 172                 if (is_length)
 173                         return g_strdup_printf(_("%d m"), dist);
 174                 else
 175                         return g_strdup_printf(_("in %d m"), dist);
 176         }
 177         if (dist < 1000) {
 178                 if (is_length)
 179                         return g_strdup_printf(_("%d meters"), dist);
 180                 else
 181                         return g_strdup_printf(_("in %d meters"), dist);
 182         }
 183         if (dist < 5000) {
 184                 int rem=(dist/100)%10;
 185                 if (rem) {
 186                         if (is_length)
 187                                 return g_strdup_printf(_("%d.%d kilometer"), dist/1000, rem);
 188                         else
 189                                 return g_strdup_printf(_("in %d.%d kilometers"), dist/1000, rem);
 190                 }
 191         }
 192         if (is_length)
 193                 return g_strdup_printf(ngettext("one kilometer","%d kilometers", dist/1000), dist/1000);
 194         else
 195                 return g_strdup_printf(ngettext("in one kilometer","in %d kilometers", dist/1000), dist/1000);
 196 }
 197
 198 static void
 199 navigation_destroy_itms_cmds(struct navigation *this_, struct navigation_itm *end)
 200 {
 201         struct navigation_itm *itm;
 202         struct navigation_command *cmd;
 203         dbg(2,"enter this_=%p end=%p\n", this_, end);
 204         while (this_->first && this_->first != end) {
 205                 itm=this_->first;
 206                 dbg(3,"destroying %p\n", itm);
 207                 item_hash_remove(this_->hash, &itm->item);
 208                 this_->first=itm->next;
 209                 if (this_->first)
 210                         this_->first->prev=NULL;
 211                 if (this_->cmd_first && this_->cmd_first->itm == itm) {
 212                         cmd=this_->cmd_first;
 213                         this_->cmd_first=cmd->next;
 214                         g_free(cmd);
 215                 }
 216                 g_free(itm);
 217         }
 218         if (! this_->first)
 219                 this_->last=NULL;
 220         if (! this_->first && end)
 221                 dbg(0,"end wrong\n");
 222         dbg(2,"ret\n");
 223 }
 224
 225 static struct navigation_itm *
 226 navigation_itm_new(struct navigation *this_, struct item *item, struct coord *start)
 227 {
 228         struct navigation_itm *ret=g_new0(struct navigation_itm, 1);
 229         int l,i=0,a1,a2,dir=0;
 230         struct map_rect *mr;
 231         struct attr attr;
 232         struct coord c[5];
 233
 234         if (item) {
 235                 ret->item=*item;
 236                 item_hash_insert(this_->hash, item, ret);
 237                 mr=map_rect_new(item->map, NULL);
 238                 item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
 239                 if (item_attr_get(item, attr_street_name, &attr))
 240                         ret->name1=g_strdup(attr.u.str);
 241                 if (item_attr_get(item, attr_street_name_systematic, &attr))
 242                         ret->name2=g_strdup(attr.u.str);
 243                 l=-1;
 244                 while (item_coord_get(item, &c[i], 1)) {
 245                         dbg(1, "coord %d 0x%x 0x%x\n", i, c[i].x ,c[i].y);
 246                         l=i;
 247                         if (i < 4)
 248                                 i++;
 249                         else {
 250                                 c[2]=c[3];
 251                                 c[3]=c[4];
 252                         }
 253                 }
 254                 dbg(1,"count=%d\n", l);
 255                 if (l == 4)
 256                         l=3;
 257                 if (start->x != c[0].x || start->y != c[0].y)
 258                         dir=-1;
 259                 a1=road_angle(&c[0], &c[1], dir);
 260                 a2=road_angle(&c[l-1], &c[l], dir);
 261                 if (dir >= 0) {
 262                         ret->angle_start=a1;
 263                         ret->angle_end=a2;
 264                 } else {
 265                         ret->angle_start=a2;
 266                         ret->angle_end=a1;
 267                 }
 268                 dbg(1,"i=%d dir=%d a1 %d a2 %d '%s' '%s'\n", i, dir, a1, a2, ret->name1, ret->name2);
 269                 map_rect_destroy(mr);
 270         }
 271         if (start)
 272                 ret->start=*start;
 273         if (! this_->first)
 274                 this_->first=ret;
 275         if (this_->last) {
 276                 this_->last->next=ret;
 277                 ret->prev=this_->last;
 278         }
 279         dbg(1,"ret=%p\n", ret);
 280         this_->last=ret;
 281         return ret;
 282 }
 283
 284 static void
 285 calculate_dest_distance(struct navigation *this_, int incr)
 286 {
 287         int len=0, time=0;
 288         struct navigation_itm *next,*itm=this_->last;
 289         dbg(1, "enter this_=%p, incr=%d\n", this_, incr);
 290         if (incr) {
 291                 if (itm)
 292                         dbg(2, "old values: (%p) time=%d lenght=%d\n", itm, itm->dest_length, itm->dest_time);
 293                 else
 294                         dbg(2, "old values: itm is null\n");
 295                 itm=this_->first;
 296                 next=itm->next;
 297                 dbg(2, "itm values: time=%d lenght=%d\n", itm->length, itm->time);
 298                 dbg(2, "next values: (%p) time=%d lenght=%d\n", next, next->dest_length, next->dest_time);
 299                 itm->dest_length=next->dest_length+itm->length;
 300                 itm->dest_time=next->dest_time+itm->time;
 301                 dbg(2, "new values: time=%d lenght=%d\n", itm->dest_length, itm->dest_time);
 302                 return;
 303         }
 304         while (itm) {
 305                 len+=itm->length;
 306                 time+=itm->time;
 307                 itm->dest_length=len;
 308                 itm->dest_time=time;
 309                 itm=itm->prev;
 310         }
 311         dbg(1,"len %d time %d\n", len, time);
 312 }
 313
 314 static int
 315 is_same_street2(struct navigation_itm *old, struct navigation_itm *new)
 316 {
 317         if (old->name1 && new->name1 && !strcmp(old->name1, new->name1)) {
 318                 dbg(1,"is_same_street: '%s' '%s' vs '%s' '%s' yes (1.)\n", old->name2, new->name2, old->name1, new->name1);
 319                 return 1;
 320         }
 321         if (old->name2 && new->name2 && !strcmp(old->name2, new->name2)) {
 322                 dbg(1,"is_same_street: '%s' '%s' vs '%s' '%s' yes (2.)\n", old->name2, new->name2, old->name1, new->name1);
 323                 return 1;
 324         }
 325         dbg(1,"is_same_street: '%s' '%s' vs '%s' '%s' no\n", old->name2, new->name2, old->name1, new->name1);
 326         return 0;
 327 }
 328
 329 static int
 330 maneuver_required2(struct navigation_itm *old, struct navigation_itm *new, int *delta)
 331 {
 332         dbg(1,"enter %p %p %p\n",old, new, delta);
 333         if (new->item.type != old->item.type && (new->item.type == type_ramp || old->item.type == type_ramp)) {
 334                 dbg(1, "maneuver_required: old or new is ramp\n");
 335                 return 1;
 336         }
 337         if (is_same_street2(old, new)) {
 338                 dbg(1, "maneuver_required: is_same_street: no\n");
 339                 return 0;
 340         }
 341 #if 0
 342         if (old->crossings_end == 2) {
 343                 dbg(1, "maneuver_required: only 2 connections: no\n");
 344                 return 0;
 345         }
 346 #endif
 347         *delta=new->angle_start-old->angle_end;
 348         if (*delta < -180)
 349                 *delta+=360;
 350         if (*delta > 180)
 351                 *delta-=360;
 352         dbg(1,"delta=%d-%d=%d\n", new->angle_start, old->angle_end, *delta);
 353         if (*delta < 20 && *delta >-20) {
 354                 dbg(1, "maneuver_required: delta(%d) < 20: no\n", *delta);
 355                 return 0;
 356         }
 357         dbg(1, "maneuver_required: delta=%d: yes\n", *delta);
 358         return 1;
 359 }
 360
 361 static struct navigation_command *
 362 command_new(struct navigation *this_, struct navigation_itm *itm, int delta)
 363 {
 364         struct navigation_command *ret=g_new0(struct navigation_command, 1);
 365         ret->delta=delta;
 366         ret->itm=itm;
 367         if (this_->cmd_last)
 368                 this_->cmd_last->next=ret;
 369         this_->cmd_last=ret;
 370
 371         if (!this_->cmd_first)
 372                 this_->cmd_first=ret;
 373         return ret;
 374 }
 375
 376 static void
 377 make_maneuvers(struct navigation *this_)
 378 {
 379         struct navigation_itm *itm, *last=NULL, *last_itm=NULL;
 380         itm=this_->first;
 381         int delta;
 382         this_->cmd_last=NULL;
 383         this_->cmd_first=NULL;
 384         while (itm) {
 385                 if (last) {
 386                         if (maneuver_required2(last_itm, itm, &delta)) {
 387                                 command_new(this_, itm, delta);
 388                         }
 389                 } else
 390                         last=itm;
 391                 last_itm=itm;
 392                 itm=itm->next;
 393         }
 394 }
 395
 396 static char *
 397 show_maneuver(struct navigation *nav, struct navigation_itm *itm, struct navigation_command *cmd, enum attr_type type)
 398 {
 399         char *dir=_("right"),*strength="";
 400         int distance=itm->dest_length-cmd->itm->dest_length;
 401         char *d,*ret;
 402         int delta=cmd->delta;
 403         int level;
 404         level=1;
 405         if (delta < 0) {
 406                 dir=_("left");
 407                 delta=-delta;
 408         }
 409         if (delta < 45) {
 410                 strength=_("easily ");
 411         } else if (delta < 105) {
 412                 strength="";
 413         } else if (delta < 165) {
 414                 strength=_("strongly ");
 415         } else {
 416                 dbg(1,"delta=%d\n", delta);
 417                 strength=_("unknown ");
 418         }
 419         if (type != attr_navigation_long_exact)
 420                 distance=round_distance(distance);
 421         if (type == attr_navigation_speech) {
 422                 if (nav->turn_around)
 423                         return g_strdup(_("When possible, please turn around"));
 424                 level=navigation_get_announce_level(nav, itm->item.type, distance);
 425                 dbg(1,"distance=%d level=%d type=0x%x\n", distance, level, itm->item.type);
 426         }
 427         switch(level) {
 428         case 3:
 429                 d=get_distance(distance, type, 1);
 430                 ret=g_strdup_printf(_("Follow the road for the next %s"), d);
 431                 g_free(d);
 432                 return ret;
 433         case 2:
 434                 d=g_strdup(_("soon"));
 435                 break;
 436         case 1:
 437                 d=get_distance(distance, type, 0);
 438                 break;
 439         case 0:
 440                 d=g_strdup(_("now"));
 441                 break;
 442         default:
 443                 d=g_strdup(_("error"));
 444         }
 445         if (cmd->itm->next) {
 446                 /* TRANSLATORS: The first argument is strength, the second direction and the third distance */
 447                 ret=g_strdup_printf(_("Turn %1$s%2$s %3$s"), strength, dir, d);
 448         }
 449         else
 450                 ret=g_strdup_printf(_("You have reached your destination %s"), d);
 451         g_free(d);
 452         return ret;
 453 }
 454
 455 static int
 456 navigation_list_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
 457 {
 458         struct navigation_list *this_=priv_data;
 459         attr->type=attr_type;
 460         switch(attr_type) {
 461         case attr_navigation_short:
 462         case attr_navigation_long:
 463         case attr_navigation_long_exact:
 464         case attr_navigation_speech:
 465                 attr->u.str=show_maneuver(this_->nav, this_->itm, this_->cmd, attr_type);
 466                 return 1;
 467         case attr_length:
 468                 attr->u.num=this_->itm->dest_length-this_->cmd->itm->dest_length;
 469                 return 1;
 470         case attr_time:
 471                 attr->u.num=(this_->itm->dest_time-this_->cmd->itm->dest_time)/10;
 472                 return 1;
 473         case attr_destination_length:
 474                 attr->u.num=this_->itm->dest_length;
 475                 return 1;
 476         case attr_destination_time:
 477                 attr->u.num=this_->itm->dest_time/10;
 478                 return 1;
 479         default:
 480                 attr->type=attr_none;
 481                 return 0;
 482         }
 483 }
 484
 485 struct item_methods navigation_list_item_methods = {
 486         NULL,
 487         NULL,
 488         NULL,
 489         navigation_list_item_attr_get,
 490 };
 491
 492
 493
 494 struct navigation_list *
 495 navigation_list_new(struct navigation *this_)
 496 {
 497         struct navigation_list *ret=g_new0(struct navigation_list, 1);
 498         ret->nav=this_;
 499         ret->cmd_next=this_->cmd_first;
 500         ret->itm_next=this_->first;
 501         ret->item.meth=&navigation_list_item_methods;
 502         ret->item.priv_data=ret;
 503         return ret;
 504 }
 505
 506 struct item *
 507 navigation_list_get_item(struct navigation_list *this_)
 508 {
 509         if (!this_->cmd_next)
 510                 return NULL;
 511         this_->cmd=this_->cmd_next;
 512         this_->itm=this_->itm_next;
 513 #if 0
 514         this_->str=show_maneuver(this_->nav, this_->itm, this_->cmd, mode);
 515 #endif
 516         this_->itm_next=this_->cmd->itm;
 517         this_->cmd_next=this_->cmd->next;
 518
 519         return &this_->item;
 520 }
 521
 522
 523 void
 524 navigation_list_destroy(struct navigation_list *this_)
 525 {
 526         g_free(this_);
 527 }
 528
 529 static void
 530 navigation_call_callbacks(struct navigation *this_, int force_speech)
 531 {
 532         int distance, level = 0;
 533         void *p=this_;
 534         if (!this_->cmd_first)
 535                 return;
 536         callback_list_call(this_->callback, 1, &p);
 537         distance=round_distance(this_->first->dest_length-this_->cmd_first->itm->dest_length);
 538         if (this_->turn_around) {
 539                 if (distance > this_->distance_turn) {
 540                         this_->level_last=4;
 541                         level=4;
 542                         force_speech=1;
 543                         if (this_->distance_turn > 500)
 544                                 this_->distance_turn*=2;
 545                         else
 546                                 this_->distance_turn=500;
 547                 }
 548         } else {
 549                 this_->distance_turn=50;
 550                 level=navigation_get_announce_level(this_, this_->first->item.type, distance);
 551                 if (level < this_->level_last) {
 552                         dbg(0,"level %d < %d\n", level, this_->level_last);
 553                         this_->level_last=level;
 554                         force_speech=1;
 555                 }
 556                 if (!item_is_equal(this_->cmd_first->itm->item, this_->item_last)) {
 557                         dbg(0,"item different\n");
 558                         this_->item_last=this_->cmd_first->itm->item;
 559                         force_speech=1;
 560                 }
 561         }
 562         if (force_speech) {
 563                 this_->level_last=level;
 564                 dbg(0,"distance=%d level=%d type=0x%x\n", distance, level, this_->first->item.type);
 565                 callback_list_call(this_->callback_speech, 1, &p);
 566         }
 567 }
 568
 569 void
 570 navigation_update(struct navigation *this_, struct route *route)
 571 {
 572         struct route_path_handle *rph;
 573         struct route_path_segment *s;
 574         struct navigation_itm *itm;
 575         struct route_info *pos,*dst;
 576         struct street_data *sd;
 577         int *speedlist;
 578         int len,end_flag=0;
 579         int incr;
 580
 581         profile(0,NULL);
 582         pos=route_get_pos(route);
 583         dst=route_get_dst(route);
 584         if (! pos || ! dst)
 585                 return;
 586         speedlist=route_get_speedlist(route);
 587         len=route_info_length(pos, dst, 0);
 588         dbg(2,"len pos,dst = %d\n", len);
 589         if (len == -1) {
 590                 len=route_info_length(pos, NULL, 0);
 591                 dbg(2,"len pos = %d\n", len);
 592                 end_flag=1;
 593         }
 594         sd=route_info_street(pos);
 595         itm=item_hash_lookup(this_->hash, &sd->item);
 596         dbg(2,"itm for item with id (0x%x,0x%x) is %p\n", sd->item.id_hi, sd->item.id_lo, itm);
 597         navigation_destroy_itms_cmds(this_, itm);
 598         if (itm)
 599                 incr=1;
 600         else {
 601                 itm=navigation_itm_new(this_, &sd->item, route_info_point(pos, -1));
 602                 incr=0;
 603         }
 604         itm->length=len;
 605         itm->time=route_time(speedlist, &sd->item, len);
 606         dbg(2,"%p time = %d\n", itm, itm->time);
 607         if (!incr) {
 608                 printf("not on track\n");
 609                 rph=route_path_open(route);
 610                 if (rph) {
 611                         while((s=route_path_get_segment(rph))) {
 612                                 itm=navigation_itm_new(this_, route_path_segment_get_item(s),route_path_segment_get_start(s));
 613                                 itm->time=route_path_segment_get_time(s);
 614                                 itm->length=route_path_segment_get_length(s);
 615                         }
 616                         route_path_close(rph);
 617                 }
 618                 if (end_flag) {
 619                         len=route_info_length(NULL, dst, 0);
 620                         dbg(1, "end %d\n", len);
 621                         sd=route_info_street(dst);
 622                         itm=navigation_itm_new(this_, &sd->item, route_info_point(pos, 2));
 623                         itm->length=len;
 624                         itm->time=route_time(speedlist, &sd->item, len);
 625                 }
 626                 itm=navigation_itm_new(this_, NULL, NULL);
 627                 make_maneuvers(this_);
 628         }
 629         calculate_dest_distance(this_, incr);
 630         dbg(2,"destination distance old=%d new=%d\n", this_->distance_last, this_->first->dest_length);
 631         if (this_->first->dest_length > this_->distance_last && this_->distance_last >= 0)
 632                 this_->turn_around=1;
 633         else
 634                 this_->turn_around=0;
 635         dbg(2,"turn_around=%d\n", this_->turn_around);
 636         this_->distance_last=this_->first->dest_length;
 637         profile(0,"end");
 638         navigation_call_callbacks(this_, FALSE);
 639 }
 640
 641 void
 642 navigation_destroy(struct navigation *this_)
 643 {
 644         item_hash_destroy(this_->hash);
 645         callback_list_destroy(this_->callback);
 646         callback_list_destroy(this_->callback_speech);
 647         g_free(this_);
 648 }
 649
 650 int
 651 navigation_register_callback(struct navigation *this_, enum attr_type type, struct callback *cb)
 652 {
 653         if (type == attr_navigation_speech)
 654                 callback_list_add(this_->callback_speech, cb);
 655         else
 656                 callback_list_add(this_->callback, cb);
 657         return 1;
 658 }
 659
 660 void
 661 navigation_unregister_callback(struct navigation *this_, enum attr_type type, struct callback *cb)
 662 {
 663         if (type == attr_navigation_speech)
 664                 callback_list_remove_destroy(this_->callback_speech, cb);
 665         else
 666                 callback_list_remove_destroy(this_->callback, cb);
 667 }