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
  33 struct tracking_line
  34 {
  35         struct street_data *street;
  36 #if 0
  37         long segid;
  38         int linenum;
  39         struct coord c[2];
  40         struct coord lpnt;
  41         int value;
  42         int dir;
  43 #endif
  44         struct tracking_line *next;
  45         int angle[0];
  46 };
  47
  48
  49 struct tracking {
  50         struct mapset *ms;
  51 #if 0
  52         struct transformation t;
  53 #endif
  54         struct coord last_updated;
  55         struct tracking_line *lines;
  56 #if 0
  57         struct tracking_line **last_ptr;
  58 #endif
  59         struct tracking_line *curr_line;
  60         int pos;
  61         struct coord curr[2];
  62         struct coord last_in;
  63         struct coord last_out;
  64 };
  65
  66
  67 int angle_factor=30;
  68 int connected_pref=-10;
  69 int nostop_pref=10;
  70 int offroad_limit_pref=5000;
  71
  72
  73 struct coord *
  74 tracking_get_pos(struct tracking *tr)
  75 {
  76         return &tr->last_out;
  77 }
  78
  79 int
  80 tracking_get_segment_pos(struct tracking *tr)
  81 {
  82         return tr->pos;
  83 }
  84
  85 struct street_data *
  86 tracking_get_street_data(struct tracking *tr)
  87 {
  88         return tr->curr_line->street;
  89 }
  90
  91 int
  92 tracking_get_current_attr(struct tracking *_this, enum attr_type type, struct attr *attr)
  93 {
  94         struct item *item;
  95         struct map_rect *mr;
  96         int result=0;
  97         if (! _this->curr_line || ! _this->curr_line->street)
  98                 return 0;
  99         item=&_this->curr_line->street->item;
 100         mr=map_rect_new(item->map,NULL);
 101         item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
 102         if (item_attr_get(item, type, attr))
 103                 result=1;
 104         map_rect_destroy(mr);
 105         return result;
 106 }
 107
 108 struct item *
 109 tracking_get_current_item(struct tracking *_this)
 110 {
 111         if (! _this->curr_line || ! _this->curr_line->street)
 112                 return NULL;
 113         return &_this->curr_line->street->item;
 114 }
 115
 116 static void
 117 tracking_get_angles(struct tracking_line *tl)
 118 {
 119         int i;
 120         struct street_data *sd=tl->street;
 121         for (i = 0 ; i < sd->count-1 ; i++)
 122                 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
 123 }
 124
 125 static void
 126 tracking_doupdate_lines(struct tracking *tr, struct coord *cc)
 127 {
 128         int max_dist=1000;
 129         struct map_selection *sel=route_rect(18, cc, cc, 0, max_dist);
 130         struct mapset_handle *h;
 131         struct map *m;
 132         struct map_rect *mr;
 133         struct item *item;
 134         struct street_data *street;
 135         struct tracking_line *tl;
 136 #if 0
 137         struct coord c;
 138 #endif
 139
 140         dbg(1,"enter\n");
 141         h=mapset_open(tr->ms);
 142         while ((m=mapset_next(h,1))) {
 143                 mr=map_rect_new(m, sel);
 144                 if (! mr)
 145                         continue;
 146                 while ((item=map_rect_get_item(mr))) {
 147                         if (item->type >= type_street_0 && item->type <= type_ferry) {
 148                                 street=street_get_data(item);
 149                                 tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
 150                                 tl->street=street;
 151                                 tracking_get_angles(tl);
 152                                 tl->next=tr->lines;
 153                                 tr->lines=tl;
 154                         }
 155                 }
 156                 map_rect_destroy(mr);
 157         }
 158         mapset_close(h);
 159         map_selection_destroy(sel);
 160         dbg(1, "exit\n");
 161 #if 0
 162
 163         struct transformation t;
 164
 165         tr->last_ptr=&tr->lines;
 166         transform_setup_source_rect_limit(&t,c,1000);
 167         transform_setup_source_rect_limit(&tr->t,c,1000);
 168
 169
 170         profile_timer(NULL);
 171         street_get_block(tr->ma,&t,tst_callback,tr);
 172         profile_timer("end");
 173 #endif
 174 }
 175
 176
 177 static void
 178 tracking_free_lines(struct tracking *tr)
 179 {
 180         struct tracking_line *tl=tr->lines,*next;
 181         dbg(1,"enter(tr=%p)\n", tr);
 182
 183         while (tl) {
 184                 next=tl->next;
 185                 street_data_free(tl->street);
 186                 g_free(tl);
 187                 tl=next;
 188         }
 189         tr->lines=NULL;
 190 }
 191
 192 static int
 193 tracking_angle_abs_diff(int a1, int a2, int full)
 194 {
 195         int ret;
 196
 197         if (a2 > a1)
 198                 ret=(a2-a1)%full;
 199         else
 200                 ret=(a1-a2)%full;
 201         if (ret > full/2)
 202                 ret=full-ret;
 203         return ret;
 204 }
 205
 206 static int
 207 tracking_angle_delta(int vehicle_angle, int street_angle, int dir)
 208 {
 209         int full=180;
 210         int ret;
 211         if (dir) {
 212                 full=360;
 213                 if (dir < 0)
 214                         street_angle=(street_angle+180)%360;
 215         }
 216         ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
 217
 218         return ret*ret;
 219 }
 220
 221 static int
 222 tracking_is_connected(struct coord *c1, struct coord *c2)
 223 {
 224         if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
 225                 return 1;
 226         if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
 227                 return 1;
 228         if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
 229                 return 1;
 230         if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
 231                 return 1;
 232         return 0;
 233 }
 234
 235 int
 236 tracking_update(struct tracking *tr, struct coord *c, int angle)
 237 {
 238         struct tracking_line *t;
 239         int i,value,min=0;
 240         struct coord lpnt;
 241 #if 0
 242         int min,dist;
 243         int debug=0;
 244 #endif
 245         dbg(1,"enter(%p,%p,%d)\n", tr, c, angle);
 246         dbg(1,"c=0x%x,0x%x\n", c->x, c->y);
 247
 248         if (c->x == tr->last_in.x && c->y == tr->last_in.y) {
 249                 if (tr->last_out.x && tr->last_out.y)
 250                         *c=tr->last_out;
 251                 return 0;
 252         }
 253         tr->last_in=*c;
 254         if (!tr->lines || transform_distance_sq(&tr->last_updated, c) > 250000) {
 255                 dbg(1, "update\n");
 256                 tracking_free_lines(tr);
 257                 tracking_doupdate_lines(tr, c);
 258                 tr->last_updated=*c;
 259                 dbg(1,"update end\n");
 260         }
 261
 262         t=tr->lines;
 263         if (! t)
 264                 return 0;
 265         tr->curr_line=NULL;
 266         while (t) {
 267                 struct street_data *sd=t->street;
 268                 int dir = 0;
 269                 switch(sd->flags & AF_ONEWAYMASK) {
 270                 case 0:
 271                         dir=0;
 272                         break;
 273                 case 1:
 274                         dir=1;
 275                         break;
 276                 case 2:
 277                         dir=-1;
 278                         break;
 279                 case 3:
 280                         t=t->next;
 281                         continue;
 282                 }
 283                 for (i = 0; i < sd->count-1 ; i++) {
 284                         dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", i, sd->c[i].x, sd->c[i].y, sd->c[i+1].x, sd->c[i+1].y);
 285                         value=transform_distance_line_sq(&sd->c[i], &sd->c[i+1], c, &lpnt);
 286                         if (value < INT_MAX/2) {
 287                                 value += tracking_angle_delta(angle, t->angle[i], dir)*angle_factor>>4;
 288                                 if (tracking_is_connected(tr->curr, &sd->c[i]))
 289                                         value += connected_pref;
 290                                 if (lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y)
 291                                         value += nostop_pref;
 292                                 if (! tr->curr_line || value < min) {
 293                                         tr->curr_line=t;
 294                                         tr->pos=i;
 295                                         tr->curr[0]=sd->c[i];
 296                                         tr->curr[1]=sd->c[i+1];
 297                                         dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
 298                                                 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], c, &lpnt),
 299                                                 tracking_angle_delta(angle, t->angle[i], 0)*angle_factor,
 300                                                 tracking_is_connected(tr->curr, &sd->c[i]) ? connected_pref : 0,
 301                                                 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0,
 302                                                 value
 303                                         );
 304                                         tr->last_out=lpnt;
 305                                         min=value;
 306                                 }
 307
 308                          }
 309                 }
 310                 t=t->next;
 311         }
 312         dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
 313         if (!tr->curr_line || min > offroad_limit_pref)
 314                 return 0;
 315         dbg(1,"found 0x%x,0x%x\n", tr->last_out.x, tr->last_out.y);
 316         *c=tr->last_out;
 317         return 1;
 318 }
 319
 320 struct tracking *
 321 tracking_new(struct mapset *ms)
 322 {
 323         struct tracking *this=g_new0(struct tracking, 1);
 324         this->ms=ms;
 325
 326         return this;
 327 }
 328
 329 void
 330 tracking_set_mapset(struct tracking *this, struct mapset *ms)
 331 {
 332         this->ms=ms;
 333 }
 334
 335 void
 336 tracking_destroy(struct tracking *tr)
 337 {
 338         tracking_free_lines(tr);
 339         g_free(tr);
 340 }