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

Contents of /navit/navit/track.c

Parent Directory Parent Directory | Revision Log Revision Log


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

   
Visit the ZANavi Wiki