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

   1 /**
   2  * Navit, a modular navigation system.
   3  * Copyright (C) 2005-2008 Navit Team
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License
   7  * version 2 as published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  17  * Boston, MA  02110-1301, USA.
  18  */
  19
  20 #include <glib.h>
  21 #include <string.h>
  22 #include "item.h"
  23 #include "attr.h"
  24 #include "track.h"
  25 #include "debug.h"
  26 #include "transform.h"
  27 #include "coord.h"
  28 #include "route.h"
  29 #include "projection.h"
  30 #include "map.h"
  31 #include "mapset.h"
  32 #include "plugin.h"
  33
  34 struct tracking_line
  35 {
  36         struct street_data *street;
  37 #if 0
  38         long segid;
  39         int linenum;
  40         struct coord c[2];
  41         struct coord lpnt;
  42         int value;
  43         int dir;
  44 #endif
  45         struct tracking_line *next;
  46         int angle[0];
  47 };
  48
  49
  50 struct tracking {
  51         struct mapset *ms;
  52         struct route *rt;
  53         struct map *map;
  54 #if 0
  55         struct transformation t;
  56 #endif
  57         struct pcoord last_updated;
  58         struct tracking_line *lines;
  59 #if 0
  60         struct tracking_line **last_ptr;
  61 #endif
  62         struct tracking_line *curr_line;
  63         int pos;
  64         struct coord curr[2];
  65         struct pcoord curr_in, curr_out;
  66         int curr_angle;
  67         struct coord last[2];
  68         struct pcoord last_in, last_out;
  69 };
  70
  71
  72 int angle_factor=10;
  73 int connected_pref=10;
  74 int nostop_pref=10;
  75 int offroad_limit_pref=5000;
  76 int route_pref=300;
  77
  78
  79 struct pcoord *
  80 tracking_get_pos(struct tracking *tr)
  81 {
  82         return &tr->curr_out;
  83 }
  84
  85 int
  86 tracking_get_segment_pos(struct tracking *tr)
  87 {
  88         return tr->pos;
  89 }
  90
  91 struct street_data *
  92 tracking_get_street_data(struct tracking *tr)
  93 {
  94         return tr->curr_line->street;
  95 }
  96
  97 int
  98 tracking_get_current_attr(struct tracking *_this, enum attr_type type, struct attr *attr)
  99 {
 100         struct item *item;
 101         struct map_rect *mr;
 102         int result=0;
 103         if (! _this->curr_line || ! _this->curr_line->street)
 104                 return 0;
 105         item=&_this->curr_line->street->item;
 106         mr=map_rect_new(item->map,NULL);
 107         item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
 108         if (item_attr_get(item, type, attr))
 109                 result=1;
 110         map_rect_destroy(mr);
 111         return result;
 112 }
 113
 114 struct item *
 115 tracking_get_current_item(struct tracking *_this)
 116 {
 117         if (! _this->curr_line || ! _this->curr_line->street)
 118                 return NULL;
 119         return &_this->curr_line->street->item;
 120 }
 121
 122 static void
 123 tracking_get_angles(struct tracking_line *tl)
 124 {
 125         int i;
 126         struct street_data *sd=tl->street;
 127         for (i = 0 ; i < sd->count-1 ; i++)
 128                 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
 129 }
 130
 131 static void
 132 tracking_doupdate_lines(struct tracking *tr, struct pcoord *pc)
 133 {
 134         int max_dist=1000;
 135         struct map_selection *sel;
 136         struct mapset_handle *h;
 137         struct map *m;
 138         struct map_rect *mr;
 139         struct item *item;
 140         struct street_data *street;
 141         struct tracking_line *tl;
 142         struct coord_geo g;
 143         struct coord cc;
 144
 145         dbg(1,"enter\n");
 146         h=mapset_open(tr->ms);
 147         while ((m=mapset_next(h,1))) {
 148                 cc.x = pc->x;
 149                 cc.y = pc->y;
 150                 if (map_projection(m) != pc->pro) {
 151                         transform_to_geo(pc->pro, &cc, &g);
 152                         transform_from_geo(map_projection(m), &g, &cc);
 153                 }
 154                 sel = route_rect(18, &cc, &cc, 0, max_dist);
 155                 mr=map_rect_new(m, sel);
 156                 if (!mr)
 157                         continue;
 158                 while ((item=map_rect_get_item(mr))) {
 159                         if (item->type >= type_street_0 && item->type <= type_ferry) {
 160                                 street=street_get_data(item);
 161                                 tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
 162                                 tl->street=street;
 163                                 tracking_get_angles(tl);
 164                                 tl->next=tr->lines;
 165                                 tr->lines=tl;
 166                         }
 167                 }
 168                 map_selection_destroy(sel);
 169                 map_rect_destroy(mr);
 170         }
 171         mapset_close(h);
 172         dbg(1, "exit\n");
 173 #if 0
 174
 175         struct transformation t;
 176
 177         tr->last_ptr=&tr->lines;
 178         transform_setup_source_rect_limit(&t,c,1000);
 179         transform_setup_source_rect_limit(&tr->t,c,1000);
 180
 181
 182         profile_timer(NULL);
 183         street_get_block(tr->ma,&t,tst_callback,tr);
 184         profile_timer("end");
 185 #endif
 186 }
 187
 188
 189 static void
 190 tracking_free_lines(struct tracking *tr)
 191 {
 192         struct tracking_line *tl=tr->lines,*next;
 193         dbg(1,"enter(tr=%p)\n", tr);
 194
 195         while (tl) {
 196                 next=tl->next;
 197                 street_data_free(tl->street);
 198                 g_free(tl);
 199                 tl=next;
 200         }
 201         tr->lines=NULL;
 202 }
 203
 204 static int
 205 tracking_angle_abs_diff(int a1, int a2, int full)
 206 {
 207         int ret;
 208
 209         if (a2 > a1)
 210                 ret=(a2-a1)%full;
 211         else
 212                 ret=(a1-a2)%full;
 213         if (ret > full/2)
 214                 ret=full-ret;
 215         return ret;
 216 }
 217
 218 static int
 219 tracking_angle_delta(int vehicle_angle, int street_angle, int flags)
 220 {
 221         int full=180;
 222         int ret;
 223         if (flags) {
 224                 full=360;
 225                 if (flags & 2) {
 226                         street_angle=(street_angle+180)%360;
 227                         if (flags & 1)
 228                                 return 360*360;
 229                 }
 230         }
 231         ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
 232
 233         return ret*ret;
 234 }
 235
 236 static int
 237 tracking_is_connected(struct coord *c1, struct coord *c2)
 238 {
 239         if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
 240                 return 0;
 241         if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
 242                 return 0;
 243         if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
 244                 return 0;
 245         if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
 246                 return 0;
 247         return connected_pref;
 248 }
 249
 250 static int
 251 tracking_is_no_stop(struct coord *c1, struct pcoord *c2)
 252 {
 253         if (c1->x == c2->x && c1->y == c2->y)
 254                 return nostop_pref;
 255         return 0;
 256 }
 257
 258 static int
 259 tracking_is_on_route(struct route *rt, struct item *item)
 260 {
 261         if (! rt)
 262                 return 0;
 263         if (route_pos_contains(rt, item))
 264                 return 0;
 265         if (route_contains(rt, item))
 266                 return 0;
 267         return route_pref;
 268 }
 269
 270 static int
 271 tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
 272 {
 273         int value=0;
 274         struct street_data *sd=t->street;
 275         struct coord c, c1, c2;
 276         c.x = sd->c[offset].x;
 277         c.y = sd->c[offset].y;
 278         if (flags & 1) {
 279                 c1.x = sd->c[offset].x;
 280                 c1.y = sd->c[offset].y;
 281         }
 282         c2.x = tr->curr_in.x;
 283         c2.y = tr->curr_in.y;
 284         dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y);
 285         if (flags & 1)
 286                 value+=transform_distance_line_sq(&c, &c1, &c2, lpnt);
 287         if (value >= min)
 288                 return value;
 289         if (flags & 2)
 290                 value += tracking_angle_delta(tr->curr_angle, t->angle[offset], sd->flags)*angle_factor>>4;
 291         if (value >= min)
 292                 return value;
 293         if (flags & 4)
 294                 value += tracking_is_connected(tr->last, &sd->c[offset]);
 295         if (flags & 8)
 296                 value += tracking_is_no_stop(lpnt, &tr->last_out);
 297         if (value >= min)
 298                 return value;
 299         if (flags & 16)
 300                 value += tracking_is_on_route(tr->rt, &sd->item);
 301         return value;
 302 }
 303
 304
 305 int
 306 tracking_update(struct tracking *tr, struct pcoord *pc, int angle)
 307 {
 308         struct tracking_line *t;
 309         int i,value,min;
 310         struct coord lpnt;
 311         struct coord cin;
 312 #if 0
 313         int min,dist;
 314         int debug=0;
 315 #endif
 316         dbg(1,"enter(%p,%p,%d)\n", tr, pc, angle);
 317         dbg(1,"c=%d:0x%x,0x%x\n", pc->pro, pc->x, pc->y);
 318
 319         if (pc->x == tr->curr_in.x && pc->y == tr->curr_in.y) {
 320                 if (tr->curr_out.x && tr->curr_out.y)
 321                         *pc=tr->curr_out;
 322                 return 0;
 323         }
 324         tr->last_in=tr->curr_in;
 325         tr->last_out=tr->curr_out;
 326         tr->last[0]=tr->curr[0];
 327         tr->last[1]=tr->curr[1];
 328         tr->curr_in=*pc;
 329         tr->curr_angle=angle;
 330         cin.x = pc->x;
 331         cin.y = pc->y;
 332         if (!tr->lines || transform_distance_sq_pc(&tr->last_updated, pc) > 250000) {
 333                 dbg(1, "update\n");
 334                 tracking_free_lines(tr);
 335                 tracking_doupdate_lines(tr, pc);
 336                 tr->last_updated=*pc;
 337                 dbg(1,"update end\n");
 338         }
 339
 340         t=tr->lines;
 341         if (! t)
 342                 return 0;
 343         tr->curr_line=NULL;
 344         min=INT_MAX/2;
 345         while (t) {
 346                 struct street_data *sd=t->street;
 347                 if ((sd->flags & 3) == 3) {
 348                         t=t->next;
 349                         continue;
 350                 }
 351                 for (i = 0; i < sd->count-1 ; i++) {
 352                         value=tracking_value(tr,t,i,&lpnt,min,-1);
 353                         if (value < min) {
 354                                 tr->curr_line=t;
 355                                 tr->pos=i;
 356                                 tr->curr[0]=sd->c[i];
 357                                 tr->curr[1]=sd->c[i+1];
 358                                 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
 359                                         transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt),
 360                                         tracking_angle_delta(angle, t->angle[i], 0)*angle_factor,
 361                                         tracking_is_connected(tr->last, &sd->c[i]) ? connected_pref : 0,
 362                                         lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0,
 363                                         value
 364                                 );
 365                                 tr->curr_out.x=lpnt.x;
 366                                 tr->curr_out.y=lpnt.y;
 367                                 tr->curr_out.pro = pc->pro;
 368                                 min=value;
 369                         }
 370                 }
 371                 t=t->next;
 372         }
 373         dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
 374         if (!tr->curr_line || min > offroad_limit_pref)
 375                 return 0;
 376         dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
 377         *pc=tr->curr_out;
 378         return 1;
 379 }
 380
 381 struct tracking *
 382 tracking_new(struct attr *parent, struct attr **attrs)
 383 {
 384         struct tracking *this=g_new0(struct tracking, 1);
 385
 386         return this;
 387 }
 388
 389 void
 390 tracking_set_mapset(struct tracking *this, struct mapset *ms)
 391 {
 392         this->ms=ms;
 393 }
 394
 395 void
 396 tracking_set_route(struct tracking *this, struct route *rt)
 397 {
 398         this->rt=rt;
 399 }
 400
 401 void
 402 tracking_destroy(struct tracking *tr)
 403 {
 404         tracking_free_lines(tr);
 405         g_free(tr);
 406 }
 407
 408 struct map *
 409 tracking_get_map(struct tracking *this_)
 410 {
 411         if (! this_->map)
 412                 this_->map=map_new(NULL, (struct attr*[]){
 413                         &(struct attr){attr_type,{"tracking"}},
 414                         &(struct attr){attr_trackingo,.u.tracking=this_},
 415                         &(struct attr){attr_data,{""}},
 416                         &(struct attr){attr_description,{"Tracking"}},
 417                         NULL});
 418         return this_->map;
 419 }
 420
 421
 422 struct map_priv {
 423         struct tracking *tracking;
 424 };
 425
 426 struct map_rect_priv {
 427         struct tracking *tracking;
 428         struct item item;
 429         struct tracking_line *curr,*next;
 430         int coord;
 431         enum attr_type attr_next;
 432         int ccount;
 433         int debug_idx;
 434         char *str;
 435 };
 436
 437 static int
 438 tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
 439 {
 440         struct map_rect_priv *this=priv_data;
 441         enum projection pro;
 442         int ret=0;
 443         dbg(1,"enter\n");
 444         while (this->ccount < 2 && count > 0) {
 445                 pro = map_projection(this->curr->street->item.map);
 446                 if (projection_mg != pro) {
 447                         transform_from_to(&this->curr->street->c[this->ccount+this->coord],
 448                                 pro,
 449                                 c ,projection_mg);
 450                 } else
 451                 *c=this->curr->street->c[this->ccount+this->coord];
 452                 dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y);
 453                 this->ccount++;
 454                 ret++;
 455                 c++;
 456                 count--;
 457         }
 458         return ret;
 459 }
 460
 461 static int
 462 tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
 463 {
 464         struct map_rect_priv *this_=priv_data;
 465         attr->type=attr_type;
 466         struct coord lpnt,*c;
 467         struct tracking *tr=this_->tracking;
 468         int value;
 469
 470         if (this_->str) {
 471                 g_free(this_->str);
 472                 this_->str=NULL;
 473         }
 474
 475         switch(attr_type) {
 476         case attr_debug:
 477                 switch(this_->debug_idx) {
 478                 case 0:
 479                         this_->debug_idx++;
 480                         this_->str=attr->u.str=g_strdup_printf("overall: %d",tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, -1));
 481                         return 1;
 482                 case 1:
 483                         this_->debug_idx++;
 484                         c=&this_->curr->street->c[this_->coord];
 485                         value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1);
 486                         this_->str=attr->u.str=g_strdup_printf("distance: (0x%x,0x%x) from (0x%x,0x%x)-(0x%x,0x%x) at (0x%x,0x%x) %d",
 487                                 tr->curr_in.x, tr->curr_in.y,
 488                                 c[0].x, c[0].y, c[1].x, c[1].y,
 489                                 lpnt.x, lpnt.y, value);
 490                         return 1;
 491                 case 2:
 492                         this_->debug_idx++;
 493                         this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d",
 494                                 tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
 495                                 tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2));
 496                         return 1;
 497                 case 3:
 498                         this_->debug_idx++;
 499                         this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 4));
 500                         return 1;
 501                 case 4:
 502                         this_->debug_idx++;
 503                         this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 8));
 504                         return 1;
 505                 case 5:
 506                         this_->debug_idx++;
 507                         this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 16));
 508                         return 1;
 509                 case 6:
 510                         this_->debug_idx++;
 511                         this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr);
 512                         return 1;
 513                 default:
 514                         this_->attr_next=attr_none;
 515                         return 0;
 516                 }
 517         case attr_any:
 518                 while (this_->attr_next != attr_none) {
 519                         if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
 520                                 return 1;
 521                 }
 522                 return 0;
 523         default:
 524                 attr->type=attr_none;
 525                 return 0;
 526         }
 527 }
 528
 529 static struct item_methods tracking_map_item_methods = {
 530         NULL,
 531         tracking_map_item_coord_get,
 532         NULL,
 533         tracking_map_item_attr_get,
 534 };
 535
 536
 537 static void
 538 tracking_map_destroy(struct map_priv *priv)
 539 {
 540         g_free(priv);
 541 }
 542
 543 static void
 544 tracking_map_rect_init(struct map_rect_priv *priv)
 545 {
 546         priv->next=priv->tracking->lines;
 547         priv->curr=NULL;
 548         priv->coord=0;
 549         priv->item.id_lo=0;
 550         priv->item.id_hi=0;
 551 }
 552
 553 static struct map_rect_priv *
 554 tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
 555 {
 556         struct tracking *tracking=priv->tracking;
 557         struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
 558         ret->tracking=tracking;
 559         tracking_map_rect_init(ret);
 560         ret->item.meth=&tracking_map_item_methods;
 561         ret->item.priv_data=ret;
 562         ret->item.type=type_tracking_100;
 563         return ret;
 564 }
 565
 566 static void
 567 tracking_map_rect_destroy(struct map_rect_priv *priv)
 568 {
 569         g_free(priv);
 570 }
 571
 572 static struct item *
 573 tracking_map_get_item(struct map_rect_priv *priv)
 574 {
 575         struct item *ret=&priv->item;
 576         int value;
 577         struct coord lpnt;
 578
 579         if (!priv->next)
 580                 return NULL;
 581         if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) {
 582                 priv->curr=priv->next;
 583                 priv->next=priv->curr->next;
 584                 priv->coord=0;
 585                 priv->item.id_lo=0;
 586                 priv->item.id_hi++;
 587         } else {
 588                 priv->coord++;
 589                 priv->item.id_lo++;
 590         }
 591         value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1);
 592         if (value < 64)
 593                 priv->item.type=type_tracking_100;
 594         else if (value < 128)
 595                 priv->item.type=type_tracking_90;
 596         else if (value < 256)
 597                 priv->item.type=type_tracking_80;
 598         else if (value < 512)
 599                 priv->item.type=type_tracking_70;
 600         else if (value < 1024)
 601                 priv->item.type=type_tracking_60;
 602         else if (value < 2048)
 603                 priv->item.type=type_tracking_50;
 604         else if (value < 4096)
 605                 priv->item.type=type_tracking_40;
 606         else if (value < 8192)
 607                 priv->item.type=type_tracking_30;
 608         else if (value < 16384)
 609                 priv->item.type=type_tracking_20;
 610         else if (value < 32768)
 611                 priv->item.type=type_tracking_10;
 612         else
 613                 priv->item.type=type_tracking_0;
 614         dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count);
 615         priv->ccount=0;
 616         priv->attr_next=attr_debug;
 617         priv->debug_idx=0;
 618         return ret;
 619 }
 620
 621 static struct item *
 622 tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
 623 {
 624         struct item *ret;
 625         tracking_map_rect_init(priv);
 626         while ((ret=tracking_map_get_item(priv))) {
 627                 if (ret->id_hi == id_hi && ret->id_lo == id_lo)
 628                         return ret;
 629         }
 630         return NULL;
 631 }
 632
 633 static struct map_methods tracking_map_meth = {
 634         projection_mg,
 635         "utf-8",
 636         tracking_map_destroy,
 637         tracking_map_rect_new,
 638         tracking_map_rect_destroy,
 639         tracking_map_get_item,
 640         tracking_map_get_item_byid,
 641         NULL,
 642         NULL,
 643         NULL,
 644 };
 645
 646 static struct map_priv *
 647 tracking_map_new(struct map_methods *meth, struct attr **attrs)
 648 {
 649         struct map_priv *ret;
 650         struct attr *tracking_attr;
 651
 652         tracking_attr=attr_search(attrs, NULL, attr_trackingo);
 653         if (! tracking_attr)
 654                 return NULL;
 655         ret=g_new0(struct map_priv, 1);
 656         *meth=tracking_map_meth;
 657         ret->tracking=tracking_attr->u.tracking;
 658
 659         return ret;
 660 }
 661
 662
 663 void
 664 tracking_init(void)
 665 {
 666         plugin_register_map_type("tracking", tracking_map_new);
 667 }