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

   1 #include <stdio.h>
   2 #include <glib.h>
   3 #include "coord.h"
   4 #include "block.h"
   5 #include "street.h"
   6 #include "profile.h"
   7 #include "track.h"
   8
   9
  10 struct track_line
  11 {
  12         long segid;
  13         int linenum;
  14         struct coord c[2];
  15         struct coord lpnt;
  16         int value;
  17         int angle;
  18         int dir;
  19         struct track_line *next;
  20 };
  21
  22 struct track {
  23         struct map_data *ma;
  24         struct transformation t;
  25         struct coord last_updated;
  26         struct track_line *lines;
  27         struct track_line **last_ptr;
  28         struct coord curr[2];
  29         struct coord last_in;
  30         struct coord last_out;
  31 };
  32
  33 int angle_factor=5;
  34 int connected_pref=-10;
  35 int nostop_pref=10;
  36
  37
  38 struct track_line **last;
  39
  40 static void
  41 tst_callback(struct street_str *str, void *handle, void *data)
  42 {
  43         struct coord c[2];
  44         int visible=0,count=0;
  45         struct track *tr=data;
  46         struct track_line *lines;
  47         int debug_segid=0;
  48         int debug=0;
  49
  50         /* printf("tst_callback id=0x%x ",str->segid < 0 ? -str->segid : str->segid); */
  51         if (street_coord_handle_get(handle, &c[0], 1)) {
  52                 if (str->segid == debug_segid)
  53                         printf("0x%lx,0x%lx ", c->x, c->y);
  54                 c[1]=c[0];
  55                 while (street_coord_handle_get(handle, &c[0], 1)) {
  56                         if (is_line_visible(&tr->t, c)) {
  57                                 visible=1;
  58                         }
  59                         c[1]=c[0];
  60                         count++;
  61                         if (str->segid == debug_segid)
  62                                 printf("0x%lx,0x%lx ", c->x, c->y);
  63                 }
  64         }
  65         if (visible) {
  66                 lines=g_new(struct track_line, count);
  67                 street_coord_handle_rewind(handle);
  68                 street_coord_handle_get(handle, &c[0], 1);
  69                 count=0;
  70                 while (street_coord_handle_get(handle, &c[1], 1)) {
  71                         *(tr->last_ptr)=lines;
  72                         tr->last_ptr=&lines->next;
  73                         lines->segid=str->segid;
  74                         lines->linenum=count;
  75                         lines->c[0]=c[0];
  76                         lines->c[1]=c[1];
  77                         lines->angle=transform_get_angle(c,0);
  78                         lines->next=NULL;
  79                         lines++;
  80                         count++;
  81                         c[0]=c[1];
  82                 }
  83                 if (debug)
  84                         printf("%d lines\n", count);
  85         }
  86         /* printf("\n"); */
  87 }
  88
  89 static void
  90 track_doupdate_lines(struct track *tr, struct coord *c)
  91 {
  92
  93         struct transformation t;
  94
  95         tr->last_ptr=&tr->lines;
  96         transform_setup_source_rect_limit(&t,c,1000);
  97         transform_setup_source_rect_limit(&tr->t,c,1000);
  98
  99
 100         profile_timer(NULL);
 101         street_get_block(tr->ma,&t,tst_callback,tr);
 102         profile_timer("end");
 103 }
 104
 105 static void
 106 track_free_lines(struct track *tr)
 107 {
 108         struct track_line *tl=tr->lines,*next;
 109 #ifdef DEBUG
 110         printf("track_free_lines(tr=%p)\n", tr);
 111 #endif
 112         while (tl) {
 113                 next=tl->next;
 114                 if (! tl->linenum) {
 115                         g_free(tl);
 116                 }
 117                 tl=next;
 118         }
 119         tr->lines=NULL;
 120 }
 121
 122 static int
 123 track_angle_abs_diff(int a1, int a2, int full)
 124 {
 125         int ret;
 126
 127         if (a2 > a1)
 128                 ret=(a2-a1)%full;
 129         else
 130                 ret=(a1-a2)%full;
 131         if (ret > full/2)
 132                 ret=full-ret;
 133         return ret;
 134 }
 135
 136 static int
 137 track_angle_delta(int a1, int a2, int dir)
 138 {
 139         if (! dir)
 140                 return track_angle_abs_diff(a1, a2, 180);
 141         else
 142                 return track_angle_abs_diff(a1, a2, 360);
 143 }
 144
 145 static int
 146 track_is_connected(struct coord *c1, struct coord *c2)
 147 {
 148         if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
 149                 return 1;
 150         if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
 151                 return 1;
 152         if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
 153                 return 1;
 154         if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
 155                 return 1;
 156         return 0;
 157 }
 158
 159 void
 160 track_update(struct track *tr, struct coord *c, int angle)
 161 {
 162         struct track_line *t,*tm;
 163         int min,dist;
 164         int debug=0;
 165
 166         if (c->x == tr->last_in.x && c->y == tr->last_in.y) {
 167                 *c=tr->last_out;
 168                 return;
 169         }
 170         if (transform_distance_sq(&tr->last_updated, c) > 250000 || !tr->lines) {
 171                 printf("update\n");
 172                 track_free_lines(tr);
 173                 track_doupdate_lines(tr, c);
 174                 tr->last_updated=*c;
 175         }
 176         profile_timer(NULL);
 177
 178         t=tr->lines;
 179         g_assert(t != NULL);
 180
 181         if (debug) printf("0x%lx,0x%lx (%d deg)\n", c->x, c->y, angle);
 182         while (t) {
 183                 if (debug) printf("0x%lx 0x%lx,0x%lx - 0x%lx,0x%lx (%d deg) ", -t->segid, t->c[0].x, t->c[0].y, t->c[1].x, t->c[1].y, t->angle);
 184                 t->value=transform_distance_line_sq(&t->c[0], &t->c[1], c, &t->lpnt);
 185                 if (t->value < INT_MAX/2)
 186                          t->value += track_angle_delta(angle, t->angle, 0)*angle_factor;
 187                 if (track_is_connected(tr->curr, t->c))
 188                         t->value += connected_pref;
 189                 if (t->lpnt.x == tr->last_out.x && t->lpnt.y == tr->last_out.y)
 190                         t->value += nostop_pref;
 191                 if (debug) printf(" %d\n", t->value);
 192                 t=t->next;
 193         }
 194         t=tr->lines;
 195         tm=t;
 196         min=t->value;
 197         while (t) {
 198                 if (t->value < min) {
 199                         min=t->value;
 200                         tm=t;
 201                 }
 202                 t=t->next;
 203         }
 204         dist=transform_distance_sq(&tm->lpnt, c);
 205         if (debug) printf("dist=%d id=0x%lx\n", dist, tm->segid);
 206         *c=tm->lpnt;
 207         tr->curr[0]=tm->c[0];
 208         tr->curr[1]=tm->c[1];
 209         tr->last_out=tm->lpnt;
 210
 211         // printf("pos 0x%lx,0x%lx value %d dist %d angle %d vs %d (%d)\n", c->x, c->y, tm->value, dist, angle, tm->angle, track_angle_delta(angle, tm->angle, 0));
 212         g_assert(dist < 10000);
 213 #if 0
 214         profile_timer("track_update end");
 215 #endif
 216
 217 }
 218
 219 struct track *
 220 track_new(struct map_data *ma)
 221 {
 222         struct track *this=g_new0(struct track, 1);
 223         this->ma=ma;
 224
 225         return this;
 226 }
 227
 228 void
 229 track_destroy(struct track *tr)
 230 {
 231         track_free_lines(tr);
 232         g_free(tr);
 233 }