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

Contents of /navit/navit/track.c

Parent Directory Parent Directory | Revision Log Revision Log


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

   
Visit the ZANavi Wiki