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

Contents of /navit/navit/track.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 52 - (show annotations) (download)
Mon Nov 7 10:01:21 2016 UTC (7 years, 4 months ago) by zoff99
File MIME type: text/plain
File size: 77154 byte(s)
v2.0.53
1 /**
2 * ZANavi, Zoff Android Navigation system.
3 * Copyright (C) 2011-2014 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 <sys/time.h>
43 #include <math.h>
44
45 #include "navit.h"
46
47 #include "item.h"
48 #include "attr.h"
49 #include "track.h"
50 #include "debug.h"
51 #include "transform.h"
52 #include "coord.h"
53 #include "route.h"
54 #include "projection.h"
55 #include "map.h"
56 #include "mapset.h"
57 #include "plugin.h"
58 #include "vehicleprofile.h"
59 #include "vehicle.h"
60 #include "roadprofile.h"
61 #include "util.h"
62 #include "config.h"
63
64
65 // #define NAVIT_FUNC_CALLS_DEBUG_PRINT 1
66
67 #define TRACKING_MAX_DIST_RELOAD_MAP_DATA_LIMIT 1500
68 #define TRACKING_MAX_DIST_RELOAD_MAP_DATA_THRESHOLD 750
69
70 #define FIVE_KHM 18
71 #define TEN_KHM 36
72 #define CORRECT_FOR_ON_ROUTE_SEG_VALUE 1100
73 #define CORRECT_BC_FOR_ON_ROUTE_SEG_VALUE 20000 // if this far away from winner segment on route, then dont snap to route
74
75 struct tracking_line
76 {
77 struct street_data *street;
78 struct tracking_line *next;
79 int angle[0];
80 };
81
82 /**
83 * @brief Conatins a list of previous speeds
84 *
85 * This structure is used to hold a list of previously reported
86 * speeds. This data is used by the CDF.
87 */
88 struct cdf_speed
89 {
90 struct cdf_speed *next;
91 int speed;
92 time_t time;
93 };
94
95 /**
96 * @brief Contains data for the CDF
97 *
98 * This structure holds all data needed by the
99 * cumulative displacement filter.
100 */
101 struct cdf_data
102 {
103 int extrapolating;
104 int available;
105 int first_pos;
106 int poscount;
107 int hist_size;
108 struct cdf_speed *speed_hist;
109 struct pcoord *pos_hist;
110 int *dir_hist;
111 double last_dist;
112 struct pcoord last_out;
113 int last_dir;
114 };
115
116 struct tacking_item_mark
117 {
118 int id_hi;
119 int id_lo;
120 struct map *map;
121 };
122
123 struct tracking
124 {
125 struct mapset *ms;
126 struct route *rt;
127 struct map *map;
128 struct vehicle *vehicle;
129 struct vehicleprofile *vehicleprofile;
130 struct coord last_updated;
131 struct tracking_line *lines;
132 struct tracking_line *curr_line;
133 int pos;
134 struct coord curr[2], curr_in, curr_out;
135 int curr_angle;
136 struct coord last[2], last_in, last_out;
137 struct cdf_data cdf;
138 struct attr *attr;
139 int valid;
140 int time;
141 double direction;
142 double speed;
143 int coord_geo_valid;
144 struct coord_geo coord_geo;
145 enum projection pro;
146 int street_direction;
147 int no_gps;
148 int tunnel;
149 int angle_pref;
150 int connected_pref;
151 int nostop_pref;
152 int offroad_limit_pref;
153 int route_pref;
154 int overspeed_pref;
155 int overspeed_percent_pref;
156 int tunnel_extrapolation;
157 int curr_max_speed;
158 struct tacking_item_mark curr_item_mark, last_item_mark;
159 };
160
161
162 int next_lanes_id_hi__prev = 0;
163 int next_lanes_id_lo__prev = 0;
164 struct map* next_lanes_id_map__prev = NULL;
165 int street_dir_next__prev = 1;
166 struct route_graph_segment *s__prev = NULL;
167
168
169 static void tracking_init_cdf(struct cdf_data *cdf, int hist_size)
170 {
171 cdf->extrapolating = 0;
172 cdf->available = 0;
173 cdf->poscount = 0;
174 cdf->last_dist = 0;
175 cdf->hist_size = hist_size;
176
177 cdf->pos_hist = g_new0(struct pcoord, hist_size);
178 cdf->dir_hist = g_new0(int, hist_size);
179 }
180
181 // Variables for finetuning the CDF
182
183 // Minimum average speed
184 #define CDF_MINAVG 1.f
185 // Maximum average speed
186 #define CDF_MAXAVG 6.f // only ~ 20 km/h
187 // We need a low value here because otherwise we would extrapolate whenever we are not accelerating
188
189 // Mininum distance (square of it..), below which we ignore gps updates
190 #define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
191 #if 0
192 static void
193 tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime)
194 {
195 struct cdf_speed *speed,*sc,*sl;
196 double speed_avg;
197 int speed_num,i;
198
199 if (cdf->hist_size == 0)
200 {
201 dbg(1,"No CDF.\n");
202 *pout = *pin;
203 *dirout = dirin;
204 return;
205 }
206
207 speed = g_new0(struct cdf_speed, 1);
208 speed->speed = cur_speed;
209 speed->time = fixtime;
210
211 speed->next = cdf->speed_hist;
212 cdf->speed_hist = speed;
213
214 sc = speed;
215 sl = NULL;
216 speed_num = 0;
217 speed_avg = 0;
218 while (sc && ((fixtime - speed->time) < 4))
219 { // FIXME static maxtime
220 speed_num++;
221 speed_avg += sc->speed;
222 sl = sc;
223 sc = sc->next;
224 }
225
226 speed_avg /= (double)speed_num;
227
228 if (sl)
229 {
230 sl->next = NULL;
231 }
232
233 while (sc)
234 {
235 sl = sc->next;
236 g_free(sc);
237 sc = sl;
238 }
239
240 if (speed_avg < CDF_MINAVG)
241 {
242 speed_avg = CDF_MINAVG;
243 }
244 else if (speed_avg > CDF_MAXAVG)
245 {
246 speed_avg = CDF_MAXAVG;
247 }
248
249 if (cur_speed >= speed_avg)
250 {
251 if (cdf->extrapolating)
252 {
253 cdf->poscount = 0;
254 cdf->extrapolating = 0;
255 }
256
257 cdf->first_pos--;
258 if (cdf->first_pos < 0)
259 {
260 cdf->first_pos = cdf->hist_size - 1;
261 }
262
263 if (cdf->poscount < cdf->hist_size)
264 {
265 cdf->poscount++;
266 }
267
268 cdf->pos_hist[cdf->first_pos] = *pin;
269 cdf->dir_hist[cdf->first_pos] = dirin;
270
271 *pout = *pin;
272 *dirout = dirin;
273 }
274 else if (cdf->poscount > 0)
275 {
276
277 double mx,my; // Average position's x and y values
278 double sx,sy; // Support vector
279 double dx,dy; // Difference between average and current position
280 double len; // Length of support vector
281 double dist;
282
283 mx = my = 0;
284 sx = sy = 0;
285
286 for (i = 0; i < cdf->poscount; i++)
287 {
288 mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
289 my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;
290
291 if (i != 0)
292 {
293 sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x;
294 sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y;
295 }
296
297 }
298
299 if (cdf->poscount > 1)
300 {
301 // Normalize the support vector
302 len = sqrt(sx * sx + sy * sy);
303 sx /= len;
304 sy /= len;
305
306 // Calculate the new direction
307 *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180);
308 }
309 else
310 {
311 // If we only have one position, we can't use differences of positions, but we have to use the reported
312 // direction of that position
313 sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
314 sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
315 *dirout = cdf->dir_hist[cdf->first_pos];
316 }
317
318 dx = pin->x - mx;
319 dy = pin->y - my;
320 dist = dx * sx + dy * sy;
321
322 if (cdf->extrapolating && (dist < cdf->last_dist))
323 {
324 dist = cdf->last_dist;
325 }
326
327 cdf->last_dist = dist;
328 cdf->extrapolating = 1;
329
330 pout->x = (int)rint(mx + sx * dist);
331 pout->y = (int)rint(my + sy * dist);
332 pout->pro = pin->pro;
333
334 }
335 else
336 {
337 // We should extrapolate, but don't have an old position available
338 *pout = *pin;
339 *dirout = dirin;
340 }
341
342 if (cdf->available)
343 {
344 int dx,dy;
345
346 dx = pout->x - cdf->last_out.x;
347 dy = pout->y - cdf->last_out.y;
348
349 if ((dx*dx + dy*dy) < CDF_MINDIST)
350 {
351 *pout = cdf->last_out;
352 *dirout = cdf->last_dir;
353 }
354 }
355
356 cdf->last_out = *pout;
357 cdf->last_dir = *dirout;
358
359 cdf->available = 1;
360 }
361 #endif
362
363 int tracking_get_angle(struct tracking *tr)
364 {
365 return tr->curr_angle;
366 }
367
368 struct coord *
369 tracking_get_pos(struct tracking *tr)
370 {
371 #ifdef NAVIT_ROUTING_DEBUG_PRINT
372 dbg(0, "ROUTExxPOSxx:tracking_get_pos: enter\n");
373 #endif
374 return &tr->curr_out;
375 }
376
377 int tracking_get_street_direction(struct tracking *tr)
378 {
379 return tr->street_direction;
380 }
381
382 double tracking_get_direction(struct tracking *tr)
383 {
384 return tr->direction;
385 }
386
387 int tracking_get_segment_pos(struct tracking *tr)
388 {
389 return tr->pos;
390 }
391
392 struct street_data *
393 tracking_get_street_data(struct tracking *tr)
394 {
395 if (tr->curr_line)
396 {
397 return tr->curr_line->street;
398 }
399
400 return NULL;
401 }
402
403 int tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter)
404 {
405 struct item *item;
406 struct map_rect *mr;
407 int result = 0;
408
409 if (_this->attr)
410 {
411 attr_free(_this->attr);
412 _this->attr = NULL;
413 }
414
415 switch (type)
416 {
417 case attr_position_valid:
418 attr->u.num = _this->valid;
419 return 1;
420 case attr_position_direction:
421 attr->u.numd = &_this->direction;
422 return 1;
423 case attr_position_speed:
424 attr->u.numd = &_this->speed;
425 return 1;
426 case attr_directed:
427 attr->u.num = _this->street_direction;
428 return 1;
429 case attr_position_coord_geo:
430 #ifdef NAVIT_ROUTING_DEBUG_PRINT
431 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo\n");
432 #endif
433 if (!_this->coord_geo_valid)
434 {
435 struct coord c;
436 c.x = _this->curr_out.x;
437 c.y = _this->curr_out.y;
438 #ifdef NAVIT_ROUTING_DEBUG_PRINT
439 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:(coord_geo_valid == 0): %d %d\n", c.x, c.y);
440 #endif
441 transform_to_geo(_this->pro, &c, &_this->coord_geo);
442 #ifdef NAVIT_ROUTING_DEBUG_PRINT
443 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:(coord_geo_valid == 0)1:http://maps.google.com/maps/api/staticmap?size=512x512&markers=color:red|label:AA|%4.6f,%4.6f\n", _this->coord_geo.lat, _this->coord_geo.lng);
444 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:(coord_geo_valid == 0)1: %4.6f,%4.6f\n", _this->coord_geo.lat, _this->coord_geo.lng);
445 #endif
446 _this->coord_geo_valid = 1;
447 }
448 attr->u.coord_geo = &_this->coord_geo;
449 #ifdef NAVIT_ROUTING_DEBUG_PRINT
450 // ----- DEBUG ------
451 // ----- DEBUG ------
452 // ----- DEBUG ------
453 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:3: %4.6f,%4.6f\n", _this->coord_geo.lat, _this->coord_geo.lng);
454 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:3.1a:http://maps.google.com/maps/api/staticmap?size=512x512&markers=color:red|label:AA|%4.6f,%4.6f\n", _this->coord_geo.lat, _this->coord_geo.lng);
455 struct coord c88;
456 transform_from_geo(_this->pro, &_this->coord_geo, &c88);
457 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:2: %d %d\n", c88.x, c88.y);
458 struct coord_geo gg54;
459 transform_to_geo(_this->pro, &c88, &gg54);
460 dbg(0, "ROUTExxPOSxx:attr_position_coord_geo:3.1b:http://maps.google.com/maps/api/staticmap?size=512x512&markers=color:green|label:A2|%4.6f,%4.6f&markers=color:red|label:AA|%4.6f,%4.6f\n", gg54.lat, gg54.lng, _this->coord_geo.lat, _this->coord_geo.lng);
461 // ----- DEBUG ------
462 // ----- DEBUG ------
463 // ----- DEBUG ------
464 #endif
465 return 1;
466 case attr_current_item:
467 if (!_this->curr_line || !_this->curr_line->street)
468 {
469 return 0;
470 }
471 attr->u.item = &_this->curr_line->street->item;
472 return 1;
473 default:
474 if (!_this->curr_line || !_this->curr_line->street)
475 {
476 return 0;
477 }
478
479 dbg(0,"ATTR %s\n", attr_to_name(type));
480
481 item = &_this->curr_line->street->item;
482 mr = map_rect_new(item->map, NULL);
483 // COST: 101
484 dbg(0, "COST:101\n");
485 item = map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
486
487 if (item_attr_get(item, type, attr))
488 {
489 _this->attr = attr_dup(attr);
490 *attr = *_this->attr;
491 result = 1;
492 }
493 map_rect_destroy(mr);
494 return result;
495 }
496 }
497
498 struct item *
499 tracking_get_current_item(struct tracking *_this)
500 {
501 if (!_this->curr_line || !_this->curr_line->street)
502 return NULL;
503
504 return &_this->curr_line->street->item;
505 }
506
507 int *
508 tracking_get_current_flags(struct tracking *_this)
509 {
510 if (!_this->curr_line || !_this->curr_line->street)
511 return NULL;
512
513 return &_this->curr_line->street->flags;
514 }
515
516 static void tracking_get_angles(struct tracking_line *tl)
517 {
518 int i;
519 struct street_data *sd = tl->street;
520
521 // COST: 103
522 // dbg(0, "COST:103\n");
523
524 for (i = 0; i < sd->count - 1; i++)
525 {
526 tl->angle[i] = transform_get_angle_delta(&sd->c[i], &sd->c[i + 1], 0);
527 }
528 }
529
530 static int street_data_within_selection(struct street_data *sd, struct map_selection *sel)
531 {
532 struct coord_rect r;
533 struct map_selection *curr;
534 int i;
535
536 if (!sel)
537 return 1;
538
539 r.lu = sd->c[0];
540 r.rl = sd->c[0];
541
542 for (i = 1; i < sd->count; i++)
543 {
544 if (r.lu.x > sd->c[i].x)
545 r.lu.x = sd->c[i].x;
546 if (r.rl.x < sd->c[i].x)
547 r.rl.x = sd->c[i].x;
548 if (r.rl.y > sd->c[i].y)
549 r.rl.y = sd->c[i].y;
550 if (r.lu.y < sd->c[i].y)
551 r.lu.y = sd->c[i].y;
552 }
553 curr = sel;
554
555 while (curr)
556 {
557 struct coord_rect *sr = &curr->u.c_rect;
558 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)
559 {
560 return 1;
561 }
562
563 curr = curr->next;
564 }
565
566 return 0;
567 }
568
569 static void tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro, int max_dist)
570 {
571 struct map_selection *sel;
572 struct mapset_handle *h;
573 struct map *m;
574 struct map_rect *mr;
575 struct item *item;
576 struct street_data *street;
577 struct tracking_line *tl;
578 struct coord_geo g;
579 struct coord cc;
580
581 //dbg(1,"enter\n");
582 h = mapset_open(tr->ms);
583 while ((m = mapset_next(h, 2)))
584 {
585 #if 0
586 struct attr map_name_attr;
587 if (map_get_attr(m, attr_name, &map_name_attr, NULL))
588 {
589 if (strncmp("_ms_sdcard_map:-special-:worldmap", map_name_attr.u.str, 34) == 0)
590 {
591 dbg(0, "dont use special map %s for tracking\n", map_name_attr.u.str);
592 continue;
593 }
594 }
595 #endif
596
597 cc.x = pc->x;
598 cc.y = pc->y;
599
600 if (map_projection(m) != pro)
601 {
602 transform_to_geo(pro, &cc, &g);
603 transform_from_geo(map_projection(m), &g, &cc);
604 }
605
606 sel = route_rect(18, &cc, &cc, 0, max_dist);
607 mr = map_rect_new(m, sel);
608
609 if (!mr)
610 {
611 continue;
612 }
613
614 while ((item = map_rect_get_item(mr)))
615 {
616 if (item_get_default_flags(item->type))
617 {
618 street = street_get_data(item);
619 if (street_data_within_selection(street, sel))
620 {
621 tl = g_malloc(sizeof(struct tracking_line) + (street->count - 1) * sizeof(int));
622 tl->street = street;
623 tracking_get_angles(tl);
624 tl->next = tr->lines;
625 tr->lines = tl;
626 }
627 else
628 {
629 street_data_free(street);
630 }
631 }
632 }
633
634 map_selection_destroy(sel);
635 map_rect_destroy(mr);
636
637 }
638
639 mapset_close(h);
640 //dbg(1, "exit\n");
641 }
642
643 void tracking_flush(struct tracking *tr)
644 {
645 struct tracking_line *tl = tr->lines, *next;
646 //dbg(1, "enter(tr=%p)\n", tr);
647
648 while (tl)
649 {
650 next = tl->next;
651 street_data_free(tl->street);
652 g_free(tl);
653 tl = next;
654 }
655
656 tr->lines = NULL;
657 tr->curr_line = NULL;
658 }
659
660 static int tracking_angle_diff(int a1, int a2, int full)
661 {
662 int ret = (a1 - a2) % full;
663
664 if (ret > full / 2)
665 ret -= full;
666
667 if (ret < -full / 2)
668 ret += full;
669
670 return ret;
671 }
672
673 static int tracking_angle_abs_diff(int a1, int a2, int full)
674 {
675 int ret = tracking_angle_diff(a1, a2, full);
676
677 if (ret < 0)
678 {
679 ret = -ret;
680 }
681
682 return ret;
683 }
684
685 static int tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags)
686 {
687 int full = 180, ret = 360, fwd = 0, rev = 0;
688 struct vehicleprofile *profile = tr->vehicleprofile;
689
690 if (profile)
691 {
692 fwd = ((flags & route_get_real_oneway_mask(flags, profile->flags_forward_mask)) == profile->flags);
693 rev = ((flags & route_get_real_oneway_mask(flags, profile->flags_reverse_mask)) == profile->flags);
694 }
695
696 if (fwd || rev)
697 {
698 if (!fwd || !rev)
699 {
700 full = 360;
701 if (rev)
702 {
703 street_angle = (street_angle + 180) % 360;
704 }
705 }
706 ret = tracking_angle_abs_diff(vehicle_angle, street_angle, full);
707 }
708 return ret * ret;
709 }
710
711
712 static int tracking_angle_delta_no_squared(struct tracking *tr, int vehicle_angle, int street_angle, int flags)
713 {
714 int full = 180, ret = 360, fwd = 0, rev = 0;
715 struct vehicleprofile *profile = tr->vehicleprofile;
716
717 if (profile)
718 {
719 fwd = ((flags & route_get_real_oneway_mask(flags, profile->flags_forward_mask)) == profile->flags);
720 rev = ((flags & route_get_real_oneway_mask(flags, profile->flags_reverse_mask)) == profile->flags);
721 }
722
723 if (fwd || rev)
724 {
725 if (!fwd || !rev)
726 {
727 full = 360;
728 if (rev)
729 {
730 street_angle = (street_angle + 180) % 360;
731 }
732 }
733 ret = tracking_angle_abs_diff(vehicle_angle, street_angle, full);
734 }
735 return ret;
736 }
737
738
739 static int tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2, struct item *ii)
740 {
741
742 #ifdef NAVIT_ROUTING_DEBUG_PRINT
743 // dbg(0, "c1[0].x=%d c1[0].y=%d c1[1].x=%d c1[1].y=%d\n", c1[0].x, c1[0].y, c1[1].x, c1[1].y);
744 // dbg(0, "c2[0].x=%d c2[0].y=%d c2[1].x=%d c2[1].y=%d\n", c2[0].x, c2[0].y, c2[1].x, c2[1].y);
745 #endif
746
747 if (ii)
748 {
749 if ((tr->last_item_mark.id_hi == ii->id_hi) && (tr->last_item_mark.id_lo == ii->id_lo)&&(tr->last_item_mark.map == ii->map))
750 {
751 return 0;
752 }
753 }
754
755 if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
756 {
757 return 0;
758 }
759
760 if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
761 {
762 return 0;
763 }
764
765 if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
766 {
767 return 0;
768 }
769
770 if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
771 {
772 return 0;
773 }
774
775 return tr->connected_pref;
776 }
777
778 static int tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2)
779 {
780 if (c1->x == c2->x && c1->y == c2->y)
781 {
782 return tr->nostop_pref;
783 }
784
785 return 0;
786 }
787
788 static int tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item)
789 {
790 #ifdef USE_ROUTING
791 if (! rt)
792 {
793 return 0;
794 }
795
796 if (route_contains(rt, item))
797 {
798 return 0;
799 }
800
801 return tr->route_pref;
802 #else
803 return 0;
804 #endif
805 }
806
807 static int tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags, struct item *ii)
808 {
809 int value = 0;
810 struct street_data *sd = t->street;
811
812 if (flags & 1)
813 {
814 struct coord c1, c2, cp;
815 c1.x = sd->c[offset].x;
816 c1.y = sd->c[offset].y;
817 c2.x = sd->c[offset + 1].x;
818 c2.y = sd->c[offset + 1].y;
819 cp.x = tr->curr_in.x;
820 cp.y = tr->curr_in.y;
821 // set "lpnt" coords to the point in the line (from "c1" -> "c2") that is closest to "cp"
822 value += transform_distance_line_sq(&c1, &c2, &cp, lpnt);
823
824 #ifdef NAVIT_ROUTING_DEBUG_PRINT
825 // dbg(0, "flags=%d:1:value=%d min=%d\n", flags, value, min);
826 #endif
827
828 }
829
830 if (value >= min)
831 {
832 return value;
833 }
834
835 // if road is not connected to previously used tracking road -> penalty
836 if ((flags & 4) && tr->connected_pref)
837 {
838 #ifdef NAVIT_ROUTING_DEBUG_PRINT
839 // int value_before = value;
840 #endif
841 value += tracking_is_connected(tr, tr->last, &sd->c[offset], ii);
842
843 #ifdef NAVIT_ROUTING_DEBUG_PRINT
844 // if (value_before != value)
845 // {
846 // dbg(0, "flags=%d:4:value_before=%d\n", flags, value_before);
847 // dbg(0, "flags=%d:4:value=%d min=%d tr->connected_pref=%d\n", flags, value, min, tr->connected_pref);
848 // }
849 #endif
850 }
851
852 if (value >= min)
853 {
854 return value;
855 }
856
857 // if road goes against our driving angle -> penalty
858 if (flags & 2)
859 {
860 // value too high!!! // value += ( (tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags) * tr->angle_pref) >> 4 );
861 value += ( tracking_angle_delta_no_squared(tr, tr->curr_angle, t->angle[offset], sd->flags) * tr->angle_pref);
862
863 #ifdef NAVIT_ROUTING_DEBUG_PRINT
864 // dbg(0, "flags=%d:2:value=%d min=%d curr_angle=%f road_angle=%f\n", flags, value, min, (float)tr->curr_angle, (float)t->angle[offset]);
865 #endif
866 }
867
868 if (value >= min)
869 {
870 return value;
871 }
872
873 // if position that is currently evaluated is the same as the last position -> penalty
874 if ((flags & 8) && tr->nostop_pref)
875 {
876 value += tracking_is_no_stop(tr, lpnt, &tr->last_out);
877
878 #ifdef NAVIT_ROUTING_DEBUG_PRINT
879 // dbg(0, "flags=%d:8:value=%d min=%d\n", flags, value, min);
880 #endif
881 }
882
883 if (value >= min)
884 {
885 return value;
886 }
887
888 if ((flags & 16) && tr->route_pref)
889 {
890 value += tracking_is_on_route(tr, tr->rt, &sd->item);
891
892 #ifdef NAVIT_ROUTING_DEBUG_PRINT
893 // dbg(0, "flags=%d:16:value=%d min=%d\n", flags, value, min);
894 #endif
895 }
896
897 if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref)
898 {
899 struct roadprofile *roadprofile = g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *) t->street->item.type);
900
901 if (roadprofile && tr->speed > (double)(roadprofile->speed * tr->overspeed_percent_pref) / 100)
902 {
903 value += tr->overspeed_pref;
904 }
905 }
906
907 return value;
908 }
909
910
911 static int tracking_time_millis()
912 {
913 struct timeval tv;
914 gettimeofday(&tv, NULL);
915 double time_in_mill = (tv.tv_sec) * 1000 + (tv.tv_usec) / 1000;
916 return (int)(time_in_mill);
917 }
918
919
920
921
922
923
924 void tracking_calc_and_send_possbile_turn_info(struct route_graph_point *rgp, struct route_graph_segment *s, int street_dir, int depth)
925 {
926
927 struct route_graph_segment *start_3; // other segments that i could drive on next (but not on route)
928 struct route_graph_segment *start_3_save; // other segments that i could drive on next (but not on route)
929 struct route_graph_segment *drive_here; // i should drive on this segment next
930 int dir = 1;
931
932
933 #if 1
934
935 #define MAX_DUPL_CHECK_ENTRIES 20
936 int angle_dupl_check[MAX_DUPL_CHECK_ENTRIES + 1];
937 int angle_dupl_check_count = 0;
938 int jk;
939 int found_dupl = 0;
940 int angles_found[MAX_DUPL_CHECK_ENTRIES + 1];
941 int angles_found_count = 0;
942 int drive_here_angle = -999;
943
944 // FIXME : move codeblock down
945
946 if (street_dir == 1)
947 {
948 drive_here = s->end_from_seg;
949 }
950 else
951 {
952 drive_here = s->start_from_seg;
953 }
954 int found_first = 0;
955
956 char *next_roads_and_angles = NULL;
957 char *next_roads_and_angles_old = NULL;
958
959 struct route_graph_segment *tmp1 = NULL;
960
961 int cur_is_lower_type = navigation_is_low_level_street(s->data.item.type);
962
963 int dummy_int;
964
965 tmp1 = rgp->start;
966 while (tmp1)
967 {
968 if ((cur_is_lower_type == 1) || (navigation_is_low_level_street(tmp1->data.item.type) == 0 ))
969 {
970
971 if (tmp1 != s
972 && tmp1->data.item.type != type_street_turn_restriction_no
973 && tmp1->data.item.type != type_street_turn_restriction_only
974 && !(route_get_real_oneway_flag(tmp1->data.flags, NAVIT_AF_ONEWAYREV))
975 && is_turn_allowed(rgp, s, tmp1))
976 {
977 //dbg(0, "RR:x:001:sn:tmp1=%p\n", tmp1);
978
979 //dbg(0, "RR:x:001:sn:item hi:%d lo:%d type=%s\n", tmp1->data.item.id_hi, tmp1->data.item.id_lo, item_to_name(tmp1->data.item.type));
980
981 // item_dump_attr_stdout(&tmp1->data.item, tmp1->data.item.map);
982
983 struct coord ccc_cc, ccc_ss, ccc_ee;
984 int turn_angle3 = route_road_to_road_angle_get_segs(s, tmp1, street_dir, &dummy_int, &ccc_cc, &ccc_ss, &ccc_ee, 0);
985 //dbg(0, "RR:04.22.2:angle (real)=%d\n", turn_angle3);
986 #ifdef NAVIT_ANGLE_LIST_DEBUG_PRINT_2
987 route_add_to_sharp_turn_list(&ccc_cc, &ccc_ss, &ccc_ee, turn_angle3, street_dir);
988 #endif
989
990
991 angles_found[angles_found_count] = turn_angle3;
992 angles_found_count++;
993 if (angles_found_count > MAX_DUPL_CHECK_ENTRIES)
994 {
995 angles_found_count--;
996 }
997
998
999 if (drive_here == tmp1)
1000 {
1001 //dbg(0, "RR:04.22.1:DRIVE HERE!!\n");
1002
1003 drive_here_angle = turn_angle3;
1004 }
1005
1006 if (found_first == 0)
1007 {
1008 found_first = 1;
1009 }
1010
1011
1012 }
1013 }
1014
1015 tmp1 = tmp1->start_next;
1016
1017 }
1018
1019 tmp1 = rgp->end;
1020 while (tmp1)
1021 {
1022 if ((cur_is_lower_type == 1) || (navigation_is_low_level_street(tmp1->data.item.type) == 0 ))
1023 {
1024
1025 if (tmp1 != s
1026 && tmp1->data.item.type != type_street_turn_restriction_no
1027 && tmp1->data.item.type != type_street_turn_restriction_only
1028 && !(route_get_real_oneway_flag(tmp1->data.flags, NAVIT_AF_ONEWAY))
1029 && is_turn_allowed(rgp, s, tmp1))
1030 {
1031 //dbg(0, "RR:x:001:EN:tmp1=%p\n", tmp1);
1032
1033 //dbg(0, "RR:x:001:EN:item hi:%d lo:%d type=%s\n", tmp1->data.item.id_hi, tmp1->data.item.id_lo, item_to_name(tmp1->data.item.type));
1034
1035 // item_dump_attr_stdout(&tmp1->data.item, tmp1->data.item.map);
1036
1037 struct coord ccc_cc, ccc_ss, ccc_ee;
1038 int turn_angle3 = route_road_to_road_angle_get_segs(s, tmp1, street_dir, &dummy_int, &ccc_cc, &ccc_ss, &ccc_ee, 0);
1039 //dbg(0, "RR:04.22.2:angle (real)=%d\n", turn_angle3);
1040 #ifdef NAVIT_ANGLE_LIST_DEBUG_PRINT_2
1041 route_add_to_sharp_turn_list(&ccc_cc, &ccc_ss, &ccc_ee, turn_angle3, street_dir);
1042 #endif
1043
1044
1045 angles_found[angles_found_count] = turn_angle3;
1046 angles_found_count++;
1047 if (angles_found_count > MAX_DUPL_CHECK_ENTRIES)
1048 {
1049 angles_found_count--;
1050 }
1051
1052
1053 if (drive_here == tmp1)
1054 {
1055 //dbg(0, "RR:04.22.1:DRIVE HERE!!\n");
1056
1057 drive_here_angle = turn_angle3;
1058 }
1059
1060 if (found_first == 0)
1061 {
1062 found_first = 1;
1063 }
1064
1065 }
1066 }
1067
1068 tmp1 = tmp1->end_next;
1069
1070 }
1071
1072 if (found_first == 1)
1073 {
1074 //dbg(0, "RR:x:008:\n");
1075
1076 if (angles_found_count > 0)
1077 {
1078 int jk;
1079 int ad;
1080 int is_dupl = 0;
1081 for (jk = 0; jk < angles_found_count; jk++)
1082 {
1083 //dbg(0, "RR:x:009:\n");
1084
1085 is_dupl = 0;
1086
1087 if (jk > 0)
1088 {
1089 for (ad = 0; ad < angles_found_count; ad++)
1090 {
1091 if ((ad != jk) && (angles_found[ad] == angles_found[jk]))
1092 {
1093 // we already have this angle, continue to the next one
1094 //dbg(0, "RR:x:dupl\n");
1095 is_dupl = 1;
1096 continue;
1097 }
1098 }
1099 }
1100
1101 if (is_dupl == 1)
1102 {
1103 continue;
1104 }
1105
1106 // dbg(0, "RR:x:010\n");
1107
1108 if (angles_found[jk] == drive_here_angle)
1109 {
1110 next_roads_and_angles_old = next_roads_and_angles;
1111 if (jk != 0)
1112 {
1113 next_roads_and_angles = g_strdup_printf("%s|x%d", next_roads_and_angles, angles_found[jk]);
1114 g_free(next_roads_and_angles_old);
1115 }
1116 else
1117 {
1118 next_roads_and_angles = g_strdup_printf("x%d", angles_found[jk]);
1119 }
1120 }
1121 else
1122 {
1123 next_roads_and_angles_old = next_roads_and_angles;
1124 if (jk != 0)
1125 {
1126 next_roads_and_angles = g_strdup_printf("%s|%d", next_roads_and_angles, angles_found[jk]);
1127 g_free(next_roads_and_angles_old);
1128 }
1129 else
1130 {
1131 next_roads_and_angles = g_strdup_printf("%d", angles_found[jk]);
1132 }
1133 }
1134 }
1135
1136 // example: "175|180|x250" // "x" mean this is the road we should drive on next, others are angles
1137 // 0 -> straight
1138 // less than 0 -> right
1139 // more than 0 -> left
1140 // ** // dbg(0, "RR:x:RES=%s\n", next_roads_and_angles);
1141 #ifdef HAVE_API_ANDROID
1142 android_send_generic_text((4 + depth), next_roads_and_angles);
1143 #endif
1144 g_free(next_roads_and_angles);
1145 }
1146 else
1147 {
1148 #ifdef HAVE_API_ANDROID
1149 android_send_generic_text((4 + depth), "");
1150 #endif
1151 }
1152 }
1153 else
1154 {
1155 #ifdef HAVE_API_ANDROID
1156 android_send_generic_text((4 + depth), "");
1157 #endif
1158 }
1159
1160
1161 #endif
1162
1163 }
1164
1165
1166 void tracking_send_lanes_info(struct map_rect *mr, int id_hi, int id_lo, int street_dir, int depth)
1167 {
1168
1169 struct item *item_lanes;
1170
1171 // COST: 102
1172 dbg(0, "COST:102\n");
1173 item_lanes = map_rect_get_item_byid(mr, id_hi, id_lo);
1174
1175 if (item_lanes)
1176 {
1177 //dbg(0, "LL01:000.1\n");
1178
1179 struct attr attr_l;
1180 gchar *lanes_info_l = NULL;
1181 gchar *lanes_info_l_for = NULL;
1182 gchar *lanes_info_tl = NULL;
1183
1184 if (item_attr_get(item_lanes, attr_street_lanes, &attr_l))
1185 {
1186 lanes_info_l = g_strdup_printf("%s", attr_l.u.str);
1187 //dbg(0, "LL01:000.2 %s\n", attr_l.u.str);
1188 }
1189
1190 if (item_attr_get(item_lanes, attr_street_lanes_forward, &attr_l))
1191 {
1192 lanes_info_l_for = g_strdup_printf("%s", attr_l.u.str);
1193 //dbg(0, "LL01:000.3 %s\n", attr_l.u.str);
1194 }
1195
1196 if (item_attr_get(item_lanes, attr_street_turn_lanes, &attr_l))
1197 {
1198 lanes_info_tl = g_strdup_printf("%s", attr_l.u.str);
1199 //dbg(0, "LL01:000.4 %s\n", attr_l.u.str);
1200 }
1201
1202 if ((lanes_info_l) && (lanes_info_tl))
1203 {
1204 //dbg(0, "LL01:001\n");
1205
1206 gchar *lanes_info = NULL;
1207
1208 if (lanes_info_l_for)
1209 {
1210 //dbg(0, "LL01:002\n");
1211 lanes_info = g_strdup_printf("%d:%s:%s:%s", street_dir, lanes_info_l, lanes_info_l_for, lanes_info_tl);
1212 g_free(lanes_info_l_for);
1213 lanes_info_l_for = NULL;
1214 }
1215 else
1216 {
1217 //dbg(0, "LL01:003\n");
1218 lanes_info = g_strdup_printf("%d:%s:%s:%s", street_dir, lanes_info_l, lanes_info_l, lanes_info_tl);
1219 }
1220
1221 g_free(lanes_info_l);
1222 lanes_info_l = NULL;
1223 g_free(lanes_info_tl);
1224 lanes_info_tl = NULL;
1225
1226 //dbg(0, "LL01:004 %s\n", lanes_info);
1227
1228 #ifdef HAVE_API_ANDROID
1229 // example: "1:3:2:none|through;left"
1230 android_send_generic_text((8 + depth), lanes_info);
1231 #endif
1232 g_free(lanes_info);
1233 lanes_info = NULL;
1234 }
1235 else
1236 {
1237 #ifdef HAVE_API_ANDROID
1238 android_send_generic_text((8 + depth), "");
1239 #endif
1240 }
1241
1242 if (lanes_info_l)
1243 {
1244 g_free(lanes_info_l);
1245 lanes_info_l = NULL;
1246 }
1247
1248 if (lanes_info_tl)
1249 {
1250 g_free(lanes_info_tl);
1251 lanes_info_tl = NULL;
1252 }
1253
1254 if (lanes_info_l_for)
1255 {
1256 g_free(lanes_info_l_for);
1257 lanes_info_l_for = NULL;
1258 }
1259
1260 }
1261 }
1262
1263
1264
1265
1266
1267
1268 // ###############
1269 //
1270 // this gets called (also) every time the gps sends new coord data (called from navit.c --> navit_vehicle_update) only if "tracking_flag" is "true" !!
1271 //
1272 // ###############
1273 void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro)
1274 {
1275 struct tracking_line *t;
1276 int i, value, min, time;
1277 struct coord lpnt;
1278 struct coord cin;
1279 struct attr valid, speed_attr, direction_attr, coord_geo, lag, time_attr, static_speed, static_distance;
1280 double speed, direction;
1281 int too_slow_or_little_movement = 0;
1282
1283 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1284 dbg(0, "ROUTExxPOSxx:routetracking_update: enter\n");
1285 #endif
1286
1287 if (v)
1288 {
1289 tr->vehicle = v;
1290 }
1291
1292 if (vehicleprofile)
1293 {
1294 tr->vehicleprofile = vehicleprofile;
1295 }
1296
1297 if (!tr->vehicle)
1298 {
1299 return;
1300 }
1301
1302 if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
1303 {
1304 valid.u.num = attr_position_valid_valid;
1305 }
1306
1307 if (valid.u.num == attr_position_valid_invalid)
1308 {
1309 tr->valid = valid.u.num;
1310 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1311 dbg(0, "ROUTExxPOSxx:(position invalid):return 002\n");
1312 #endif
1313 return;
1314 }
1315
1316 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))
1317 {
1318 //dbg(0,"failed to get position data %d %d %d %d\n",
1319 //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL),
1320 //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL),
1321 //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL),
1322 //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL));
1323 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1324 dbg(0, "ROUTExxPOSxx:(some or all position attributes missing):return 003\n");
1325 #endif
1326 return;
1327 }
1328
1329 if (!vehicleprofile_get_attr(vehicleprofile, attr_static_speed, &static_speed, NULL) || !vehicleprofile_get_attr(vehicleprofile, attr_static_distance, &static_distance, NULL))
1330 {
1331 static_speed.u.num = 3;
1332 static_distance.u.num = 10;
1333 //dbg(1,"Using defaults for static position detection\n");
1334 }
1335
1336 //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num);
1337 time = iso8601_to_secs(time_attr.u.str);
1338 speed = *speed_attr.u.numd;
1339 direction = *direction_attr.u.numd;
1340 tr->valid = attr_position_valid_valid;
1341
1342 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1343 dbg(0, "ROUTExxPOSxx:routetracking_update: v direction=%f lat=%f lon=%f\n", direction, coord_geo.u.coord_geo->lat, coord_geo.u.coord_geo->lng);
1344 struct coord cc999;
1345 transform_from_geo(pro, coord_geo.u.coord_geo, &cc999);
1346 dbg(0, "ROUTExxPOSxx:routetracking_update (unchanged GPS pos): v x=%d y=%d\n", cc999.x, cc999.y);
1347 #endif
1348
1349 transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
1350
1351
1352 if ((global_vehicle_profile != 1) && (global_vehicle_profile != 2)) // not bicycle mode
1353 {
1354 if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num))
1355 {
1356 //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);
1357 tr->valid = attr_position_valid_static;
1358 // tr->valid = attr_position_valid_valid;
1359 tr->speed = 0;
1360
1361 too_slow_or_little_movement = 1;
1362 //dbg(0, "return 001\n");
1363 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1364 dbg(0, "ROUTExxPOSxx:(too slow or to little movement):return 001 speed=%f movement=%f\n", speed, transform_distance(pro, &tr->last_in, &tr->curr_in));
1365 #endif
1366 // -- dont end here!! -- // return;
1367 }
1368 }
1369
1370 if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0)
1371 {
1372 //dbg(0, "Vehicle LAG on. lag=%d\n", lag.u.num);
1373
1374 double espeed;
1375 int edirection;
1376 if (time - tr->time == 1)
1377 {
1378 /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */
1379 espeed = speed + (speed - tr->speed) * lag.u.num / 10;
1380 /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */
1381 edirection = direction + tracking_angle_diff(direction, tr->direction, 360) * lag.u.num / 10;
1382 }
1383 else
1384 {
1385 //dbg(1,"no speed and direction extrapolation\n");
1386 espeed = speed;
1387 edirection = direction;
1388 }
1389 //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
1390 //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
1391 transform_project(pro, &tr->curr_in, espeed * lag.u.num / 36, edirection, &tr->curr_in);
1392 //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
1393 }
1394
1395 tr->time = time;
1396 tr->pro = pro;
1397 #if 0
1398
1399 tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime);
1400 #endif
1401 //dbg(0,"curr_IN :20 0x%x,0x%x\n", tr->curr_in.x, tr->curr_in.y);
1402 //dbg(0,"curr_out:21 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
1403
1404 tr->curr_angle = tr->direction = direction; // ?? int mixed with float ??
1405 tr->speed = speed;
1406 tr->last_in = tr->curr_in;
1407 tr->last_out = tr->curr_out;
1408
1409 // ---------------
1410 // tr->last[0] = tr->curr[0];
1411 // tr->last[1] = tr->curr[1];
1412
1413 tr->last[0].x = tr->curr[0].x;
1414 tr->last[0].y = tr->curr[0].y;
1415
1416 tr->last[1].x = tr->curr[1].x;
1417 tr->last[1].y = tr->curr[1].y;
1418
1419 tr->last_item_mark.id_hi = tr->curr_item_mark.id_hi;
1420 tr->last_item_mark.id_lo = tr->curr_item_mark.id_lo;
1421 tr->last_item_mark.map = tr->curr_item_mark.map;
1422
1423 global_debug_trlast_start.x = tr->last[0].x;
1424 global_debug_trlast_start.y = tr->last[0].y;
1425 global_debug_trlast_end.x = tr->last[1].x;
1426 global_debug_trlast_end.y = tr->last[1].y;
1427
1428 // ---------------
1429
1430 //dbg(0,"curr_out:60 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
1431
1432 if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > TRACKING_MAX_DIST_RELOAD_MAP_DATA_THRESHOLD) // 500 (default)
1433 {
1434 // dbg(0, "**TRACKING:flush**\n");
1435 tracking_flush(tr);
1436 tracking_doupdate_lines(tr, &tr->curr_in, pro, TRACKING_MAX_DIST_RELOAD_MAP_DATA_LIMIT); // max_dist = 1000 (default)
1437 tr->last_updated = tr->curr_in;
1438 //dbg(0,"curr_IN :61 0x%x,0x%x\n", tr->curr_in.x, tr->curr_in.y);
1439 //dbg(0,"curr_out:62 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
1440 //dbg(1,"update end\n");
1441 }
1442
1443 //dbg(0,"curr_out:70 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
1444 tr->street_direction = 0;
1445 t = tr->lines;
1446 tr->curr_line = NULL;
1447 min = INT_MAX / 2;
1448
1449 int angle_new = tr->direction;
1450 int angle_delta_save;
1451
1452
1453 // dbg(0, "TR_POSANG:002:angle=%d dir=%f\n", tr->curr_angle, tr->direction);
1454
1455
1456 // -------- full route ---------------------------------------------
1457 // -------- full route ---------------------------------------------
1458 struct route *route2 = NULL;
1459 struct map *route_map2 = NULL;
1460 struct map_rect *mr2 = NULL;
1461 struct item *item2 = NULL;
1462 struct coord c2;
1463 struct coord c2_prev;
1464 struct coord_geo g1;
1465 struct coord_geo g2;
1466 struct coord_geo g3;
1467 struct coord_geo g4;
1468 struct coord cp_xx;
1469 struct attr attr_si;
1470 struct item *sitem;
1471 int first2 = 1;
1472 int max_route_items_to_check = 5;
1473 int route_items_checked = 0;
1474 int min2 = INT_MAX / 2;
1475 int winner_hi = 0;
1476 int winner_lo = 0;
1477 int winner_offset = 0;
1478 struct map *winner_map = NULL;
1479 int temp_hi = 0;
1480 int temp_lo = 0;
1481 struct map *temp_map = NULL;
1482 int temp_offset = 0;
1483 int first_seg = 1;
1484 int route_ongoing_penalty = 0;
1485 int on_what_route_seg = -1;
1486 int route_seg_winner_hi = -1;
1487 int route_seg_winner_lo = -1;
1488
1489
1490 global_debug_route_seg_winner_start.x = 0;
1491 global_debug_route_seg_winner_start.y = 0;
1492 global_debug_route_seg_winner_end.x = 0;
1493 global_debug_route_seg_winner_end.y = 0;
1494
1495 global_debug_seg_winner_start.x = 0;
1496 global_debug_seg_winner_start.y = 0;
1497 global_debug_seg_winner_end.x = 0;
1498 global_debug_seg_winner_end.y = 0;
1499
1500 global_debug_route_seg_winner_p_start.x = 0;
1501 global_debug_route_seg_winner_p_start.y = 0;
1502
1503 global_debug_seg_route_start.x = 0;
1504 global_debug_seg_route_start.y = 0;
1505
1506 global_debug_seg_route_end.x = 0;
1507 global_debug_seg_route_end.y = 0;
1508
1509 int need_turn_around = 0;
1510
1511 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1512 int millis_saved = tracking_time_millis();
1513 dbg(0,"ROUTExxPOSxx:millis_saved=%d\n", millis_saved);
1514 #endif
1515
1516 route2 = tr->rt;
1517
1518 #ifdef NAVIT_TRACKING_STICK_TO_ROUTE
1519
1520 if ((route2) && (route2->route_status != route_status_no_destination))
1521 {
1522 route_map2 = route_get_map(route2);
1523
1524 if (route_map2)
1525 {
1526 mr2 = map_rect_new(route_map2, NULL);
1527 }
1528
1529 if (mr2)
1530 {
1531 item2 = map_rect_get_item(mr2);
1532 }
1533
1534 if (
1535 ((global_vehicle_profile != 1) && (global_vehicle_profile != 2)) // not bicycle mode
1536 && (item2 && item2->type == type_route_start_reverse) // we need to turn around! --> then disable stick to route-seg
1537 )
1538 {
1539 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1540 dbg(0,"ROUTExxPOSxx:turn around, disable snap to route-seg\n");
1541 #endif
1542 // need_turn_around = 1;
1543
1544 }
1545 else
1546 {
1547
1548 if (item2 && item2->type == type_route_start)
1549 {
1550 item2 = map_rect_get_item(mr2);
1551 }
1552 else if (item2 && item2->type == type_route_start_reverse)
1553 {
1554 item2 = map_rect_get_item(mr2);
1555 need_turn_around = 1;
1556 }
1557 //dbg(0,"curr_out:loopR.00:----------------------------------\n");
1558
1559 if (item2)
1560 {
1561 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1562 //char *f11 = g_strdup_printf("/sdcard/zanavi_route_001_%d.gpx", millis_saved);
1563 //FILE *fp2 = navit_start_gpx_file(f11);
1564 //g_free(f11);
1565 #endif
1566 if (item2)
1567 {
1568 temp_offset = -1;
1569
1570 //dbg(0,"curr_out:loopR.02i:id_hi=%d id_lo=%d\n", item2->id_hi, item2->id_lo);
1571 if (item_attr_get(item2, attr_street_item, &attr_si))
1572 {
1573 sitem = attr_si.u.item;
1574 // item_dump_attr_stdout(sitem, route_map2);
1575 if (sitem)
1576 {
1577 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1578 // dbg(0,"curr_out:loopR.02i2:id_hi=%d id_lo=%d\n", sitem->id_hi, sitem->id_lo);
1579 #endif
1580 temp_hi = sitem->id_hi;
1581 temp_lo = sitem->id_lo;
1582 temp_map = sitem->map;
1583 }
1584 }
1585 else
1586 {
1587 temp_hi = 0;
1588 temp_lo = 0;
1589 temp_map = NULL;
1590 }
1591 //item_dump_attr_stdout(item2, route_map2);
1592 }
1593
1594 int num = item_coord_get(item2, &c2, 1);
1595 if (num == 1)
1596 {
1597 global_debug_seg_route_start.x = c2.x;
1598 global_debug_seg_route_start.y = c2.y;
1599
1600 global_debug_seg_route_end.x = 0;
1601 global_debug_seg_route_end.y = 0;
1602
1603 temp_offset = -1;
1604 first2 = 0;
1605
1606 c2_prev.x = c2.x;
1607 c2_prev.y = c2.y;
1608
1609 //dbg(0,"curr_out:loopR.01:%d\n", route_items_checked);
1610 route_items_checked++;
1611 while (item2)
1612 {
1613 //dbg(0,"curr_out:loopR.02:%d\n", route_items_checked);
1614 if (!item_coord_get(item2, &c2, 1))
1615 {
1616 item2 = map_rect_get_item(mr2);
1617
1618 temp_offset = -1;
1619
1620 if (item2)
1621 {
1622 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1623 // dbg(0,"curr_out:loopR.02i:id_hi=%d id_lo=%d\n", item2->id_hi, item2->id_lo);
1624 #endif
1625 if (item_attr_get(item2, attr_street_item, &attr_si))
1626 {
1627 sitem = attr_si.u.item;
1628 // item_dump_attr_stdout(sitem, route_map2);
1629 if (sitem)
1630 {
1631 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1632 // dbg(0,"curr_out:loopR.02i2:id_hi=%d id_lo=%d\n", sitem->id_hi, sitem->id_lo);
1633 #endif
1634 temp_hi = sitem->id_hi;
1635 temp_lo = sitem->id_lo;
1636 temp_map = sitem->map;
1637 }
1638 }
1639 //item_dump_attr_stdout(item2, route_map2);
1640 }
1641
1642 route_items_checked++;
1643 //dbg(0,"curr_out:loopR.03:%d\n", route_items_checked);
1644 if (route_items_checked > max_route_items_to_check)
1645 {
1646 // check no more route items
1647 break;
1648 }
1649 first2 = 1;
1650 // we favor route segments nearer to start of route (penalty increases the further we go along the route)
1651 route_ongoing_penalty = route_ongoing_penalty + 4;
1652 continue;
1653 }
1654
1655 temp_offset++;
1656 //c2_prev.x = c2.x;
1657 //c2_prev.y = c2.y;
1658
1659 if (first2 == 1)
1660 {
1661 //dbg(0,"curr_out:loopR.02f\n");
1662 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1663 // navit_add_trkpoint_to_gpx_file(fp2, &c2);
1664 #endif
1665 first2 = 0;
1666 }
1667 else
1668 {
1669
1670 if (first_seg == 1)
1671 {
1672 global_debug_seg_route_end.x = c2.x;
1673 global_debug_seg_route_end.y = c2.y;
1674 first_seg = 0;
1675 }
1676
1677
1678 //dbg(0,"curr_out:loopR.04:%d\n", route_items_checked);
1679
1680 cp_xx.x = tr->curr_in.x;
1681 cp_xx.y = tr->curr_in.y;
1682 // set "lpnt" coords to the point in the line (from "c2_prev" -> "c2") that is closest to "cp_xx"
1683 value = transform_distance_line_sq(&c2_prev, &c2, &cp_xx, &lpnt);
1684
1685 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1686 #if 0
1687 dbg(0,"curr_out:loopR.05:v=%d hi=%d lo=%d off=%d\n", value, temp_hi, temp_lo, temp_offset);
1688 transform_to_geo(projection_mg, &c2_prev, &g1);
1689 transform_to_geo(projection_mg, &c2, &g2);
1690 transform_to_geo(projection_mg, &cp_xx, &g3);
1691 transform_to_geo(projection_mg, &lpnt, &g4);
1692 //dbg(0,"curr_out:loopR.06:route seg %4.16f,%4.16f --> %4.16f,%4.16f\n", g1.lat, g1.lng, g2.lat, g2.lng);
1693 dbg(0,"curr_out:loopR.07:http://maps.google.com/maps/api/staticmap?size=512x512&markers=color:blue|label:START|%4.6f,%4.6f&markers=color:red|label:END|%4.6f,%4.6f&markers=color:green|label:POS|%4.6f,%4.6f\n", g1.lat, g1.lng, g2.lat, g2.lng, g3.lat, g3.lng);
1694 //navit_add_trkpoint_to_gpx_file(fp2, &c2);
1695 #endif
1696 #endif
1697
1698 if ((temp_hi != 0) && (temp_lo != 0))
1699 {
1700 if ((value + route_ongoing_penalty) < min2)
1701 {
1702 min2 = value + route_ongoing_penalty;
1703 winner_hi = temp_hi;
1704 winner_lo = temp_lo;
1705 winner_map = temp_map;
1706 winner_offset = temp_offset;
1707
1708 route_seg_winner_hi = item2->id_hi;
1709 route_seg_winner_lo = item2->id_lo;
1710
1711 on_what_route_seg = route_items_checked;
1712
1713 // dbg(0, "TR_POSANG:R00:on_what_route_seg=%d hi=%d lo=%d whi=%d wlo=%d\n", on_what_route_seg, route_seg_winner_hi, route_seg_winner_lo, winner_hi, winner_lo);
1714
1715 global_debug_route_seg_winner_start.x = c2_prev.x;
1716 global_debug_route_seg_winner_start.y = c2_prev.y;
1717 global_debug_route_seg_winner_end.x = c2.x;
1718 global_debug_route_seg_winner_end.y = c2.y;
1719
1720 global_debug_route_seg_winner_p_start.x = lpnt.x;
1721 global_debug_route_seg_winner_p_start.y = lpnt.y;
1722
1723 }
1724 }
1725 }
1726
1727 c2_prev.x = c2.x;
1728 c2_prev.y = c2.y;
1729 }
1730 }
1731 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1732 //navit_end_gpx_file(fp2);
1733 #endif
1734
1735 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1736 dbg(0,"curr_out:loopR.99:++++++++++++++++++++++++++++++++++ min2=%d w_hi=%d w_lo=%d w_off=%d\n", min2, winner_hi, winner_lo, winner_offset);
1737 #endif
1738 winner_offset--;
1739
1740 // dbg(0, "TR_POSANG:S00:whi=%d wlo=%d\n", winner_hi, winner_lo);
1741
1742 int min_dist_max = 800;
1743
1744 if ((global_vehicle_profile == 1) || (global_vehicle_profile == 2)) // bicycle mode
1745 {
1746 min_dist_max = 1500; // 3500; // HINT: if vehicle is further from route than this distance, then dont stick to route (and re-route!)
1747 }
1748
1749 //if (min2 < min_dist_max)
1750 //{
1751 // --> min3 and min2 should be the same, so no need to calculate min3 here
1752 int min3 = transform_distance_sq(&global_debug_route_seg_winner_p_start, &tr->curr_in);
1753 if (min3 >= min_dist_max) // real gps position too far from route!!
1754 {
1755 // make winner invalid! so it won't be used for current position
1756 winner_hi = 0;
1757 winner_lo = 0;
1758
1759 global_debug_route_seg_winner_start.x = 0;
1760 global_debug_route_seg_winner_start.y = 0;
1761 global_debug_route_seg_winner_end.x = 0;
1762 global_debug_route_seg_winner_end.y = 0;
1763
1764 global_debug_route_seg_winner_p_start.x = 0;
1765 global_debug_route_seg_winner_p_start.y = 0;
1766
1767 }
1768 //}
1769
1770 }
1771 }
1772
1773 if (mr2)
1774 {
1775 map_rect_destroy(mr2);
1776 }
1777 }
1778
1779 #endif
1780
1781 // -------- full route ---------------------------------------------
1782 // -------- full route ---------------------------------------------
1783
1784
1785
1786
1787 //dbg(0,"curr_out:loop.00:----------------------------------\n");
1788 int count_001 = 0;
1789
1790 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1791 //char *f22 = g_strdup_printf("/sdcard/zanavi_route_002_%d.gpx", millis_saved);
1792 //FILE *fp3 = navit_start_gpx_file(f22);
1793 //g_free(f22);
1794
1795 //char *f23 = g_strdup_printf("/sdcard/zanavi_route_002_%d_chosen.gpx", millis_saved);
1796 //FILE *fp3a = navit_start_gpx_file(f23);
1797 //g_free(f23);
1798 #endif
1799
1800 struct item *item33 = NULL;
1801 struct item *item34 = NULL;
1802 struct map_rect *mr33 = NULL;
1803 struct roadprofile *roadp;
1804
1805 #if 0
1806 global_debug_coord_list_items = 0;
1807 #endif
1808
1809 int winner_is_route_seg = 0;
1810 int is_route_seg = 0;
1811
1812 while (t)
1813 {
1814 count_001++;
1815 //dbg(0,"curr_out:loop.01:%d\n", count_001);
1816
1817 struct street_data *sd = t->street;
1818 item33 = &(sd->item);
1819
1820 // if (route2) // are we in routing mode?
1821 {
1822 roadp = vehicleprofile_get_roadprofile(tr->vehicleprofile, item33->type);
1823 // only include any roads that have a road profile in our vehicle profile
1824 if (roadp)
1825 {
1826
1827 // if ( ((winner_hi == 0) && (winner_lo == 0)) || (!item33) || ((winner_hi == item33->id_hi) && (winner_lo == item33->id_lo)) )
1828 {
1829 for (i = 0; i < sd->count - 1; i++)
1830 {
1831
1832 count_001++;
1833 // dbg(0,"curr_out:loop.02:%d\n", i);
1834
1835 // value = distance from point tr->curr_in.(x,y) to closest point on line (sd[i] -> sd[i+1]) squared
1836 // lpnt is set to point on line (sd[i] -> sd[i+1]) which is closest to tr->curr_in.(x,y)
1837
1838 if (global_demo_vehicle_short_switch == 1)
1839 {
1840 value = tracking_value(tr, t, i, &lpnt, min, 1, item33);
1841 }
1842 else
1843 {
1844 value = tracking_value(tr, t, i, &lpnt, min, ( 1 | 2 | 4 ), item33);
1845 }
1846
1847 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1848 // if (value < min)
1849 // {
1850 // dbg(0,"curr_out:loop.02a:i=%d v=%d min=%d id_hi=%d id_lo=%d w_off=%d w_value=%d\n", i, value, min, item33->id_hi, item33->id_lo, winner_offset, min2);
1851 // }
1852 #endif
1853
1854 // dbg(0, "TR_POSANG:R00w1:i=%d v=%d min=%d id_hi=%d id_lo=%d w_off=%d w_value=%d winner_hi=%d winner_lo=%d\n", i, value, min, item33->id_hi, item33->id_lo, winner_offset, min2, winner_hi, winner_lo);
1855
1856 is_route_seg = 0;
1857
1858 if ( ((winner_hi != 0) && (winner_lo != 0)) && (item33) && ((winner_hi == item33->id_hi) && (winner_lo == item33->id_lo)) && (winner_map == item33->map) )
1859 {
1860 //if (winner_offset == i)
1861 //{
1862
1863 is_route_seg = 1;
1864
1865 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1866 // dbg(0,"curr_out:loop.02a1:i=%d v=%d min=%d\n", i, value, min);
1867 #endif
1868 if ((global_vehicle_profile == 1) || (global_vehicle_profile == 2)) // bicycle mode
1869 {
1870 value = value - CORRECT_BC_FOR_ON_ROUTE_SEG_VALUE;
1871 }
1872 else
1873 {
1874 value = value - CORRECT_FOR_ON_ROUTE_SEG_VALUE; // default = 2600
1875 }
1876 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1877 // dbg(0,"curr_out:loop.02a2:i=%d v=%d min=%d\n", i, value, min);
1878 #endif
1879 //}
1880 }
1881
1882
1883 #ifdef NAVIT_ROUTING_DEBUG_PRINT
1884 //navit_add_trkpoint_to_gpx_file(fp3, &sd->c[i]);
1885 //navit_add_trkpoint_to_gpx_file(fp3, &sd->c[i + 1]);
1886 //navit_end_gpx_track_seg(fp3);
1887 //navit_start_gpx_track_seg(fp3);
1888 #endif
1889
1890
1891 #if 0
1892 if ((sd) && (item33))
1893 {
1894 if ((item33->id_hi == 8013)&&(item33->id_lo == 108612))
1895 {
1896 if ((global_debug_coord_list_items + 2) > MAX_DEBUG_COORDS)
1897 {
1898 global_debug_coord_list_items = 0;
1899 }
1900
1901 dbg(0,"curr_out:debug:START:coord_list=%d\n", global_debug_coord_list_items);
1902 global_debug_coord_list[global_debug_coord_list_items].x = sd->c[i].x;
1903 global_debug_coord_list[global_debug_coord_list_items].y = sd->c[i].y;
1904 global_debug_coord_list_items++;
1905 dbg(0,"curr_out:debug:END:coord_list=%d\n", global_debug_coord_list_items);
1906 global_debug_coord_list[global_debug_coord_list_items].x = sd->c[i + 1].x;
1907 global_debug_coord_list[global_debug_coord_list_items].y = sd->c[i + 1].y;
1908 global_debug_coord_list_items++;
1909 }
1910 }
1911 #endif
1912
1913
1914 #if 0
1915 //dbg(0,"curr_out:loop.02i1:id_hi=%d id_lo=%d\n", item33->id_hi, item33->id_lo);
1916 if (item33->map)
1917 {
1918 mr33 = map_rect_new(item33->map, NULL);
1919 if (mr33)
1920 {
1921 item33 = map_rect_get_item_byid(mr33, item33->id_hi, item33->id_lo);
1922 // ------ DEBUG ---------
1923 // ------ DEBUG ---------
1924 // ------ DEBUG ---------
1925 if (item33)
1926 {
1927 dbg(0,"curr_out:loop.02i2:id_hi=%d id_lo=%d\n", item33->id_hi, item33->id_lo);
1928 item_dump_attr_stdout(item33, item33->map);
1929 }
1930 // ------ DEBUG ---------
1931 // ------ DEBUG ---------
1932 // ------ DEBUG ---------
1933 map_rect_destroy(mr33);
1934 }
1935 }
1936 #endif
1937
1938 // dbg(0, "TR_POSANG:R00:rr2:is_route_seg=%d\n", is_route_seg);
1939
1940 if (is_route_seg == 1)
1941 {
1942 int angle_route2 = transform_get_angle_delta(&global_debug_route_seg_winner_start, &global_debug_route_seg_winner_end, 1);
1943 int angle_delta_save2 = tracking_angle_abs_diff(angle_route2, t->angle[i], 180);
1944 if (angle_delta_save2 > 90)
1945 {
1946 angle_delta_save2 = 180 - angle_delta_save2;
1947 }
1948
1949 #if 0
1950 int tt = tracking_angle_abs_diff(0, 180, 180);
1951 dbg(0, "NAV_TAANG:002:t:0,180=%d\n", tt);
1952 tt = tracking_angle_abs_diff(180, 180, 180);
1953 dbg(0, "NAV_TAANG:002:t:180,180=%d\n", tt);
1954 tt = tracking_angle_abs_diff(-180, 180, 180);
1955 dbg(0, "NAV_TAANG:002:t:-180,180=%d\n", tt);
1956 tt = tracking_angle_abs_diff(361, 1, 180);
1957 dbg(0, "NAV_TAANG:002:t:361,1=%d\n", tt);
1958 tt = tracking_angle_abs_diff(-8, (360-8), 180);
1959 dbg(0, "NAV_TAANG:002:t:-8,(360-8)=%d\n", tt);
1960 #endif
1961
1962 // dbg(0, "NAV_TAANG:002:angle_delta_save2=%d v=%d\n", angle_delta_save2, value);
1963
1964 value = value - (50 * (100 - abs(angle_delta_save2)));
1965
1966 // dbg(0, "NAV_TAANG:002:v new=%d\n", value);
1967
1968 }
1969
1970 if (value < min)
1971 {
1972 //dbg(0,"curr_out:loop.03\n");
1973
1974 if (is_route_seg == 1)
1975 {
1976 winner_is_route_seg = 1;
1977 }
1978
1979 struct coord lpnt_tmp;
1980 int angle_delta = tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
1981 tr->curr_line = t;
1982 tr->pos = i;
1983
1984 // -------------------
1985 // -------------------
1986 // tr->curr[0] = sd->c[i];
1987 // tr->curr[1] = sd->c[i + 1];
1988
1989 tr->curr[0].x = sd->c[i].x;
1990 tr->curr[0].y = sd->c[i].y;
1991
1992 tr->curr[1].x = sd->c[i + 1].x;
1993 tr->curr[1].y = sd->c[i + 1].y;
1994
1995 if ((sd) && (item33))
1996 {
1997 tr->curr_item_mark.id_hi = item33->id_hi;
1998 tr->curr_item_mark.id_lo = item33->id_lo;
1999 tr->curr_item_mark.map = item33->map;
2000 }
2001 else
2002 {
2003 tr->curr_item_mark.id_hi = 0x0;
2004 tr->curr_item_mark.id_lo = 0x0;
2005 tr->curr_item_mark.map = NULL;
2006 }
2007 // -------------------
2008 // -------------------
2009
2010
2011 if (sd)
2012 {
2013 tr->curr_max_speed = sd->maxspeed;
2014 }
2015 else
2016 {
2017 tr->curr_max_speed = -1;
2018 }
2019
2020
2021
2022 global_debug_seg_winner_start.x = sd->c[i].x;
2023 global_debug_seg_winner_start.y = sd->c[i].y;
2024 global_debug_seg_winner_end.x = sd->c[i + 1].x;
2025 global_debug_seg_winner_end.y = sd->c[i + 1].y;
2026
2027
2028 #if 0
2029 dbg(0,"curr_out:loop.02i1:id_hi=%d id_lo=%d\n", item33->id_hi, item33->id_lo);
2030 if (item33->map)
2031 {
2032 mr33 = map_rect_new(item33->map, NULL);
2033 if (mr33)
2034 {
2035 item34 = map_rect_get_item_byid(mr33, item33->id_hi, item33->id_lo);
2036 // ------ DEBUG ---------
2037 // ------ DEBUG ---------
2038 // ------ DEBUG ---------
2039 if (item34)
2040 {
2041 dbg(0,"curr_out:loop.02i2:id_hi=%d id_lo=%d map=%p\n", item34->id_hi, item34->id_lo, item34->map);
2042 item_dump_attr_stdout(item34, item34->map);
2043 }
2044 // ------ DEBUG ---------
2045 // ------ DEBUG ---------
2046 // ------ DEBUG ---------
2047 map_rect_destroy(mr33);
2048 }
2049 item34 = NULL;
2050 }
2051 #endif
2052
2053
2054 #ifdef NAVIT_ROUTING_DEBUG_PRINT
2055 //transform_to_geo(projection_mg, &sd->c[i], &g1);
2056 //transform_to_geo(projection_mg, &sd->c[i + 1], &g2);
2057 //dbg(0,"curr_out:loop.06:route seg %4.6f,%4.6f --> %4.6f,%4.6f\n", g1.lat, g1.lng, g2.lat, g2.lng);
2058 //dbg(0,"curr_out:loop.07:http://maps.google.com/maps/api/staticmap?size=512x512&markers=color:blue|label:START|%4.6f,%4.6f&markers=color:red|label:END|%4.6f,%4.6f\n", g1.lat, g1.lng, g2.lat, g2.lng);
2059 #endif
2060
2061 /*
2062 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
2063 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp),
2064 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref,
2065 tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0,
2066 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0,
2067 value
2068 );
2069 */
2070
2071 // ---------------------------------------------------------------------------------
2072 // ---------------------------------------------------------------------------------
2073 // change the current Vehicle (GPS-)Position to the point on the closest line/street
2074
2075 if (is_route_seg == 1)
2076 {
2077 lpnt.x = global_debug_route_seg_winner_p_start.x;
2078 lpnt.y = global_debug_route_seg_winner_p_start.y;
2079 }
2080
2081 tr->curr_out.x = lpnt.x;
2082 tr->curr_out.y = lpnt.y;
2083
2084 global_debug_seg_winner_p_start.x = lpnt.x;
2085 global_debug_seg_winner_p_start.y = lpnt.y;
2086
2087 if (is_route_seg == 1)
2088 {
2089 int angle_route = transform_get_angle_delta(&global_debug_route_seg_winner_start, &global_debug_route_seg_winner_end, 1);
2090 int angle_delta_save = tracking_angle_abs_diff(angle_route,t->angle[i], 360);
2091 if (angle_delta_save > 180)
2092 {
2093 angle_delta_save = 360 - angle_delta_save;
2094 }
2095
2096
2097
2098 dbg(0, "NAV_TAANG:001:winner dir=%d ads=%d road angle=%d\n", angle_route, angle_delta_save, t->angle[i]);
2099
2100 if (angle_delta_save > 92)
2101 {
2102 angle_new = (angle_route + 180) % 360;
2103 tr->street_direction = -1;
2104 }
2105 else
2106 {
2107 angle_new = angle_route;
2108 tr->street_direction = 1;
2109 }
2110
2111 if (need_turn_around == 1)
2112 {
2113 #if 1
2114 // int angle_delta_route = tracking_angle_abs_diff(tr->curr_angle, angle_route, 360); // delta between route and vehicle direction
2115 angle_new = t->angle[i]; // set angle/direction to the angle of the line/street
2116
2117 if (angle_delta < 70) // 90° - 20°
2118 {
2119 tr->street_direction = 1;
2120 }
2121 else if (angle_delta > 110) // 90° + 20°
2122 {
2123 tr->street_direction = -1;
2124 }
2125 else
2126 {
2127 tr->street_direction = 0;
2128 }
2129 #endif
2130
2131 #if 0
2132 tr->street_direction = -tr->street_direction;
2133 #endif
2134
2135 }
2136 }
2137 else
2138 {
2139 angle_new = t->angle[i]; // also set angle/direction to the angle of the line/street
2140 }
2141 // dbg(0, "ROUTExxPOSxx:coord_geo_valid=0 001\n");
2142 tr->coord_geo_valid = 0;
2143 //dbg(0,"curr_out:70.2 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
2144 // change the current Vehicle (GPS-)Position to the point on the closest line/street
2145 // ---------------------------------------------------------------------------------
2146 // ---------------------------------------------------------------------------------
2147
2148 // dbg(0, "TR_POSANG:R01:angle_new=%d angle_delta=%d is_route_seg=%d winner_is_route_seg=%d\n", angle_new, angle_delta, is_route_seg, winner_is_route_seg);
2149
2150 angle_delta_save = angle_delta;
2151
2152 if (is_route_seg != 1)
2153 {
2154 if (angle_delta < 70) // 90° - 20°
2155 {
2156 tr->street_direction = 1;
2157 }
2158 else if (angle_delta > 110) // 90° + 20°
2159 {
2160 tr->street_direction = -1;
2161 }
2162 else
2163 {
2164 tr->street_direction = 0;
2165 }
2166 }
2167 min = value;
2168 }
2169 }
2170 }
2171 }
2172 #ifdef NAVIT_ROUTING_DEBUG_PRINT
2173
2174 #if 0
2175 else
2176 {
2177 dbg(0, "ROUTExxPOSxx:(no roadprofile for this road)\n");
2178 }
2179 #endif
2180
2181 #endif
2182
2183 }
2184 t = t->next;
2185 }
2186
2187 #ifdef NAVIT_ROUTING_DEBUG_PRINT
2188 //navit_add_trkpoint_to_gpx_file(fp3a, &(tr->curr_out));
2189 //navit_end_gpx_file(fp3a);
2190 #endif
2191
2192
2193 #ifdef NAVIT_ROUTING_DEBUG_PRINT
2194 //navit_end_gpx_file(fp3);
2195 dbg(0,"curr_out:loop.99:++++++++++++++++++++++++++++++++++ count=%d\n", count_001);
2196 #endif
2197
2198 if ((global_vehicle_profile == 1) || (global_vehicle_profile == 2)) // bicycle mode
2199 {
2200
2201 #if 0
2202 if (tr->speed < TEN_KHM)
2203 {
2204 if (tr->street_direction != 0)
2205 {
2206 if (angle_delta_save < 85) // 90° - 5°
2207 {
2208 tr->street_direction = 1;
2209 }
2210 else if (angle_delta_save > 95) // 90° + 5°
2211 {
2212 tr->street_direction = -1;
2213 }
2214 else
2215 {
2216 tr->street_direction = 0; // vehicle can turn freely on map
2217 }
2218 }
2219 }
2220 #endif
2221
2222 }
2223 else
2224 {
2225
2226
2227 #ifndef CAR_STICK_TO_ROUTE_001
2228
2229 if (tr->speed < FIVE_KHM)
2230 {
2231 tr->street_direction = 0; // less than 5 km/h vehicle can turn freely on map
2232 }
2233 else if (tr->speed < TEN_KHM)
2234 {
2235 if (tr->street_direction != 0)
2236 {
2237 if (angle_delta_save < 85) // 90° - 5°
2238 {
2239 tr->street_direction = 1;
2240 }
2241 else if (angle_delta_save > 95) // 90° + 5°
2242 {
2243 tr->street_direction = -1;
2244 }
2245 else
2246 {
2247 tr->street_direction = 0; // vehicle can turn freely on map
2248 }
2249 }
2250 }
2251
2252 #endif
2253
2254 }
2255
2256
2257 // dbg(0, "SD:max_speed=%d\n", tr->curr_max_speed);
2258 #ifdef HAVE_API_ANDROID
2259 android_return_generic_int(8, tr->curr_max_speed);
2260 #endif
2261
2262 if (winner_is_route_seg == 1)
2263 {
2264 if ((global_vehicle_profile == 1) || (global_vehicle_profile == 2)) // bicycle mode
2265 {
2266
2267 #if 0
2268 if ((global_debug_route_seg_winner_start.x == global_debug_seg_winner_start.x) && (global_debug_route_seg_winner_start.y == global_debug_seg_winner_start.y))
2269 {
2270 tr->street_direction = 1;
2271 }
2272 else
2273 {
2274 tr->street_direction = -1;
2275 }
2276 #endif
2277
2278 #if 0
2279 if (angle_delta_save < 90)
2280 {
2281 tr->street_direction = 1;
2282 }
2283 else
2284 {
2285 tr->street_direction = -1;
2286 }
2287 #endif
2288 }
2289 }
2290
2291 // also set angle/direction to the angle of the line/street
2292 //dbg(0, "v-angle=%d\n", tr->curr_angle);
2293 if (tr->street_direction == -1)
2294 {
2295 // we need to drive the street in the other direction
2296 angle_new = (angle_new + 180) % 360;
2297
2298 tr->curr_angle = angle_new;
2299 tr->direction = tr->curr_angle;
2300 }
2301 else if (tr->street_direction == 1)
2302 {
2303 tr->curr_angle = angle_new;
2304 tr->direction = tr->curr_angle;
2305 }
2306 else // if (tr->street_direction == 0)
2307 {
2308 // we are almost 90° to the street/line, leave direction unchanged (for now) or we disabled this function (because of low speed)
2309 // tr->curr_angle = angle_new;
2310 // tr->direction = tr->curr_angle;
2311 }
2312 //dbg(0, "s-angle=%d\n", tr->curr_angle);
2313 // tr->coord_geo_valid = 0;
2314 // also set angle/direction to the angle of the line/street
2315
2316
2317 // dbg(0, "TR_POSANG:003:angle=%d dir=%f street_dir=%d angle_new=%d\n", tr->curr_angle, tr->direction, tr->street_direction, angle_new);
2318
2319
2320 //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
2321
2322 if (!tr->curr_line || min > tr->offroad_limit_pref)
2323 {
2324 tr->curr_out = tr->curr_in;
2325 //dbg(0,"curr_out:80 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
2326 #ifdef NAVIT_ROUTING_DEBUG_PRINT
2327 dbg(0, "ROUTExxPOSxx:YYYYYYY:coord_geo_valid=0 002\n");
2328 #endif
2329 tr->coord_geo_valid = 0;
2330 tr->street_direction = 0;
2331 }
2332
2333
2334 #if 1
2335 #ifdef HAVE_API_ANDROID
2336 // send lanes information to Java code
2337
2338 if ((tr->curr_item_mark.map) && (tr->curr_item_mark.id_hi != 0))
2339 {
2340 int next_lanes_id_hi = 0;
2341 int next_lanes_id_lo = 0;
2342 struct map* next_lanes_id_map = NULL;
2343 int street_dir_next = 1;
2344 int same_segment = 0;
2345
2346
2347 // -------------- all ways allowed to drive next ----------------------
2348 // -------------- all ways allowed to drive next ----------------------
2349
2350 #ifdef NAVIT_CALC_ALLOWED_NEXT_WAYS
2351
2352 if ((route_seg_winner_hi > -1) && (route_seg_winner_lo > -1))
2353 {
2354 struct route *route2 = NULL;
2355 struct map *route_map2 = NULL;
2356 struct map_rect *mr2 = NULL;
2357 struct item *item2 = NULL;
2358
2359 route2 = navit_get_route(global_navit);
2360
2361
2362 if ((route2) && (route2->graph))
2363 {
2364
2365 //dbg(0, "RR:03.1:\n");
2366 struct street_data *sd2 = tr->curr_line->street;
2367 //dbg(0, "RR:03.2:\n");
2368 // tr->street_direction;
2369 // tr->curr_angle;
2370 // struct route_info *ri = route_get_pos(route2);
2371 struct route_graph *rg = route2->graph;
2372 struct route_graph_segment *s = NULL; // i am currently on this segment
2373 struct route_segment_data *data2 = NULL;
2374 // s = route_graph_get_segment(rg, sd2, s);
2375 // struct route_graph_segment *start_next2;
2376 struct route_graph_segment *start_3; // other segments that i could drive on next (but not on route)
2377 struct route_graph_segment *drive_here; // i should drive on this segment next
2378 struct route_graph_segment *next_route_seg; // route segment one should drive next after next
2379 struct route_graph_point *rgp_next; // next route graph point the we should drive next after next
2380 int street_dir = 1;
2381
2382
2383 if ((s = route_graph_get_segment(rg, sd2, s)))
2384 {
2385
2386 // if we are on same segment, dont calc it all again!
2387 if (s != s__prev)
2388 {
2389 s__prev = s;
2390
2391 #ifdef HAVE_API_ANDROID
2392 // length of this segment
2393 char *seg_len = g_strdup_printf("%d", s->data.len);
2394 android_send_generic_text(13, seg_len);
2395 g_free(seg_len);
2396 #endif
2397
2398 // struct route_graph_segment *cur; unused ??
2399 struct route_graph_point *rgp;
2400 if (tr->street_direction == -1)
2401 {
2402 //dbg(0, "RR:03.5:\n");
2403 rgp = s->start; //jandegr : first next point
2404 next_route_seg = s->start_from_seg; //jandegr : first next segment
2405 street_dir = -1;
2406 }
2407 else // tr->street_direction == 1 or 0
2408 {
2409 //dbg(0, "RR:03.6:\n");
2410 rgp = s->end; //jandegr : first next point
2411 next_route_seg = s->end_from_seg; //jandegr : first next segment
2412 street_dir = 1;
2413 }
2414
2415
2416 //dbg(0, "RR:04.0:\n");
2417 struct coord_geo coord_geo54;
2418
2419 #ifdef NAVIT_ANGLE_LIST_DEBUG_PRINT_2
2420 route_clear_sharp_turn_list();
2421 #endif
2422
2423
2424 tracking_calc_and_send_possbile_turn_info(rgp, s, street_dir, 0);
2425
2426 // save the next segment data --------------
2427 // if (rgp->seg) jandegr
2428 if (next_route_seg && rgp)
2429 {
2430 // simplify this back later, done for clarity temporary
2431 struct route_graph_segment *second_next_route_seg;
2432
2433 // next_route_seg = rgp->seg; jandegr
2434 //dbg(0, "RR:04.0a:%p\n", next_route_seg);
2435 if (next_route_seg->start == rgp)
2436 {
2437 rgp_next = next_route_seg->end;
2438 street_dir_next = 1;
2439 second_next_route_seg = next_route_seg->start_from_seg;
2440 //dbg(0, "RR:04.2:%p %p %p\n", rgp, next_route_seg->start, next_route_seg->end);
2441 }
2442 else
2443 {
2444 rgp_next = next_route_seg->start;
2445 street_dir_next = -1;
2446 second_next_route_seg = next_route_seg->end_from_seg;
2447 //dbg(0, "RR:04.3:%p %p %p\n", rgp, next_route_seg->start, next_route_seg->end);
2448 }
2449 //dbg(0, "RR:04.4:%p\n", rgp_next);
2450
2451 tracking_calc_and_send_possbile_turn_info(rgp_next, next_route_seg, street_dir_next, 1);
2452
2453 next_lanes_id_hi = next_route_seg->data.item.id_hi;
2454 next_lanes_id_lo = next_route_seg->data.item.id_lo;
2455 next_lanes_id_map = next_route_seg->data.item.map;
2456
2457 next_lanes_id_hi__prev = next_lanes_id_hi;
2458 next_lanes_id_lo__prev = next_lanes_id_lo;
2459 next_lanes_id_map__prev = next_lanes_id_map;
2460 street_dir_next__prev = street_dir_next;
2461
2462 // --- and also the next next seg ----
2463 // --- and also the next next seg ----
2464 // if (rgp_next->seg) jandegr
2465 if (second_next_route_seg)
2466 {
2467 // next_route_seg = rgp_next->seg;
2468 //dbg(0, "RR:04.0a:%p\n", next_route_seg);
2469 if (second_next_route_seg->start == rgp_next)
2470 {
2471 rgp_next = second_next_route_seg->end;
2472 street_dir_next = 1;
2473 //dbg(0, "RR:04.2:%p %p %p\n", rgp_next, next_route_seg->start, next_route_seg->end);
2474 }
2475 else
2476 {
2477 rgp_next = second_next_route_seg->start;
2478 street_dir_next = -1;
2479 //dbg(0, "RR:04.3:%p %p %p\n", rgp_next, next_route_seg->start, next_route_seg->end);
2480 }
2481 //dbg(0, "RR:04.4:%p\n", rgp_next);
2482
2483 tracking_calc_and_send_possbile_turn_info(rgp_next, second_next_route_seg, street_dir_next, 2);
2484 street_dir_next__prev = street_dir_next;
2485
2486 }
2487 else
2488 {
2489 #ifdef HAVE_API_ANDROID
2490 android_send_generic_text((4 + 2), "");
2491 #endif
2492 }
2493 // --- and also the next next seg ----
2494 // --- and also the next next seg ----
2495
2496
2497 }
2498 else
2499 {
2500 #ifdef HAVE_API_ANDROID
2501 android_send_generic_text((4 + 1), "");
2502 #endif
2503
2504 next_lanes_id_hi = 0;
2505 next_lanes_id_lo = 0;
2506 next_lanes_id_map = NULL;
2507
2508 }
2509 // save the next segment data --------------
2510
2511 }
2512 else
2513 {
2514 next_lanes_id_hi = next_lanes_id_hi__prev;
2515 next_lanes_id_lo = next_lanes_id_lo__prev;
2516 next_lanes_id_map = next_lanes_id_map__prev;
2517 street_dir_next = street_dir_next__prev;
2518
2519 same_segment = 1;
2520 }
2521 }
2522 }
2523 }
2524 #endif
2525
2526
2527 // -------------- all way allowed to drive next ----------------------
2528 // -------------- all way allowed to drive next ----------------------
2529
2530 #ifdef NAVIT_CALC_LANES
2531 if (same_segment == 0)
2532 {
2533 struct map_rect *mr_lanes;
2534 mr_lanes = map_rect_new(tr->curr_item_mark.map, NULL);
2535
2536 if (mr_lanes)
2537 {
2538 tracking_send_lanes_info(mr_lanes, tr->curr_item_mark.id_hi, tr->curr_item_mark.id_lo, tr->street_direction, 0);
2539
2540
2541 if (next_lanes_id_map != NULL)
2542 {
2543
2544 if (next_lanes_id_map != tr->curr_item_mark.map)
2545 {
2546 map_rect_destroy(mr_lanes);
2547 mr_lanes = map_rect_new(next_lanes_id_map, NULL);
2548 if (mr_lanes)
2549 {
2550 tracking_send_lanes_info(mr_lanes, next_lanes_id_hi, next_lanes_id_lo, street_dir_next, 1);
2551 }
2552 }
2553 else
2554 {
2555 tracking_send_lanes_info(mr_lanes, next_lanes_id_hi, next_lanes_id_lo, street_dir_next, 1);
2556 }
2557 }
2558
2559 map_rect_destroy(mr_lanes);
2560 }
2561 }
2562
2563 #endif
2564
2565
2566 }
2567
2568
2569 #endif
2570 #endif
2571
2572
2573 if (tr->curr_line && (tr->curr_line->street->flags & NAVIT_AF_UNDERGROUND))
2574 {
2575
2576 // the current position is underground (= tunnel)
2577 #ifdef HAVE_API_ANDROID
2578 if (global_pos_is_underground == 0)
2579 {
2580 android_return_generic_int(7, 1);
2581 }
2582 #endif
2583 global_pos_is_underground = 1;
2584
2585 //dbg(0,"NAVIT_AF_UNDERGROUND 1");
2586 //if (tr->no_gps)
2587 //{
2588 // tr->tunnel = 1;
2589 //}
2590 }
2591 else
2592 {
2593 #ifdef HAVE_API_ANDROID
2594 if (global_pos_is_underground == 1)
2595 {
2596 android_return_generic_int(7, 0);
2597 }
2598 #endif
2599 global_pos_is_underground = 0;
2600 }
2601 //else if (tr->tunnel)
2602 //{
2603 // //dbg(0,"NAVIT_AF_UNDERGROUND 2");
2604 // tr->speed = 0;
2605 //}
2606
2607
2608 //dbg(0,"curr_IN :98 0x%x,0x%x\n", tr->curr_in.x, tr->curr_in.y);
2609 //dbg(0,"curr_out:99 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
2610
2611 if (too_slow_or_little_movement == 1)
2612 {
2613 tr->valid = attr_position_valid_static;
2614 tr->speed = 0;
2615
2616 // dbg(0, "TR_POSANG:001:too_slow_or_little_movement\n");
2617 }
2618
2619 dbg(0, "TR_POSANG:012:->angle=%d dir=%f str.dir=%d\n", tr->curr_angle, tr->direction, tr->street_direction);
2620
2621 #ifdef NAVIT_ROUTING_DEBUG_PRINT
2622 dbg(0, "ROUTExxPOSxx:003 coord_geo_valid=%d\n", tr->coord_geo_valid);
2623 dbg(0, "ROUTExxPOSxx:routetracking_update: leave x=%d y=%d in.x=%d in.y=%d\n", tr->curr_out.x, tr->curr_out.y, tr->curr_in.x, tr->curr_in.y);
2624 #endif
2625 }
2626
2627 static int tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial)
2628 {
2629 switch (attr->type)
2630 {
2631 case attr_angle_pref:
2632 tr->angle_pref = attr->u.num;
2633 return 1;
2634 case attr_connected_pref:
2635 tr->connected_pref = attr->u.num;
2636 dbg(0, "connected_pref=%d\n", attr->u.num);
2637 return 1;
2638 case attr_nostop_pref:
2639 tr->nostop_pref = attr->u.num;
2640 return 1;
2641 case attr_offroad_limit_pref:
2642 tr->offroad_limit_pref = attr->u.num;
2643 return 1;
2644 case attr_route_pref:
2645 tr->route_pref = attr->u.num;
2646 return 1;
2647 case attr_overspeed_pref:
2648 tr->overspeed_pref = attr->u.num;
2649 return 1;
2650 case attr_overspeed_percent_pref:
2651 tr->overspeed_percent_pref = attr->u.num;
2652 return 1;
2653 case attr_tunnel_extrapolation:
2654 tr->tunnel_extrapolation = attr->u.num;
2655 return 1;
2656 default:
2657 return 0;
2658 }
2659 }
2660
2661 int tracking_set_attr(struct tracking *tr, struct attr *attr)
2662 {
2663 return tracking_set_attr_do(tr, attr, 0);
2664 }
2665
2666 struct tracking *
2667 tracking_new(struct attr *parent, struct attr **attrs)
2668 {
2669 struct tracking *this=g_new0(struct tracking, 1);
2670 struct attr hist_size;
2671 this->angle_pref = 4; // not used anymore !!!
2672 this->connected_pref = 10;
2673 this->nostop_pref = 4;
2674 this->offroad_limit_pref = 98000; // was 5000 originally! // find nearst road, even when it is very far away (maybe in some desert, or somewhere with only very few roads!)
2675 this->route_pref = 300;
2676
2677 if (!attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL))
2678 {
2679 hist_size.u.num = 0;
2680 }
2681
2682 if (attrs)
2683 {
2684 for (; *attrs; attrs++)
2685 {
2686 tracking_set_attr_do(this, *attrs, 1);
2687 }
2688 }
2689
2690 tracking_init_cdf(&this->cdf, hist_size.u.num);
2691
2692 return this;
2693 }
2694
2695 void tracking_set_mapset(struct tracking *this, struct mapset *ms)
2696 {
2697 this->ms = ms;
2698 }
2699
2700 void tracking_set_route(struct tracking *this, struct route *rt)
2701 {
2702 this->rt = rt;
2703 }
2704
2705 void tracking_destroy(struct tracking *tr)
2706 {
2707 if (tr->attr)
2708 attr_free(tr->attr);
2709
2710 tracking_flush(tr);
2711 g_free(tr);
2712 }
2713
2714 struct map *
2715 tracking_get_map(struct tracking *this_)
2716 {
2717 struct attr *attrs[5];
2718 struct attr type, navigation, data, description;
2719 type.type = attr_type;
2720 type.u.str = "tracking";
2721 navigation.type = attr_trackingo;
2722 navigation.u.tracking = this_;
2723 data.type = attr_data;
2724 data.u.str = "";
2725 description.type = attr_description;
2726 description.u.str = "Tracking";
2727
2728 attrs[0] = &type;
2729 attrs[1] = &navigation;
2730 attrs[2] = &data;
2731 attrs[3] = &description;
2732 attrs[4] = NULL;
2733
2734 if (!this_->map)
2735 {
2736 this_->map = map_new(NULL, attrs);
2737 }
2738
2739 return this_->map;
2740 }
2741
2742 struct map_priv
2743 {
2744 struct tracking *tracking;
2745 };
2746
2747 struct map_rect_priv
2748 {
2749 struct tracking *tracking;
2750 struct item item;
2751 struct tracking_line *curr, *next;
2752 int coord;
2753 enum attr_type attr_next;
2754 int ccount;
2755 int debug_idx;
2756 char *str;
2757 };
2758
2759 static int tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
2760 {
2761 struct map_rect_priv *this = priv_data;
2762 enum projection pro;
2763 int ret = 0;
2764 //dbg(1, "enter\n");
2765 while (this->ccount < 2 && count > 0)
2766 {
2767 pro = map_projection(this->curr->street->item.map);
2768
2769 if (projection_mg != pro)
2770 {
2771 transform_from_to(&this->curr->street->c[this->ccount + this->coord], pro, c, projection_mg);
2772 }
2773 else
2774 {
2775 *c = this->curr->street->c[this->ccount + this->coord];
2776 }
2777
2778 //dbg(1, "coord %d 0x%x,0x%x\n", this->ccount, c->x, c->y);
2779 this->ccount++;
2780 ret++;
2781 c++;
2782 count--;
2783 }
2784 return ret;
2785 }
2786
2787 static int tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
2788 {
2789 struct map_rect_priv *this_ = priv_data;
2790 struct coord lpnt, *c;
2791 struct tracking *tr = this_->tracking;
2792 int value;
2793 attr->type = attr_type;
2794
2795 if (this_->str)
2796 {
2797 g_free(this_->str);
2798 this_->str = NULL;
2799 }
2800
2801 switch (attr_type)
2802 {
2803 case attr_debug:
2804 switch (this_->debug_idx)
2805 {
2806 #if 0
2807 case 0:
2808 this_->debug_idx++;
2809 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);
2810 return 1;
2811 case 1:
2812 this_->debug_idx++;
2813 c = &this_->curr->street->c[this_->coord];
2814 value = tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 1);
2815 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);
2816 return 1;
2817 case 2:
2818 this_->debug_idx++;
2819 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));
2820 return 1;
2821 case 3:
2822 this_->debug_idx++;
2823 this_->str = attr->u.str = g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 4));
2824 return 1;
2825 case 4:
2826 this_->debug_idx++;
2827 this_->str = attr->u.str = g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 8));
2828 return 1;
2829 case 5:
2830 this_->debug_idx++;
2831 this_->str = attr->u.str = g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 16));
2832 return 1;
2833 case 6:
2834 this_->debug_idx++;
2835 this_->str = attr->u.str = g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX / 2, 32));
2836 return 1;
2837 case 7:
2838 this_->debug_idx++;
2839 this_->str = attr->u.str = g_strdup_printf("line %p", this_->curr);
2840 return 1;
2841 #endif
2842 default:
2843 this_->attr_next = attr_none;
2844 return 0;
2845 }
2846 case attr_any:
2847 while (this_->attr_next != attr_none)
2848 {
2849 if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
2850 return 1;
2851 }
2852 return 0;
2853 default:
2854 attr->type = attr_none;
2855 return 0;
2856 }
2857 }
2858
2859 static struct item_methods tracking_map_item_methods = { NULL, tracking_map_item_coord_get, NULL, tracking_map_item_attr_get, };
2860
2861 static void tracking_map_destroy(struct map_priv *priv)
2862 {
2863 g_free(priv);
2864 }
2865
2866 static void tracking_map_rect_init(struct map_rect_priv *priv)
2867 {
2868 priv->next = priv->tracking->lines;
2869 priv->curr = NULL;
2870 priv->coord = 0;
2871 priv->item.id_lo = 0;
2872 priv->item.id_hi = 0;
2873 }
2874
2875 static struct map_rect_priv *
2876 tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
2877 {
2878 struct tracking *tracking = priv->tracking;
2879 struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
2880 ret->tracking = tracking;
2881 tracking_map_rect_init(ret);
2882 ret->item.meth = &tracking_map_item_methods;
2883 ret->item.priv_data = ret;
2884 ret->item.type = type_tracking_100;
2885 return ret;
2886 }
2887
2888 static void tracking_map_rect_destroy(struct map_rect_priv *priv)
2889 {
2890 g_free(priv);
2891 }
2892
2893 static struct item *
2894 tracking_map_get_item(struct map_rect_priv *priv)
2895 {
2896 struct item *ret = &priv->item;
2897 int value;
2898 struct coord lpnt;
2899
2900 if (!priv->next)
2901 {
2902 return NULL;
2903 }
2904
2905 if (!priv->curr || priv->coord + 2 >= priv->curr->street->count)
2906 {
2907 priv->curr = priv->next;
2908 priv->next = priv->curr->next;
2909 priv->coord = 0;
2910 priv->item.id_lo = 0;
2911 priv->item.id_hi++;
2912 }
2913 else
2914 {
2915 priv->coord++;
2916 priv->item.id_lo++;
2917 }
2918
2919 value = tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX / 2, -1, NULL);
2920
2921 if (value < 64)
2922 priv->item.type = type_tracking_100;
2923 else if (value < 128)
2924 priv->item.type = type_tracking_90;
2925 else if (value < 256)
2926 priv->item.type = type_tracking_80;
2927 else if (value < 512)
2928 priv->item.type = type_tracking_70;
2929 else if (value < 1024)
2930 priv->item.type = type_tracking_60;
2931 else if (value < 2048)
2932 priv->item.type = type_tracking_50;
2933 else if (value < 4096)
2934 priv->item.type = type_tracking_40;
2935 else if (value < 8192)
2936 priv->item.type = type_tracking_30;
2937 else if (value < 16384)
2938 priv->item.type = type_tracking_20;
2939 else if (value < 32768)
2940 priv->item.type = type_tracking_10;
2941 else
2942 priv->item.type = type_tracking_0;
2943
2944
2945 //dbg(1, "item %d %d points\n", priv->coord, priv->curr->street->count);
2946 priv->ccount = 0;
2947 priv->attr_next = attr_debug;
2948 priv->debug_idx = 0;
2949
2950 return ret;
2951 }
2952
2953 static struct item *
2954 tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
2955 {
2956 struct item *ret;
2957 tracking_map_rect_init(priv);
2958 while ((ret = tracking_map_get_item(priv)))
2959 {
2960 if (ret->id_hi == id_hi && ret->id_lo == id_lo)
2961 {
2962 return ret;
2963 }
2964 }
2965 return NULL;
2966 }
2967
2968 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, };
2969
2970 struct map_priv *
2971 tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl)
2972 {
2973 struct map_priv *ret;
2974 struct attr *tracking_attr;
2975
2976 tracking_attr = attr_search(attrs, NULL, attr_trackingo);
2977
2978 if (!tracking_attr)
2979 {
2980 return NULL;
2981 }
2982
2983 ret=g_new0(struct map_priv, 1);
2984
2985 *meth = tracking_map_meth;
2986 ret->tracking = tracking_attr->u.tracking;
2987
2988 return ret;
2989 }
2990
2991 void tracking_init(void)
2992 {
2993 #ifdef PLUGSSS
2994 plugin_register_map_type("tracking", tracking_map_new);
2995 #endif
2996 }
2997

   
Visit the ZANavi Wiki