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