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

Contents of /navit/navit/track.c

Parent Directory Parent Directory | Revision Log Revision Log


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

   
Visit the ZANavi Wiki