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 map *map;
  53 #if 0
  54         struct transformation t;
  55 #endif
  56         struct coord last_updated;
  57         struct tracking_line *lines;
  58 #if 0
  59         struct tracking_line **last_ptr;
  60 #endif
  61         struct tracking_line *curr_line;
  62         int pos;
  63         struct coord curr[2], curr_in, curr_out;
  64         int curr_angle;
  65         struct coord last[2], last_in, last_out;
  66 };
  67
  68
  69 int angle_factor=30;
  70 int connected_pref=-10;
  71 int nostop_pref=10;
  72 int offroad_limit_pref=5000;
  73
  74
  75 struct coord *
  76 tracking_get_pos(struct tracking *tr)
  77 {
  78         return &tr->curr_out;
  79 }
  80
  81 int
  82 tracking_get_segment_pos(struct tracking *tr)
  83 {
  84         return tr->pos;
  85 }
  86
  87 struct street_data *
  88 tracking_get_street_data(struct tracking *tr)
  89 {
  90         return tr->curr_line->street;
  91 }
  92
  93 int
  94 tracking_get_current_attr(struct tracking *_this, enum attr_type type, struct attr *attr)
  95 {
  96         struct item *item;
  97         struct map_rect *mr;
  98         int result=0;
  99         if (! _this->curr_line || ! _this->curr_line->street)
 100                 return 0;
 101         item=&_this->curr_line->street->item;
 102         mr=map_rect_new(item->map,NULL);
 103         item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
 104         if (item_attr_get(item, type, attr))
 105                 result=1;
 106         map_rect_destroy(mr);
 107         return result;
 108 }
 109
 110 struct item *
 111 tracking_get_current_item(struct tracking *_this)
 112 {
 113         if (! _this->curr_line || ! _this->curr_line->street)
 114                 return NULL;
 115         return &_this->curr_line->street->item;
 116 }
 117
 118 static void
 119 tracking_get_angles(struct tracking_line *tl)
 120 {
 121         int i;
 122         struct street_data *sd=tl->street;
 123         for (i = 0 ; i < sd->count-1 ; i++)
 124                 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
 125 }
 126
 127 static void
 128 tracking_doupdate_lines(struct tracking *tr, struct coord *cc)
 129 {
 130         int max_dist=1000;
 131         struct map_selection *sel=route_rect(18, cc, cc, 0, max_dist);
 132         struct mapset_handle *h;
 133         struct map *m;
 134         struct map_rect *mr;
 135         struct item *item;
 136         struct street_data *street;
 137         struct tracking_line *tl;
 138 #if 0
 139         struct coord c;
 140 #endif
 141
 142         dbg(1,"enter\n");
 143         h=mapset_open(tr->ms);
 144         while ((m=mapset_next(h,1))) {
 145                 mr=map_rect_new(m, sel);
 146                 if (! mr)
 147                         continue;
 148                 while ((item=map_rect_get_item(mr))) {
 149                         if (item->type >= type_street_0 && item->type <= type_ferry) {
 150                                 street=street_get_data(item);
 151                                 tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
 152                                 tl->street=street;
 153                                 tracking_get_angles(tl);
 154                                 tl->next=tr->lines;
 155                                 tr->lines=tl;
 156                         }
 157                 }
 158                 map_rect_destroy(mr);
 159         }
 160         mapset_close(h);
 161         map_selection_destroy(sel);
 162         dbg(1, "exit\n");
 163 #if 0
 164
 165         struct transformation t;
 166
 167         tr->last_ptr=&tr->lines;
 168         transform_setup_source_rect_limit(&t,c,1000);
 169         transform_setup_source_rect_limit(&tr->t,c,1000);
 170
 171
 172         profile_timer(NULL);
 173         street_get_block(tr->ma,&t,tst_callback,tr);
 174         profile_timer("end");
 175 #endif
 176 }
 177
 178
 179 static void
 180 tracking_free_lines(struct tracking *tr)
 181 {
 182         struct tracking_line *tl=tr->lines,*next;
 183         dbg(1,"enter(tr=%p)\n", tr);
 184
 185         while (tl) {
 186                 next=tl->next;
 187                 street_data_free(tl->street);
 188                 g_free(tl);
 189                 tl=next;
 190         }
 191         tr->lines=NULL;
 192 }
 193
 194 static int
 195 tracking_angle_abs_diff(int a1, int a2, int full)
 196 {
 197         int ret;
 198
 199         if (a2 > a1)
 200                 ret=(a2-a1)%full;
 201         else
 202                 ret=(a1-a2)%full;
 203         if (ret > full/2)
 204                 ret=full-ret;
 205         return ret;
 206 }
 207
 208 static int
 209 tracking_angle_delta(int vehicle_angle, int street_angle, int flags)
 210 {
 211         int full=180;
 212         int ret;
 213         if (flags) {
 214                 full=360;
 215                 if (flags & 2) {
 216                         street_angle=(street_angle+180)%360;
 217                         if (flags & 1)
 218                                 return 360*360;
 219                 }
 220         }
 221         ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
 222
 223         return ret*ret;
 224 }
 225
 226 static int
 227 tracking_is_connected(struct coord *c1, struct coord *c2)
 228 {
 229         if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
 230                 return 1;
 231         if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
 232                 return 1;
 233         if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
 234                 return 1;
 235         if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
 236                 return 1;
 237         return 0;
 238 }
 239
 240 static int
 241 tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int flags)
 242 {
 243         int value=0;
 244         struct street_data *sd=t->street;
 245         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);
 246         if (flags & 1) {
 247                 value+=transform_distance_line_sq(&sd->c[offset], &sd->c[offset+1], &tr->curr_in, lpnt);
 248                 if (value >= INT_MAX/2)
 249                         return value;
 250         }
 251         if (flags & 2)
 252                 value += tracking_angle_delta(tr->curr_angle, t->angle[offset], sd->flags)*angle_factor>>4;
 253         if (flags & 4) {
 254                 if (tracking_is_connected(tr->last, &sd->c[offset]))
 255                         value += connected_pref;
 256         }
 257         if (flags & 8) {
 258                 if (lpnt->x == tr->last_out.x && lpnt->y == tr->last_out.y)
 259                         value += nostop_pref;
 260         }
 261         return value;
 262 }
 263
 264
 265
 266 int
 267 tracking_update(struct tracking *tr, struct coord *c, int angle)
 268 {
 269         struct tracking_line *t;
 270         int i,value,min=0;
 271         struct coord lpnt;
 272 #if 0
 273         int min,dist;
 274         int debug=0;
 275 #endif
 276         dbg(1,"enter(%p,%p,%d)\n", tr, c, angle);
 277         dbg(1,"c=0x%x,0x%x\n", c->x, c->y);
 278
 279         if (c->x == tr->curr_in.x && c->y == tr->curr_in.y) {
 280                 if (tr->curr_out.x && tr->curr_out.y)
 281                         *c=tr->curr_out;
 282                 return 0;
 283         }
 284         tr->last_in=tr->curr_in;
 285         tr->last_out=tr->curr_out;
 286         tr->last[0]=tr->curr[0];
 287         tr->last[1]=tr->curr[1];
 288         tr->curr_in=*c;
 289         tr->curr_angle=angle;
 290         if (!tr->lines || transform_distance_sq(&tr->last_updated, c) > 250000) {
 291                 dbg(1, "update\n");
 292                 tracking_free_lines(tr);
 293                 tracking_doupdate_lines(tr, c);
 294                 tr->last_updated=*c;
 295                 dbg(1,"update end\n");
 296         }
 297
 298         t=tr->lines;
 299         if (! t)
 300                 return 0;
 301         tr->curr_line=NULL;
 302         while (t) {
 303                 struct street_data *sd=t->street;
 304                 if ((sd->flags & 3) == 3) {
 305                         t=t->next;
 306                         continue;
 307                 }
 308                 for (i = 0; i < sd->count-1 ; i++) {
 309                         value=tracking_value(tr,t,i,&lpnt,-1);
 310                         if (value < INT_MAX/2 && (! tr->curr_line || value < min)) {
 311                                 tr->curr_line=t;
 312                                 tr->pos=i;
 313                                 tr->curr[0]=sd->c[i];
 314                                 tr->curr[1]=sd->c[i+1];
 315                                 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
 316                                         transform_distance_line_sq(&sd->c[i], &sd->c[i+1], c, &lpnt),
 317                                         tracking_angle_delta(angle, t->angle[i], 0)*angle_factor,
 318                                         tracking_is_connected(tr->last, &sd->c[i]) ? connected_pref : 0,
 319                                         lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0,
 320                                         value
 321                                 );
 322                                 tr->curr_out=lpnt;
 323                                 min=value;
 324                         }
 325                 }
 326                 t=t->next;
 327         }
 328         dbg(0,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
 329         if (!tr->curr_line || min > offroad_limit_pref)
 330                 return 0;
 331         dbg(0,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
 332         *c=tr->curr_out;
 333         return 1;
 334 }
 335
 336 struct tracking *
 337 tracking_new(struct mapset *ms)
 338 {
 339         struct tracking *this=g_new0(struct tracking, 1);
 340         this->ms=ms;
 341
 342         return this;
 343 }
 344
 345 void
 346 tracking_set_mapset(struct tracking *this, struct mapset *ms)
 347 {
 348         this->ms=ms;
 349 }
 350
 351 void
 352 tracking_destroy(struct tracking *tr)
 353 {
 354         tracking_free_lines(tr);
 355         g_free(tr);
 356 }
 357
 358 struct map *
 359 tracking_get_map(struct tracking *this_)
 360 {
 361         if (! this_->map)
 362                 this_->map=map_new((struct attr*[]){
 363                         &(struct attr){attr_type,{"tracking"}},
 364                         &(struct attr){attr_trackingo,.u.tracking=this_},
 365                         &(struct attr){attr_data,{""}},
 366                         &(struct attr){attr_description,{"Tracking"}},
 367                         NULL});
 368         return this_->map;
 369 }
 370
 371
 372 struct map_priv {
 373         struct tracking *tracking;
 374 };
 375
 376 struct map_rect_priv {
 377         struct tracking *tracking;
 378         struct item item;
 379         struct tracking_line *curr,*next;
 380         int coord;
 381         enum attr_type attr_next;
 382         int ccount;
 383         int debug_idx;
 384         char *str;
 385 };
 386
 387 static int
 388 tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
 389 {
 390         struct map_rect_priv *this=priv_data;
 391         int ret=0;
 392         dbg(1,"enter\n");
 393         while (this->ccount < 2 && count > 0) {
 394                 *c=this->curr->street->c[this->ccount+this->coord];
 395                 dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y);
 396                 this->ccount++;
 397                 ret++;
 398                 c++;
 399                 count--;
 400         }
 401         return ret;
 402 }
 403
 404 static int
 405 tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
 406 {
 407         struct map_rect_priv *this_=priv_data;
 408         attr->type=attr_type;
 409         struct coord lpnt,*c;
 410         struct tracking *tr=this_->tracking;
 411         int value;
 412
 413         if (this_->str) {
 414                 g_free(this_->str);
 415                 this_->str=NULL;
 416         }
 417
 418         switch(attr_type) {
 419         case attr_debug:
 420                 switch(this_->debug_idx) {
 421                 case 0:
 422                         this_->debug_idx++;
 423                         this_->str=attr->u.str=g_strdup_printf("overall: %d",tracking_value(tr, this_->curr, this_->coord, &lpnt, -1));
 424                         return 1;
 425                 case 1:
 426                         this_->debug_idx++;
 427                         c=&this_->curr->street->c[this_->coord];
 428                         value=tracking_value(tr, this_->curr, this_->coord, &lpnt, 1);
 429                         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",
 430                                 tr->curr_in.x, tr->curr_in.y,
 431                                 c[0].x, c[0].y, c[1].x, c[1].y,
 432                                 lpnt.x, lpnt.y, value);
 433                         return 1;
 434                 case 2:
 435                         this_->debug_idx++;
 436                         this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d",
 437                                 tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
 438                                 tracking_value(tr, this_->curr, this_->coord, &lpnt, 2));
 439                         return 1;
 440                 case 3:
 441                         this_->debug_idx++;
 442                         this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr);
 443                         return 1;
 444                 default:
 445                         this_->attr_next=attr_none;
 446                         return 0;
 447                 }
 448         case attr_any:
 449                 while (this_->attr_next != attr_none) {
 450                         if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
 451                                 return 1;
 452                 }
 453                 return 0;
 454         default:
 455                 attr->type=attr_none;
 456                 return 0;
 457         }
 458 }
 459
 460 static struct item_methods tracking_map_item_methods = {
 461         NULL,
 462         tracking_map_item_coord_get,
 463         NULL,
 464         tracking_map_item_attr_get,
 465 };
 466
 467
 468 static void
 469 tracking_map_destroy(struct map_priv *priv)
 470 {
 471         g_free(priv);
 472 }
 473
 474 static void
 475 tracking_map_rect_init(struct map_rect_priv *priv)
 476 {
 477         priv->next=priv->tracking->lines;
 478         priv->curr=NULL;
 479         priv->coord=0;
 480         priv->item.id_lo=0;
 481         priv->item.id_hi=0;
 482 }
 483
 484 static struct map_rect_priv *
 485 tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
 486 {
 487         struct tracking *tracking=priv->tracking;
 488         struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
 489         ret->tracking=tracking;
 490         tracking_map_rect_init(ret);
 491         ret->item.meth=&tracking_map_item_methods;
 492         ret->item.priv_data=ret;
 493         ret->item.type=type_tracking_100;
 494         return ret;
 495 }
 496
 497 static void
 498 tracking_map_rect_destroy(struct map_rect_priv *priv)
 499 {
 500         g_free(priv);
 501 }
 502
 503 static struct item *
 504 tracking_map_get_item(struct map_rect_priv *priv)
 505 {
 506         struct item *ret=&priv->item;
 507         int value;
 508         struct coord lpnt;
 509
 510         if (!priv->next)
 511                 return NULL;
 512         if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) {
 513                 priv->curr=priv->next;
 514                 priv->next=priv->curr->next;
 515                 priv->coord=0;
 516                 priv->item.id_lo=0;
 517                 priv->item.id_hi++;
 518         } else {
 519                 priv->coord++;
 520                 priv->item.id_lo++;
 521         }
 522         value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, -1);
 523         if (value < 64)
 524                 priv->item.type=type_tracking_100;
 525         else if (value < 128)
 526                 priv->item.type=type_tracking_90;
 527         else if (value < 256)
 528                 priv->item.type=type_tracking_80;
 529         else if (value < 512)
 530                 priv->item.type=type_tracking_70;
 531         else if (value < 1024)
 532                 priv->item.type=type_tracking_60;
 533         else if (value < 2048)
 534                 priv->item.type=type_tracking_50;
 535         else if (value < 4096)
 536                 priv->item.type=type_tracking_40;
 537         else if (value < 8192)
 538                 priv->item.type=type_tracking_30;
 539         else if (value < 16384)
 540                 priv->item.type=type_tracking_20;
 541         else if (value < 32768)
 542                 priv->item.type=type_tracking_10;
 543         else
 544                 priv->item.type=type_tracking_0;
 545         dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count);
 546         priv->ccount=0;
 547         priv->attr_next=attr_debug;
 548         priv->debug_idx=0;
 549         return ret;
 550 }
 551
 552 static struct item *
 553 tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
 554 {
 555         struct item *ret;
 556         tracking_map_rect_init(priv);
 557         while ((ret=tracking_map_get_item(priv))) {
 558                 if (ret->id_hi == id_hi && ret->id_lo == id_lo)
 559                         return ret;
 560         }
 561         return NULL;
 562 }
 563
 564 static struct map_methods tracking_map_meth = {
 565         projection_mg,
 566         "utf-8",
 567         tracking_map_destroy,
 568         tracking_map_rect_new,
 569         tracking_map_rect_destroy,
 570         tracking_map_get_item,
 571         tracking_map_get_item_byid,
 572         NULL,
 573         NULL,
 574         NULL,
 575 };
 576
 577 static struct map_priv *
 578 tracking_map_new(struct map_methods *meth, struct attr **attrs)
 579 {
 580         struct map_priv *ret;
 581         struct attr *tracking_attr;
 582
 583         tracking_attr=attr_search(attrs, NULL, attr_trackingo);
 584         if (! tracking_attr)
 585                 return NULL;
 586         ret=g_new0(struct map_priv, 1);
 587         *meth=tracking_map_meth;
 588         ret->tracking=tracking_attr->u.tracking;
 589
 590         return ret;
 591 }
 592
 593
 594 void
 595 tracking_init(void)
 596 {
 597         plugin_register_map_type("tracking", tracking_map_new);
 598 }