/[zanavi_public1]/navit/navit/track.c
ZANavi

Diff of /navit/navit/track.c

Parent Directory Parent Directory | Revision Log Revision Log | View Patch Patch

Revision 28 Revision 31
… …
62 struct street_data *street; 62 struct street_data *street;
63 struct tracking_line *next; 63 struct tracking_line *next;
64 int angle[0]; 64 int angle[0];
65}; 65};
66 66
67
68/** 67/**
69 * @brief Conatins a list of previous speeds 68 * @brief Conatins a list of previous speeds
70 * 69 *
71 * This structure is used to hold a list of previously reported 70 * This structure is used to hold a list of previously reported
72 * speeds. This data is used by the CDF. 71 * speeds. This data is used by the CDF.
73 */ 72 */
74struct cdf_speed { 73struct cdf_speed
74{
75 struct cdf_speed *next; 75 struct cdf_speed *next;
76 int speed; 76 int speed;
77 time_t time; 77 time_t time;
78}; 78};
79 79
… …
81 * @brief Contains data for the CDF 81 * @brief Contains data for the CDF
82 * 82 *
83 * This structure holds all data needed by the 83 * This structure holds all data needed by the
84 * cumulative displacement filter. 84 * cumulative displacement filter.
85 */ 85 */
86struct cdf_data { 86struct cdf_data
87{
87 int extrapolating; 88 int extrapolating;
88 int available; 89 int available;
89 int first_pos; 90 int first_pos;
90 int poscount; 91 int poscount;
91 int hist_size; 92 int hist_size;
92 struct cdf_speed *speed_hist; 93 struct cdf_speed *speed_hist;
93 struct pcoord *pos_hist; 94 struct pcoord *pos_hist;
94 int *dir_hist; 95 int *dir_hist;
95 double last_dist; 96 double last_dist;
96 struct pcoord last_out; 97 struct pcoord last_out;
97 int last_dir; 98 int last_dir;
98}; 99};
99 100
100struct tracking { 101struct tracking
102{
101 struct mapset *ms; 103 struct mapset *ms;
102 struct route *rt; 104 struct route *rt;
103 struct map *map; 105 struct map *map;
104 struct vehicle *vehicle; 106 struct vehicle *vehicle;
105 struct vehicleprofile *vehicleprofile; 107 struct vehicleprofile *vehicleprofile;
… …
130 int overspeed_pref; 132 int overspeed_pref;
131 int overspeed_percent_pref; 133 int overspeed_percent_pref;
132 int tunnel_extrapolation; 134 int tunnel_extrapolation;
133}; 135};
134 136
135
136
137
138static void
139tracking_init_cdf(struct cdf_data *cdf, int hist_size) 137static void tracking_init_cdf(struct cdf_data *cdf, int hist_size)
140{ 138{
141 cdf->extrapolating = 0; 139 cdf->extrapolating = 0;
142 cdf->available = 0; 140 cdf->available = 0;
143 cdf->poscount = 0; 141 cdf->poscount = 0;
144 cdf->last_dist = 0; 142 cdf->last_dist = 0;
145 cdf->hist_size = hist_size; 143 cdf->hist_size = hist_size;
146 144
147 cdf->pos_hist = g_new0(struct pcoord, hist_size); 145cdf->pos_hist = g_new0(struct pcoord, hist_size);
148 cdf->dir_hist = g_new0(int, hist_size); 146cdf->dir_hist = g_new0(int, hist_size);
149} 147}
150 148
151// Variables for finetuning the CDF 149// Variables for finetuning the CDF
152 150
153// Minimum average speed 151// Minimum average speed
154#define CDF_MINAVG 1.f 152#define CDF_MINAVG 1.f
155// Maximum average speed 153// Maximum average speed
156#define CDF_MAXAVG 6.f // only ~ 20 km/h 154#define CDF_MAXAVG 6.f // only ~ 20 km/h
157 // We need a low value here because otherwise we would extrapolate whenever we are not accelerating 155// We need a low value here because otherwise we would extrapolate whenever we are not accelerating
158 156
159// Mininum distance (square of it..), below which we ignore gps updates 157// Mininum distance (square of it..), below which we ignore gps updates
160#define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians. 158#define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
161
162#if 0 159#if 0
163static void 160static void
164tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime) 161tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime)
165{ 162{
166 struct cdf_speed *speed,*sc,*sl; 163struct cdf_speed *speed,*sc,*sl;
167 double speed_avg; 164double speed_avg;
168 int speed_num,i; 165int speed_num,i;
169 166
170 if (cdf->hist_size == 0) { 167if (cdf->hist_size == 0)
168{
171 dbg(1,"No CDF.\n"); 169 dbg(1,"No CDF.\n");
172 *pout = *pin; 170 *pout = *pin;
173 *dirout = dirin; 171 *dirout = dirin;
174 return; 172 return;
175 } 173}
176 174
177 speed = g_new0(struct cdf_speed, 1); 175speed = g_new0(struct cdf_speed, 1);
178 speed->speed = cur_speed; 176speed->speed = cur_speed;
179 speed->time = fixtime; 177speed->time = fixtime;
180 178
181 speed->next = cdf->speed_hist; 179speed->next = cdf->speed_hist;
182 cdf->speed_hist = speed; 180cdf->speed_hist = speed;
183 181
184 sc = speed; 182sc = speed;
185 sl = NULL; 183sl = NULL;
186 speed_num = 0; 184speed_num = 0;
187 speed_avg = 0; 185speed_avg = 0;
188 while (sc && ((fixtime - speed->time) < 4)) { // FIXME static maxtime 186while (sc && ((fixtime - speed->time) < 4))
187{ // FIXME static maxtime
189 speed_num++; 188 speed_num++;
190 speed_avg += sc->speed; 189 speed_avg += sc->speed;
191 sl = sc; 190 sl = sc;
192 sc = sc->next; 191 sc = sc->next;
193 } 192}
194 193
195 speed_avg /= (double)speed_num; 194speed_avg /= (double)speed_num;
196 195
197 if (sl) { 196if (sl)
197{
198 sl->next = NULL; 198 sl->next = NULL;
199 } 199}
200 200
201 while (sc) { 201while (sc)
202{
202 sl = sc->next; 203 sl = sc->next;
203 g_free(sc); 204 g_free(sc);
204 sc = sl; 205 sc = sl;
205 } 206}
206 207
207 if (speed_avg < CDF_MINAVG) { 208if (speed_avg < CDF_MINAVG)
209{
208 speed_avg = CDF_MINAVG; 210 speed_avg = CDF_MINAVG;
211}
209 } else if (speed_avg > CDF_MAXAVG) { 212else if (speed_avg > CDF_MAXAVG)
213{
210 speed_avg = CDF_MAXAVG; 214 speed_avg = CDF_MAXAVG;
211 } 215}
212 216
213
214 if (cur_speed >= speed_avg) { 217if (cur_speed >= speed_avg)
218{
215 if (cdf->extrapolating) { 219 if (cdf->extrapolating)
220 {
216 cdf->poscount = 0; 221 cdf->poscount = 0;
217 cdf->extrapolating = 0; 222 cdf->extrapolating = 0;
223 }
224
225 cdf->first_pos--;
226 if (cdf->first_pos < 0)
227 {
228 cdf->first_pos = cdf->hist_size - 1;
229 }
230
231 if (cdf->poscount < cdf->hist_size)
232 {
233 cdf->poscount++;
234 }
235
236 cdf->pos_hist[cdf->first_pos] = *pin;
237 cdf->dir_hist[cdf->first_pos] = dirin;
238
239 *pout = *pin;
240 *dirout = dirin;
241}
242else if (cdf->poscount > 0)
243{
244
245 double mx,my; // Average position's x and y values
246 double sx,sy; // Support vector
247 double dx,dy; // Difference between average and current position
248 double len; // Length of support vector
249 double dist;
250
251 mx = my = 0;
252 sx = sy = 0;
253
254 for (i = 0; i < cdf->poscount; i++)
255 {
256 mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
257 my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;
258
259 if (i != 0)
260 {
261 sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x;
262 sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y;
218 } 263 }
219 264
220 cdf->first_pos--;
221 if (cdf->first_pos < 0) {
222 cdf->first_pos = cdf->hist_size - 1;
223 } 265 }
224 266
225 if (cdf->poscount < cdf->hist_size) {
226 cdf->poscount++;
227 }
228
229 cdf->pos_hist[cdf->first_pos] = *pin;
230 cdf->dir_hist[cdf->first_pos] = dirin;
231
232 *pout = *pin;
233 *dirout = dirin;
234 } else if (cdf->poscount > 0) {
235
236 double mx,my; // Average position's x and y values
237 double sx,sy; // Support vector
238 double dx,dy; // Difference between average and current position
239 double len; // Length of support vector
240 double dist;
241
242 mx = my = 0;
243 sx = sy = 0;
244
245 for (i = 0; i < cdf->poscount; i++) {
246 mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
247 my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;
248
249
250 if (i != 0) {
251 sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x;
252 sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y;
253 }
254
255 }
256
257 if (cdf->poscount > 1) { 267 if (cdf->poscount > 1)
268 {
258 // Normalize the support vector 269 // Normalize the support vector
259 len = sqrt(sx * sx + sy * sy); 270 len = sqrt(sx * sx + sy * sy);
260 sx /= len; 271 sx /= len;
261 sy /= len; 272 sy /= len;
262 273
263 // Calculate the new direction 274 // Calculate the new direction
264 *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180); 275 *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180);
265 } else { 276 }
277 else
278 {
266 // If we only have one position, we can't use differences of positions, but we have to use the reported 279 // If we only have one position, we can't use differences of positions, but we have to use the reported
267 // direction of that position 280 // direction of that position
268 sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); 281 sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
269 sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); 282 sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
270 *dirout = cdf->dir_hist[cdf->first_pos]; 283 *dirout = cdf->dir_hist[cdf->first_pos];
271 } 284 }
272 285
273
274 dx = pin->x - mx; 286 dx = pin->x - mx;
275 dy = pin->y - my; 287 dy = pin->y - my;
276 dist = dx * sx + dy * sy; 288 dist = dx * sx + dy * sy;
277 289
278 if (cdf->extrapolating && (dist < cdf->last_dist)) { 290 if (cdf->extrapolating && (dist < cdf->last_dist))
291 {
279 dist = cdf->last_dist; 292 dist = cdf->last_dist;
280 } 293 }
281 294
282 cdf->last_dist = dist; 295 cdf->last_dist = dist;
283 cdf->extrapolating = 1; 296 cdf->extrapolating = 1;
284 297
285 pout->x = (int)rint(mx + sx * dist); 298 pout->x = (int)rint(mx + sx * dist);
286 pout->y = (int)rint(my + sy * dist); 299 pout->y = (int)rint(my + sy * dist);
287 pout->pro = pin->pro; 300 pout->pro = pin->pro;
288 301
289 } else { 302}
303else
304{
290 // We should extrapolate, but don't have an old position available 305 // We should extrapolate, but don't have an old position available
291 *pout = *pin; 306 *pout = *pin;
292 *dirout = dirin; 307 *dirout = dirin;
293 } 308}
294 309
295 if (cdf->available) { 310if (cdf->available)
311{
296 int dx,dy; 312 int dx,dy;
297 313
298 dx = pout->x - cdf->last_out.x; 314 dx = pout->x - cdf->last_out.x;
299 dy = pout->y - cdf->last_out.y; 315 dy = pout->y - cdf->last_out.y;
300 316
301 if ((dx*dx + dy*dy) < CDF_MINDIST) { 317 if ((dx*dx + dy*dy) < CDF_MINDIST)
318 {
302 *pout = cdf->last_out; 319 *pout = cdf->last_out;
303 *dirout = cdf->last_dir; 320 *dirout = cdf->last_dir;
304 } 321 }
305 } 322}
306 323
307 cdf->last_out = *pout; 324cdf->last_out = *pout;
308 cdf->last_dir = *dirout; 325cdf->last_dir = *dirout;
309 326
310 cdf->available = 1; 327cdf->available = 1;
311} 328}
312#endif 329#endif
313 330
314int
315tracking_get_angle(struct tracking *tr) 331int tracking_get_angle(struct tracking *tr)
316{ 332{
317 return tr->curr_angle; 333 return tr->curr_angle;
318} 334}
319 335
320struct coord * 336struct coord *
321tracking_get_pos(struct tracking *tr) 337tracking_get_pos(struct tracking *tr)
322{ 338{
323 return &tr->curr_out; 339 return &tr->curr_out;
324} 340}
325 341
326int
327tracking_get_street_direction(struct tracking *tr) 342int tracking_get_street_direction(struct tracking *tr)
328{ 343{
329 return tr->street_direction; 344 return tr->street_direction;
330} 345}
331 346
332int
333tracking_get_segment_pos(struct tracking *tr) 347int tracking_get_segment_pos(struct tracking *tr)
334{ 348{
335 return tr->pos; 349 return tr->pos;
336} 350}
337 351
338struct street_data * 352struct street_data *
… …
341 if (tr->curr_line) 355 if (tr->curr_line)
342 return tr->curr_line->street; 356 return tr->curr_line->street;
343 return NULL; 357 return NULL;
344} 358}
345 359
346int
347tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter) 360int tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter)
348{ 361{
349 struct item *item; 362 struct item *item;
350 struct map_rect *mr; 363 struct map_rect *mr;
351 int result=0; 364 int result = 0;
352 // dbg(1,"enter %s\n",attr_to_name(type)); 365 // dbg(1,"enter %s\n",attr_to_name(type));
353 366
354 if (_this->attr) 367 if (_this->attr)
355 { 368 {
356 attr_free(_this->attr); 369 attr_free(_this->attr);
357 _this->attr=NULL; 370 _this->attr = NULL;
358 } 371 }
359 372
360 switch (type) 373 switch (type)
361 { 374 {
362 case attr_position_valid: 375 case attr_position_valid:
363 attr->u.num=_this->valid; 376 attr->u.num = _this->valid;
364 return 1; 377 return 1;
365 case attr_position_direction: 378 case attr_position_direction:
366 attr->u.numd=&_this->direction; 379 attr->u.numd = &_this->direction;
367 return 1; 380 return 1;
368 case attr_position_speed: 381 case attr_position_speed:
369 attr->u.numd=&_this->speed; 382 attr->u.numd = &_this->speed;
370 return 1; 383 return 1;
371 case attr_directed: 384 case attr_directed:
372 attr->u.num=_this->street_direction; 385 attr->u.num = _this->street_direction;
373 return 1; 386 return 1;
374 case attr_position_coord_geo: 387 case attr_position_coord_geo:
375 if (!_this->coord_geo_valid) 388 if (!_this->coord_geo_valid)
376 { 389 {
377 struct coord c; 390 struct coord c;
378 c.x=_this->curr_out.x; 391 c.x = _this->curr_out.x;
379 c.y=_this->curr_out.y; 392 c.y = _this->curr_out.y;
380 transform_to_geo(_this->pro, &c, &_this->coord_geo); 393 transform_to_geo(_this->pro, &c, &_this->coord_geo);
381 _this->coord_geo_valid=1; 394 _this->coord_geo_valid = 1;
382 } 395 }
383 attr->u.coord_geo=&_this->coord_geo; 396 attr->u.coord_geo = &_this->coord_geo;
384 return 1; 397 return 1;
385 case attr_current_item: 398 case attr_current_item:
386 if (! _this->curr_line || ! _this->curr_line->street) 399 if (!_this->curr_line || !_this->curr_line->street)
387 return 0; 400 return 0;
388 attr->u.item=&_this->curr_line->street->item; 401 attr->u.item = &_this->curr_line->street->item;
389 return 1; 402 return 1;
390 default: 403 default:
391 if (! _this->curr_line || ! _this->curr_line->street) 404 if (!_this->curr_line || !_this->curr_line->street)
392 return 0; 405 return 0;
393 item=&_this->curr_line->street->item; 406 item = &_this->curr_line->street->item;
394 mr=map_rect_new(item->map,NULL); 407 mr = map_rect_new(item->map, NULL);
395 item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo); 408 item = map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
396 if (item_attr_get(item, type, attr)) 409 if (item_attr_get(item, type, attr))
397 { 410 {
398 _this->attr=attr_dup(attr); 411 _this->attr = attr_dup(attr);
399 *attr=*_this->attr; 412 *attr = *_this->attr;
400 result=1; 413 result = 1;
401 } 414 }
402 map_rect_destroy(mr); 415 map_rect_destroy(mr);
403 return result; 416 return result;
404 } 417 }
405} 418}
406 419
407struct item * 420struct item *
408tracking_get_current_item(struct tracking *_this) 421tracking_get_current_item(struct tracking *_this)
409{ 422{
410 if (! _this->curr_line || ! _this->curr_line->street) 423 if (!_this->curr_line || !_this->curr_line->street)
411 return NULL; 424 return NULL;
412 return &_this->curr_line->street->item; 425 return &_this->curr_line->street->item;
413} 426}
414 427
415int * 428int *
416tracking_get_current_flags(struct tracking *_this) 429tracking_get_current_flags(struct tracking *_this)
417{ 430{
418 if (! _this->curr_line || ! _this->curr_line->street) 431 if (!_this->curr_line || !_this->curr_line->street)
419 return NULL; 432 return NULL;
420 return &_this->curr_line->street->flags; 433 return &_this->curr_line->street->flags;
421} 434}
422 435
423static void
424tracking_get_angles(struct tracking_line *tl) 436static void tracking_get_angles(struct tracking_line *tl)
425{ 437{
426 int i; 438 int i;
427 struct street_data *sd=tl->street; 439 struct street_data *sd = tl->street;
428 for (i = 0 ; i < sd->count-1 ; i++) 440 for (i = 0; i < sd->count - 1; i++)
429 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0); 441 tl->angle[i] = transform_get_angle_delta(&sd->c[i], &sd->c[i + 1], 0);
430} 442}
431 443
432static int
433street_data_within_selection(struct street_data *sd, struct map_selection *sel) 444static int street_data_within_selection(struct street_data *sd, struct map_selection *sel)
434{ 445{
435 struct coord_rect r; 446 struct coord_rect r;
436 struct map_selection *curr; 447 struct map_selection *curr;
437 int i; 448 int i;
438 449
439 if (!sel) 450 if (!sel)
440 return 1; 451 return 1;
441 r.lu=sd->c[0]; 452 r.lu = sd->c[0];
442 r.rl=sd->c[0]; 453 r.rl = sd->c[0];
443 for (i = 1 ; i < sd->count ; i++) { 454 for (i = 1; i < sd->count; i++)
455 {
444 if (r.lu.x > sd->c[i].x) 456 if (r.lu.x > sd->c[i].x)
445 r.lu.x=sd->c[i].x; 457 r.lu.x = sd->c[i].x;
446 if (r.rl.x < sd->c[i].x) 458 if (r.rl.x < sd->c[i].x)
447 r.rl.x=sd->c[i].x; 459 r.rl.x = sd->c[i].x;
448 if (r.rl.y > sd->c[i].y) 460 if (r.rl.y > sd->c[i].y)
449 r.rl.y=sd->c[i].y; 461 r.rl.y = sd->c[i].y;
450 if (r.lu.y < sd->c[i].y) 462 if (r.lu.y < sd->c[i].y)
451 r.lu.y=sd->c[i].y; 463 r.lu.y = sd->c[i].y;
452 } 464 }
453 curr=sel; 465 curr = sel;
454 while (curr) { 466 while (curr)
467 {
455 struct coord_rect *sr=&curr->u.c_rect; 468 struct coord_rect *sr = &curr->u.c_rect;
456 if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x && 469 if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x && r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
457 r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
458 return 1; 470 return 1;
459 curr=curr->next; 471 curr = curr->next;
460 } 472 }
461 return 0; 473 return 0;
462} 474}
463 475
464
465static void
466tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro) 476static void tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro)
467{ 477{
468 int max_dist=1000; 478 int max_dist = 1000;
469 struct map_selection *sel; 479 struct map_selection *sel;
470 struct mapset_handle *h; 480 struct mapset_handle *h;
471 struct map *m; 481 struct map *m;
472 struct map_rect *mr; 482 struct map_rect *mr;
473 struct item *item; 483 struct item *item;
… …
475 struct tracking_line *tl; 485 struct tracking_line *tl;
476 struct coord_geo g; 486 struct coord_geo g;
477 struct coord cc; 487 struct coord cc;
478 488
479 //dbg(1,"enter\n"); 489 //dbg(1,"enter\n");
480 h=mapset_open(tr->ms); 490 h = mapset_open(tr->ms);
481 while ((m=mapset_next(h,2))) { 491 while ((m = mapset_next(h, 2)))
492 {
482 cc.x = pc->x; 493 cc.x = pc->x;
483 cc.y = pc->y; 494 cc.y = pc->y;
484 if (map_projection(m) != pro) { 495 if (map_projection(m) != pro)
496 {
485 transform_to_geo(pro, &cc, &g); 497 transform_to_geo(pro, &cc, &g);
486 transform_from_geo(map_projection(m), &g, &cc); 498 transform_from_geo(map_projection(m), &g, &cc);
487 } 499 }
488 sel = route_rect(18, &cc, &cc, 0, max_dist); 500 sel = route_rect(18, &cc, &cc, 0, max_dist);
489 mr=map_rect_new(m, sel); 501 mr = map_rect_new(m, sel);
490 if (!mr) 502 if (!mr)
491 continue; 503 continue;
492 while ((item=map_rect_get_item(mr))) { 504 while ((item = map_rect_get_item(mr)))
505 {
493 if (item_get_default_flags(item->type)) { 506 if (item_get_default_flags(item->type))
507 {
494 street=street_get_data(item); 508 street = street_get_data(item);
495 if (street_data_within_selection(street, sel)) { 509 if (street_data_within_selection(street, sel))
510 {
496 tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int)); 511 tl = g_malloc(sizeof(struct tracking_line) + (street->count - 1) * sizeof(int));
497 tl->street=street; 512 tl->street = street;
498 tracking_get_angles(tl); 513 tracking_get_angles(tl);
499 tl->next=tr->lines; 514 tl->next = tr->lines;
500 tr->lines=tl; 515 tr->lines = tl;
516 }
501 } else 517 else
502 street_data_free(street); 518 street_data_free(street);
503 } 519 }
504 } 520 }
505 map_selection_destroy(sel); 521 map_selection_destroy(sel);
506 map_rect_destroy(mr); 522 map_rect_destroy(mr);
507 } 523 }
508 mapset_close(h); 524 mapset_close(h);
509 //dbg(1, "exit\n"); 525 //dbg(1, "exit\n");
510} 526}
511 527
512
513void
514tracking_flush(struct tracking *tr) 528void tracking_flush(struct tracking *tr)
515{ 529{
516 struct tracking_line *tl=tr->lines,*next; 530 struct tracking_line *tl = tr->lines, *next;
517 dbg(1,"enter(tr=%p)\n", tr); 531 dbg(1, "enter(tr=%p)\n", tr);
518 532
519 while (tl) { 533 while (tl)
534 {
520 next=tl->next; 535 next = tl->next;
521 street_data_free(tl->street); 536 street_data_free(tl->street);
522 g_free(tl); 537 g_free(tl);
523 tl=next; 538 tl = next;
524 } 539 }
525 tr->lines=NULL; 540 tr->lines = NULL;
526 tr->curr_line = NULL; 541 tr->curr_line = NULL;
527} 542}
528 543
529static int
530tracking_angle_diff(int a1, int a2, int full) 544static int tracking_angle_diff(int a1, int a2, int full)
531{ 545{
532 int ret=(a1-a2)%full; 546 int ret = (a1 - a2) % full;
533 if (ret > full/2) 547 if (ret > full / 2)
534 ret-=full; 548 ret -= full;
535 if (ret < -full/2) 549 if (ret < -full / 2)
536 ret+=full; 550 ret += full;
537 return ret; 551 return ret;
538} 552}
539 553
540static int
541tracking_angle_abs_diff(int a1, int a2, int full) 554static int tracking_angle_abs_diff(int a1, int a2, int full)
542{ 555{
543 int ret=tracking_angle_diff(a1, a2, full); 556 int ret = tracking_angle_diff(a1, a2, full);
544 if (ret < 0) 557 if (ret < 0)
545 ret=-ret; 558 ret = -ret;
546 return ret; 559 return ret;
547} 560}
548 561
549static int
550tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags) 562static int tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags)
551{ 563{
552 int full=180,ret=360,fwd=0,rev=0; 564 int full = 180, ret = 360, fwd = 0, rev = 0;
553 struct vehicleprofile *profile=tr->vehicleprofile; 565 struct vehicleprofile *profile = tr->vehicleprofile;
554 566
555 if (profile) { 567 if (profile)
568 {
556 fwd=((flags & profile->flags_forward_mask) == profile->flags); 569 fwd = ((flags & profile->flags_forward_mask) == profile->flags);
557 rev=((flags & profile->flags_reverse_mask) == profile->flags); 570 rev = ((flags & profile->flags_reverse_mask) == profile->flags);
558 } 571 }
559 if (fwd || rev) { 572 if (fwd || rev)
573 {
560 if (!fwd || !rev) { 574 if (!fwd || !rev)
575 {
561 full=360; 576 full = 360;
562 if (rev) 577 if (rev)
563 street_angle=(street_angle+180)%360; 578 street_angle = (street_angle + 180) % 360;
564 } 579 }
565 ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full); 580 ret = tracking_angle_abs_diff(vehicle_angle, street_angle, full);
566 } 581 }
567 return ret*ret; 582 return ret * ret;
568} 583}
569 584
570static int
571tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2) 585static int tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2)
572{ 586{
573 if (c1[0].x == c2[0].x && c1[0].y == c2[0].y) 587 if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
574 return 0; 588 return 0;
575 if (c1[0].x == c2[1].x && c1[0].y == c2[1].y) 589 if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
576 return 0; 590 return 0;
… …
579 if (c1[1].x == c2[1].x && c1[1].y == c2[1].y) 593 if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
580 return 0; 594 return 0;
581 return tr->connected_pref; 595 return tr->connected_pref;
582} 596}
583 597
584static int
585tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2) 598static int tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2)
586{ 599{
587 if (c1->x == c2->x && c1->y == c2->y) 600 if (c1->x == c2->x && c1->y == c2->y)
588 return tr->nostop_pref; 601 return tr->nostop_pref;
589 return 0; 602 return 0;
590} 603}
591 604
592static int
593tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item) 605static int tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item)
594{ 606{
595#ifdef USE_ROUTING 607#ifdef USE_ROUTING
596 if (! rt) 608 if (! rt)
597 return 0; 609 return 0;
598 if (route_contains(rt, item)) 610 if (route_contains(rt, item))
599 return 0; 611 return 0;
600 return tr->route_pref; 612 return tr->route_pref;
601#else 613#else
602 return 0; 614 return 0;
603#endif 615#endif
604} 616}
605 617
606static int
607tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags) 618static int tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
608{ 619{
609 int value=0; 620 int value = 0;
610 struct street_data *sd=t->street; 621 struct street_data *sd = t->street;
611 //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); 622 //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);
612 if (flags & 1) 623 if (flags & 1)
613 { 624 {
614 struct coord c1, c2, cp; 625 struct coord c1, c2, cp;
615 c1.x = sd->c[offset].x; 626 c1.x = sd->c[offset].x;
616 c1.y = sd->c[offset].y; 627 c1.y = sd->c[offset].y;
617 c2.x = sd->c[offset+1].x; 628 c2.x = sd->c[offset + 1].x;
618 c2.y = sd->c[offset+1].y; 629 c2.y = sd->c[offset + 1].y;
619 cp.x = tr->curr_in.x; 630 cp.x = tr->curr_in.x;
620 cp.y = tr->curr_in.y; 631 cp.y = tr->curr_in.y;
621 value+=transform_distance_line_sq(&c1, &c2, &cp, lpnt); 632 value += transform_distance_line_sq(&c1, &c2, &cp, lpnt);
622 } 633 }
623 if (value >= min) 634 if (value >= min)
624 return value; 635 return value;
625 if (flags & 2) 636 if (flags & 2)
626 value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags)*tr->angle_pref>>4; 637 value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags) * tr->angle_pref >> 4;
627 if (value >= min) 638 if (value >= min)
628 return value; 639 return value;
629 if ((flags & 4) && tr->connected_pref) 640 if ((flags & 4) && tr->connected_pref)
630 value += tracking_is_connected(tr, tr->last, &sd->c[offset]); 641 value += tracking_is_connected(tr, tr->last, &sd->c[offset]);
631 if ((flags & 8) && tr->nostop_pref) 642 if ((flags & 8) && tr->nostop_pref)
632 value += tracking_is_no_stop(tr, lpnt, &tr->last_out); 643 value += tracking_is_no_stop(tr, lpnt, &tr->last_out);
633 if (value >= min) 644 if (value >= min)
634 return value; 645 return value;
635 if ((flags & 16) && tr->route_pref) 646 if ((flags & 16) && tr->route_pref)
636 value += tracking_is_on_route(tr, tr->rt, &sd->item); 647 value += tracking_is_on_route(tr, tr->rt, &sd->item);
637 if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref ) 648 if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref)
638 { 649 {
639 struct roadprofile *roadprofile=g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *)t->street->item.type); 650 struct roadprofile *roadprofile = g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *) t->street->item.type);
640 if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref/ 100) 651 if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref / 100)
641 value += tr->overspeed_pref; 652 value += tr->overspeed_pref;
642 } 653 }
643 return value; 654 return value;
644} 655}
645 656
646
647void
648tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro) 657void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro)
649{ 658{
650 struct tracking_line *t; 659 struct tracking_line *t;
651 int i,value,min,time; 660 int i, value, min, time;
652 struct coord lpnt; 661 struct coord lpnt;
653 struct coord cin; 662 struct coord cin;
654 struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr,static_speed,static_distance; 663 struct attr valid, speed_attr, direction_attr, coord_geo, lag, time_attr, static_speed, static_distance;
655 double speed, direction; 664 double speed, direction;
656 if (v) 665 if (v)
657 tr->vehicle=v; 666 tr->vehicle = v;
658 if (vehicleprofile) 667 if (vehicleprofile)
659 tr->vehicleprofile=vehicleprofile; 668 tr->vehicleprofile = vehicleprofile;
660 669
661 if (! tr->vehicle) 670 if (!tr->vehicle)
662 return; 671 return;
663 if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL)) 672 if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
664 valid.u.num=attr_position_valid_valid; 673 valid.u.num = attr_position_valid_valid;
665 if (valid.u.num == attr_position_valid_invalid) 674 if (valid.u.num == attr_position_valid_invalid)
666 { 675 {
667 tr->valid=valid.u.num; 676 tr->valid = valid.u.num;
668 return; 677 return;
669 } 678 }
670 if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) || 679 if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) || !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL))
671 !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) ||
672 !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) ||
673 !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL))
674 { 680 {
675 //dbg(0,"failed to get position data %d %d %d %d\n", 681 //dbg(0,"failed to get position data %d %d %d %d\n",
676 //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL), 682 //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL),
677 //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL), 683 //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL),
678 //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL), 684 //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL),
679 //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)); 685 //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL));
680 return; 686 return;
681 } 687 }
682 if (!vehicleprofile_get_attr(vehicleprofile,attr_static_speed,&static_speed,NULL) || !vehicleprofile_get_attr(vehicleprofile,attr_static_distance,&static_distance,NULL)) 688 if (!vehicleprofile_get_attr(vehicleprofile, attr_static_speed, &static_speed, NULL) || !vehicleprofile_get_attr(vehicleprofile, attr_static_distance, &static_distance, NULL))
683 { 689 {
684 static_speed.u.num=3; 690 static_speed.u.num = 3;
685 static_distance.u.num=10; 691 static_distance.u.num = 10;
686 //dbg(1,"Using defaults for static position detection\n"); 692 //dbg(1,"Using defaults for static position detection\n");
687 } 693 }
688 //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num); 694 //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num);
689 time=iso8601_to_secs(time_attr.u.str); 695 time = iso8601_to_secs(time_attr.u.str);
690 speed=*speed_attr.u.numd; 696 speed = *speed_attr.u.numd;
691 direction=*direction_attr.u.numd; 697 direction = *direction_attr.u.numd;
692 tr->valid=attr_position_valid_valid; 698 tr->valid = attr_position_valid_valid;
693 transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in); 699 transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
694 if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num )) 700 if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num))
695 { 701 {
696 //dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y); 702 //dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y);
697 tr->valid=attr_position_valid_static; 703 tr->valid = attr_position_valid_static;
698 tr->speed=0; 704 tr->speed = 0;
699 return; 705 return;
700 } 706 }
701 707
702 if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) 708 if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0)
703 { 709 {
704 double espeed; 710 double espeed;
705 int edirection; 711 int edirection;
706 if (time - tr->time == 1) 712 if (time - tr->time == 1)
707 { 713 {
708 /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */ 714 /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */
709 espeed=speed+(speed-tr->speed)*lag.u.num/10; 715 espeed = speed + (speed - tr->speed) * lag.u.num / 10;
710 /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */ 716 /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */
711 edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10; 717 edirection = direction + tracking_angle_diff(direction, tr->direction, 360) * lag.u.num / 10;
712 } 718 }
713 else 719 else
714 { 720 {
715 //dbg(1,"no speed and direction extrapolation\n"); 721 //dbg(1,"no speed and direction extrapolation\n");
716 espeed=speed; 722 espeed = speed;
717 edirection=direction; 723 edirection = direction;
718 } 724 }
719 //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection); 725 //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
720 //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); 726 //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
721 transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in); 727 transform_project(pro, &tr->curr_in, espeed * lag.u.num / 36, edirection, &tr->curr_in);
722 //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); 728 //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
723 } 729 }
724 tr->time=time; 730 tr->time = time;
725 tr->pro=pro; 731 tr->pro = pro;
726#if 0 732#if 0
727 733
728 tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime); 734 tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime);
729#endif 735#endif
730 tr->curr_angle=tr->direction=direction; 736 tr->curr_angle = tr->direction = direction;
731 tr->speed=speed; 737 tr->speed = speed;
732 tr->last_in=tr->curr_in; 738 tr->last_in = tr->curr_in;
733 tr->last_out=tr->curr_out; 739 tr->last_out = tr->curr_out;
734 tr->last[0]=tr->curr[0]; 740 tr->last[0] = tr->curr[0];
735 tr->last[1]=tr->curr[1]; 741 tr->last[1] = tr->curr[1];
736 if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) 742 if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500)
737 { 743 {
738 //dbg(1, "update\n"); 744 //dbg(1, "update\n");
739 tracking_flush(tr); 745 tracking_flush(tr);
740 tracking_doupdate_lines(tr, &tr->curr_in, pro); 746 tracking_doupdate_lines(tr, &tr->curr_in, pro);
741 tr->last_updated=tr->curr_in; 747 tr->last_updated = tr->curr_in;
742 //dbg(1,"update end\n"); 748 //dbg(1,"update end\n");
743 } 749 }
744 750
745 tr->street_direction=0; 751 tr->street_direction = 0;
746 t=tr->lines; 752 t = tr->lines;
747 tr->curr_line=NULL; 753 tr->curr_line = NULL;
748 min=INT_MAX/2; 754 min = INT_MAX / 2;
749 while (t) 755 while (t)
750 { 756 {
751 struct street_data *sd=t->street; 757 struct street_data *sd = t->street;
752 for (i = 0; i < sd->count-1 ; i++) 758 for (i = 0; i < sd->count - 1; i++)
753 { 759 {
754 value=tracking_value(tr,t,i,&lpnt,min,-1); 760 value = tracking_value(tr, t, i, &lpnt, min, -1);
755 if (value < min) 761 if (value < min)
756 { 762 {
757 struct coord lpnt_tmp; 763 struct coord lpnt_tmp;
758 int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360); 764 int angle_delta = tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
759 tr->curr_line=t; 765 tr->curr_line = t;
760 tr->pos=i; 766 tr->pos = i;
761 tr->curr[0]=sd->c[i]; 767 tr->curr[0] = sd->c[i];
762 tr->curr[1]=sd->c[i+1]; 768 tr->curr[1] = sd->c[i + 1];
763 /* 769 /*
764 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i, 770 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
765 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp), 771 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp),
766 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref, 772 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref,
767 tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0, 773 tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0,
768 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0, 774 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0,
769 value 775 value
770 ); 776 );
771 */ 777 */
772 tr->curr_out.x=lpnt.x; 778 tr->curr_out.x = lpnt.x;
773 tr->curr_out.y=lpnt.y; 779 tr->curr_out.y = lpnt.y;
774 tr->coord_geo_valid=0; 780 tr->coord_geo_valid = 0;
775 if (angle_delta < 70) 781 if (angle_delta < 70)
776 tr->street_direction=1; 782 tr->street_direction = 1;
777 else if (angle_delta > 110) 783 else if (angle_delta > 110)
778 tr->street_direction=-1; 784 tr->street_direction = -1;
779 else 785 else
780 tr->street_direction=0; 786 tr->street_direction = 0;
781 min=value; 787 min = value;
782 } 788 }
783 } 789 }
784 t=t->next; 790 t = t->next;
785 } 791 }
786 792
787 //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min); 793 //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
788 794
789 if (!tr->curr_line || min > tr->offroad_limit_pref) 795 if (!tr->curr_line || min > tr->offroad_limit_pref)
790 { 796 {
791 tr->curr_out=tr->curr_in; 797 tr->curr_out = tr->curr_in;
792 tr->coord_geo_valid=0; 798 tr->coord_geo_valid = 0;
793 tr->street_direction=0; 799 tr->street_direction = 0;
794 } 800 }
795 801
796 if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND)) 802 if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND))
797 { 803 {
798 //dbg(0,"AF_UNDERGROUND 1"); 804 //dbg(0,"AF_UNDERGROUND 1");
799 if (tr->no_gps) 805 if (tr->no_gps)
800 { 806 {
801 tr->tunnel=1; 807 tr->tunnel = 1;
802 } 808 }
803 } 809 }
804 else if (tr->tunnel) 810 else if (tr->tunnel)
805 { 811 {
806 //dbg(0,"AF_UNDERGROUND 2"); 812 //dbg(0,"AF_UNDERGROUND 2");
807 tr->speed=0; 813 tr->speed = 0;
808 } 814 }
809 //dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y); 815 //dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
810} 816}
811 817
812static int
813tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) 818static int tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial)
814{ 819{
815 switch (attr->type) { 820 switch (attr->type)
821 {
816 case attr_angle_pref: 822 case attr_angle_pref:
817 tr->angle_pref=attr->u.num; 823 tr->angle_pref = attr->u.num;
818 return 1; 824 return 1;
819 case attr_connected_pref: 825 case attr_connected_pref:
820 tr->connected_pref=attr->u.num; 826 tr->connected_pref = attr->u.num;
821 return 1; 827 return 1;
822 case attr_nostop_pref: 828 case attr_nostop_pref:
823 tr->nostop_pref=attr->u.num; 829 tr->nostop_pref = attr->u.num;
824 return 1; 830 return 1;
825 case attr_offroad_limit_pref: 831 case attr_offroad_limit_pref:
826 tr->offroad_limit_pref=attr->u.num; 832 tr->offroad_limit_pref = attr->u.num;
827 return 1; 833 return 1;
828 case attr_route_pref: 834 case attr_route_pref:
829 tr->route_pref=attr->u.num; 835 tr->route_pref = attr->u.num;
830 return 1; 836 return 1;
831 case attr_overspeed_pref: 837 case attr_overspeed_pref:
832 tr->overspeed_pref=attr->u.num; 838 tr->overspeed_pref = attr->u.num;
833 return 1; 839 return 1;
834 case attr_overspeed_percent_pref: 840 case attr_overspeed_percent_pref:
835 tr->overspeed_percent_pref=attr->u.num; 841 tr->overspeed_percent_pref = attr->u.num;
836 return 1; 842 return 1;
837 case attr_tunnel_extrapolation: 843 case attr_tunnel_extrapolation:
838 tr->tunnel_extrapolation=attr->u.num; 844 tr->tunnel_extrapolation = attr->u.num;
839 return 1; 845 return 1;
840 default: 846 default:
841 return 0; 847 return 0;
842 } 848 }
843} 849}
844 850
845int
846tracking_set_attr(struct tracking *tr, struct attr *attr) 851int tracking_set_attr(struct tracking *tr, struct attr *attr)
847{ 852{
848 return tracking_set_attr_do(tr, attr, 0); 853 return tracking_set_attr_do(tr, attr, 0);
849} 854}
850 855
851struct tracking * 856struct tracking *
852tracking_new(struct attr *parent, struct attr **attrs) 857tracking_new(struct attr *parent, struct attr **attrs)
853{ 858{
854 struct tracking *this=g_new0(struct tracking, 1); 859 struct tracking *this=g_new0(struct tracking, 1);
855 struct attr hist_size; 860 struct attr hist_size;
856 this->angle_pref=10; 861 this->angle_pref = 10;
857 this->connected_pref=10; 862 this->connected_pref = 10;
858 this->nostop_pref=10; 863 this->nostop_pref = 10;
859 this->offroad_limit_pref=5000; 864 this->offroad_limit_pref = 15000; // was 5000 originally!
860 this->route_pref=300; 865 this->route_pref = 300;
861 866
862
863 if (! attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) { 867 if (!attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL))
868 {
864 hist_size.u.num = 0; 869 hist_size.u.num = 0;
865 } 870 }
866 if (attrs) { 871 if (attrs)
872 {
867 for (;*attrs; attrs++) 873 for (; *attrs; attrs++)
868 tracking_set_attr_do(this, *attrs, 1); 874 tracking_set_attr_do(this, *attrs, 1);
869 } 875 }
870 876
871 tracking_init_cdf(&this->cdf, hist_size.u.num); 877 tracking_init_cdf(&this->cdf, hist_size.u.num);
872 878
873 return this; 879 return this;
874} 880}
875 881
876void
877tracking_set_mapset(struct tracking *this, struct mapset *ms) 882void tracking_set_mapset(struct tracking *this, struct mapset *ms)
878{ 883{
879 this->ms=ms; 884 this->ms = ms;
880} 885}
881 886
882void
883tracking_set_route(struct tracking *this, struct route *rt) 887void tracking_set_route(struct tracking *this, struct route *rt)
884{ 888{
885 this->rt=rt; 889 this->rt = rt;
886} 890}
887 891
888void
889tracking_destroy(struct tracking *tr) 892void tracking_destroy(struct tracking *tr)
890{ 893{
891 if (tr->attr) 894 if (tr->attr)
892 attr_free(tr->attr); 895 attr_free(tr->attr);
893 tracking_flush(tr); 896 tracking_flush(tr);
894 g_free(tr); 897 g_free(tr);
895} 898}
896 899
897struct map * 900struct map *
898tracking_get_map(struct tracking *this_) 901tracking_get_map(struct tracking *this_)
899{ 902{
900 struct attr *attrs[5]; 903 struct attr *attrs[5];
901 struct attr type,navigation,data,description; 904 struct attr type, navigation, data, description;
902 type.type=attr_type; 905 type.type = attr_type;
903 type.u.str="tracking"; 906 type.u.str = "tracking";
904 navigation.type=attr_trackingo; 907 navigation.type = attr_trackingo;
905 navigation.u.tracking=this_; 908 navigation.u.tracking = this_;
906 data.type=attr_data; 909 data.type = attr_data;
907 data.u.str=""; 910 data.u.str = "";
908 description.type=attr_description; 911 description.type = attr_description;
909 description.u.str="Tracking"; 912 description.u.str = "Tracking";
910 913
911 attrs[0]=&type; 914 attrs[0] = &type;
912 attrs[1]=&navigation; 915 attrs[1] = &navigation;
913 attrs[2]=&data; 916 attrs[2] = &data;
914 attrs[3]=&description; 917 attrs[3] = &description;
915 attrs[4]=NULL; 918 attrs[4] = NULL;
916 if (! this_->map) 919 if (!this_->map)
917 this_->map=map_new(NULL, attrs); 920 this_->map = map_new(NULL, attrs);
918 return this_->map; 921 return this_->map;
919} 922}
920 923
921
922struct map_priv { 924struct map_priv
925{
923 struct tracking *tracking; 926 struct tracking *tracking;
924}; 927};
925 928
926struct map_rect_priv { 929struct map_rect_priv
930{
927 struct tracking *tracking; 931 struct tracking *tracking;
928 struct item item; 932 struct item item;
929 struct tracking_line *curr,*next; 933 struct tracking_line *curr, *next;
930 int coord; 934 int coord;
931 enum attr_type attr_next; 935 enum attr_type attr_next;
932 int ccount; 936 int ccount;
933 int debug_idx; 937 int debug_idx;
934 char *str; 938 char *str;
935}; 939};
936 940
937static int
938tracking_map_item_coord_get(void *priv_data, struct coord *c, int count) 941static int tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
939{ 942{
940 struct map_rect_priv *this=priv_data; 943 struct map_rect_priv *this = priv_data;
941 enum projection pro; 944 enum projection pro;
942 int ret=0; 945 int ret = 0;
943 dbg(1,"enter\n"); 946 dbg(1, "enter\n");
944 while (this->ccount < 2 && count > 0) { 947 while (this->ccount < 2 && count > 0)
948 {
945 pro = map_projection(this->curr->street->item.map); 949 pro = map_projection(this->curr->street->item.map);
946 if (projection_mg != pro) { 950 if (projection_mg != pro)
951 {
947 transform_from_to(&this->curr->street->c[this->ccount+this->coord], 952 transform_from_to(&this->curr->street->c[this->ccount + this->coord], pro, c, projection_mg);
948 pro, 953 }
949 c ,projection_mg);
950 } else 954 else
951 *c=this->curr->street->c[this->ccount+this->coord]; 955 *c = this->curr->street->c[this->ccount + this->coord];
952 dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y); 956 dbg(1, "coord %d 0x%x,0x%x\n", this->ccount, c->x, c->y);
953 this->ccount++; 957 this->ccount++;
954 ret++; 958 ret++;
955 c++; 959 c++;
956 count--; 960 count--;
957 } 961 }
958 return ret; 962 return ret;
959} 963}
960 964
961static int
962tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr) 965static int tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
963{ 966{
964 struct map_rect_priv *this_=priv_data; 967 struct map_rect_priv *this_ = priv_data;
965 struct coord lpnt,*c; 968 struct coord lpnt, *c;
966 struct tracking *tr=this_->tracking; 969 struct tracking *tr = this_->tracking;
967 int value; 970 int value;
968 attr->type=attr_type; 971 attr->type = attr_type;
969 972
970 if (this_->str) { 973 if (this_->str)
974 {
971 g_free(this_->str); 975 g_free(this_->str);
972 this_->str=NULL; 976 this_->str = NULL;
973 } 977 }
974 978
975 switch(attr_type) { 979 switch (attr_type)
980 {
976 case attr_debug: 981 case attr_debug:
977 switch(this_->debug_idx) { 982 switch (this_->debug_idx)
983 {
978 case 0: 984 case 0:
979 this_->debug_idx++; 985 this_->debug_idx++;
980 this_->str=attr->u.str=g_strdup_printf("overall: %d (limit %d)",tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, -1), tr->offroad_limit_pref); 986 this_->str = attr->u.str = g_strdup_printf("overall: %d (limit %d)", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, -1), tr->offroad_limit_pref);
981 return 1; 987 return 1;
982 case 1: 988 case 1:
983 this_->debug_idx++; 989 this_->debug_idx++;
984 c=&this_->curr->street->c[this_->coord]; 990 c = &this_->curr->street->c[this_->coord];
985 value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1); 991 value = tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 1);
986 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", 992 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", tr->curr_in.x, tr->curr_in.y, c[0].x, c[0].y, c[1].x, c[1].y, lpnt.x, lpnt.y, value);
987 tr->curr_in.x, tr->curr_in.y,
988 c[0].x, c[0].y, c[1].x, c[1].y,
989 lpnt.x, lpnt.y, value);
990 return 1; 993 return 1;
991 case 2: 994 case 2:
992 this_->debug_idx++; 995 this_->debug_idx++;
993 this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d", 996 this_->str = attr->u.str = g_strdup_printf("angle: %d to %d (flags %d) %d", tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3, tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 2));
994 tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
995 tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2));
996 return 1; 997 return 1;
997 case 3: 998 case 3:
998 this_->debug_idx++; 999 this_->debug_idx++;
999 this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 4)); 1000 this_->str = attr->u.str = g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 4));
1000 return 1; 1001 return 1;
1001 case 4: 1002 case 4:
1002 this_->debug_idx++; 1003 this_->debug_idx++;
1003 this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 8)); 1004 this_->str = attr->u.str = g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 8));
1004 return 1; 1005 return 1;
1005 case 5: 1006 case 5:
1006 this_->debug_idx++; 1007 this_->debug_idx++;
1007 this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 16)); 1008 this_->str = attr->u.str = g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 16));
1008 return 1; 1009 return 1;
1009 case 6: 1010 case 6:
1010 this_->debug_idx++; 1011 this_->debug_idx++;
1011 this_->str=attr->u.str=g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 32)); 1012 this_->str = attr->u.str = g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 32));
1012 return 1; 1013 return 1;
1013 case 7: 1014 case 7:
1014 this_->debug_idx++; 1015 this_->debug_idx++;
1015 this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr); 1016 this_->str = attr->u.str = g_strdup_printf("line %p", this_->curr);
1016 return 1; 1017 return 1;
1018 default:
1019 this_->attr_next = attr_none;
1020 return 0;
1021 }
1022 case attr_any:
1023 while (this_->attr_next != attr_none)
1024 {
1025 if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
1026 return 1;
1027 }
1028 return 0;
1017 default: 1029 default:
1018 this_->attr_next=attr_none; 1030 attr->type = attr_none;
1019 return 0; 1031 return 0;
1020 } 1032 }
1021 case attr_any:
1022 while (this_->attr_next != attr_none) {
1023 if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
1024 return 1;
1025 }
1026 return 0;
1027 default:
1028 attr->type=attr_none;
1029 return 0;
1030 }
1031} 1033}
1032 1034
1033static struct item_methods tracking_map_item_methods = { 1035static struct item_methods tracking_map_item_methods = { NULL, tracking_map_item_coord_get, NULL, tracking_map_item_attr_get, };
1034 NULL,
1035 tracking_map_item_coord_get,
1036 NULL,
1037 tracking_map_item_attr_get,
1038};
1039 1036
1040
1041static void
1042tracking_map_destroy(struct map_priv *priv) 1037static void tracking_map_destroy(struct map_priv *priv)
1043{ 1038{
1044 g_free(priv); 1039 g_free(priv);
1045} 1040}
1046 1041
1047static void
1048tracking_map_rect_init(struct map_rect_priv *priv) 1042static void tracking_map_rect_init(struct map_rect_priv *priv)
1049{ 1043{
1050 priv->next=priv->tracking->lines; 1044 priv->next = priv->tracking->lines;
1051 priv->curr=NULL; 1045 priv->curr = NULL;
1052 priv->coord=0; 1046 priv->coord = 0;
1053 priv->item.id_lo=0; 1047 priv->item.id_lo = 0;
1054 priv->item.id_hi=0; 1048 priv->item.id_hi = 0;
1055} 1049}
1056 1050
1057static struct map_rect_priv * 1051static struct map_rect_priv *
1058tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel) 1052tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
1059{ 1053{
1060 struct tracking *tracking=priv->tracking; 1054 struct tracking *tracking = priv->tracking;
1061 struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1); 1055 struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
1062 ret->tracking=tracking; 1056 ret->tracking = tracking;
1063 tracking_map_rect_init(ret); 1057 tracking_map_rect_init(ret);
1064 ret->item.meth=&tracking_map_item_methods; 1058 ret->item.meth = &tracking_map_item_methods;
1065 ret->item.priv_data=ret; 1059 ret->item.priv_data = ret;
1066 ret->item.type=type_tracking_100; 1060 ret->item.type = type_tracking_100;
1067 return ret; 1061 return ret;
1068} 1062}
1069 1063
1070static void
1071tracking_map_rect_destroy(struct map_rect_priv *priv) 1064static void tracking_map_rect_destroy(struct map_rect_priv *priv)
1072{ 1065{
1073 g_free(priv); 1066 g_free(priv);
1074} 1067}
1075 1068
1076static struct item * 1069static struct item *
1077tracking_map_get_item(struct map_rect_priv *priv) 1070tracking_map_get_item(struct map_rect_priv *priv)
1078{ 1071{
1079 struct item *ret=&priv->item; 1072 struct item *ret = &priv->item;
1080 int value; 1073 int value;
1081 struct coord lpnt; 1074 struct coord lpnt;
1082 1075
1083 if (!priv->next) 1076 if (!priv->next)
1084 return NULL; 1077 return NULL;
1085 if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) { 1078 if (!priv->curr || priv->coord + 2 >= priv->curr->street->count)
1079 {
1086 priv->curr=priv->next; 1080 priv->curr = priv->next;
1087 priv->next=priv->curr->next; 1081 priv->next = priv->curr->next;
1088 priv->coord=0; 1082 priv->coord = 0;
1089 priv->item.id_lo=0; 1083 priv->item.id_lo = 0;
1090 priv->item.id_hi++; 1084 priv->item.id_hi++;
1091 } else { 1085 }
1086 else
1087 {
1092 priv->coord++; 1088 priv->coord++;
1093 priv->item.id_lo++; 1089 priv->item.id_lo++;
1094 } 1090 }
1095 value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1); 1091 value = tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX / 2, -1);
1096 if (value < 64) 1092 if (value < 64)
1097 priv->item.type=type_tracking_100; 1093 priv->item.type = type_tracking_100;
1098 else if (value < 128) 1094 else if (value < 128)
1099 priv->item.type=type_tracking_90; 1095 priv->item.type = type_tracking_90;
1100 else if (value < 256) 1096 else if (value < 256)
1101 priv->item.type=type_tracking_80; 1097 priv->item.type = type_tracking_80;
1102 else if (value < 512) 1098 else if (value < 512)
1103 priv->item.type=type_tracking_70; 1099 priv->item.type = type_tracking_70;
1104 else if (value < 1024) 1100 else if (value < 1024)
1105 priv->item.type=type_tracking_60; 1101 priv->item.type = type_tracking_60;
1106 else if (value < 2048) 1102 else if (value < 2048)
1107 priv->item.type=type_tracking_50; 1103 priv->item.type = type_tracking_50;
1108 else if (value < 4096) 1104 else if (value < 4096)
1109 priv->item.type=type_tracking_40; 1105 priv->item.type = type_tracking_40;
1110 else if (value < 8192) 1106 else if (value < 8192)
1111 priv->item.type=type_tracking_30; 1107 priv->item.type = type_tracking_30;
1112 else if (value < 16384) 1108 else if (value < 16384)
1113 priv->item.type=type_tracking_20; 1109 priv->item.type = type_tracking_20;
1114 else if (value < 32768) 1110 else if (value < 32768)
1115 priv->item.type=type_tracking_10; 1111 priv->item.type = type_tracking_10;
1116 else 1112 else
1117 priv->item.type=type_tracking_0; 1113 priv->item.type = type_tracking_0;
1118 dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count); 1114 dbg(1, "item %d %d points\n", priv->coord, priv->curr->street->count);
1119 priv->ccount=0; 1115 priv->ccount = 0;
1120 priv->attr_next=attr_debug; 1116 priv->attr_next = attr_debug;
1121 priv->debug_idx=0; 1117 priv->debug_idx = 0;
1122 return ret; 1118 return ret;
1123} 1119}
1124 1120
1125static struct item * 1121static struct item *
1126tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo) 1122tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
1127{ 1123{
1128 struct item *ret; 1124 struct item *ret;
1129 tracking_map_rect_init(priv); 1125 tracking_map_rect_init(priv);
1130 while ((ret=tracking_map_get_item(priv))) { 1126 while ((ret = tracking_map_get_item(priv)))
1127 {
1131 if (ret->id_hi == id_hi && ret->id_lo == id_lo) 1128 if (ret->id_hi == id_hi && ret->id_lo == id_lo)
1132 return ret; 1129 return ret;
1133 } 1130 }
1134 return NULL; 1131 return NULL;
1135} 1132}
1136 1133
1137static struct map_methods tracking_map_meth = { 1134static struct map_methods tracking_map_meth = { projection_mg, "utf-8", tracking_map_destroy, tracking_map_rect_new, tracking_map_rect_destroy, tracking_map_get_item, tracking_map_get_item_byid, NULL, NULL, NULL, };
1138 projection_mg,
1139 "utf-8",
1140 tracking_map_destroy,
1141 tracking_map_rect_new,
1142 tracking_map_rect_destroy,
1143 tracking_map_get_item,
1144 tracking_map_get_item_byid,
1145 NULL,
1146 NULL,
1147 NULL,
1148};
1149 1135
1150static struct map_priv * 1136static struct map_priv *
1151tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl) 1137tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl)
1152{ 1138{
1153 struct map_priv *ret; 1139 struct map_priv *ret;
1154 struct attr *tracking_attr; 1140 struct attr *tracking_attr;
1155 1141
1156 tracking_attr=attr_search(attrs, NULL, attr_trackingo); 1142 tracking_attr = attr_search(attrs, NULL, attr_trackingo);
1157 if (! tracking_attr) 1143 if (!tracking_attr)
1158 return NULL;
1159 ret=g_new0(struct map_priv, 1); 1144 return NULL; ret=g_new0(struct map_priv, 1);
1160 *meth=tracking_map_meth; 1145 *meth = tracking_map_meth;
1161 ret->tracking=tracking_attr->u.tracking; 1146 ret->tracking = tracking_attr->u.tracking;
1162 1147
1163 return ret; 1148 return ret;
1164} 1149}
1165 1150
1166
1167void
1168tracking_init(void) 1151void tracking_init(void)
1169{ 1152{
1170 plugin_register_map_type("tracking", tracking_map_new); 1153 plugin_register_map_type("tracking", tracking_map_new);
1171} 1154}
Legend:
Removed from v.28  
changed lines
  Added in v.31
   
Visit the ZANavi Wiki