2 * Navit, a modular navigation system.
3 * Copyright (C) 2005-2008 Navit Team
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.
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.
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.
28 #include "transform.h"
31 #include "projection.h"
35 #include "vehicleprofile.h"
41 struct street_data *street;
42 struct tracking_line *next;
48 * @brief Conatins a list of previous speeds
50 * This structure is used to hold a list of previously reported
51 * speeds. This data is used by the CDF.
54 struct cdf_speed *next;
60 * @brief Contains data for the CDF
62 * This structure holds all data needed by the
63 * cumulative displacement filter.
71 struct cdf_speed *speed_hist;
72 struct pcoord *pos_hist;
75 struct pcoord last_out;
83 struct vehicle *vehicle;
84 struct vehicleprofile *vehicleprofile;
85 struct coord last_updated;
86 struct tracking_line *lines;
87 struct tracking_line *curr_line;
89 struct coord curr[2], curr_in, curr_out;
91 struct coord last[2], last_in, last_out;
99 struct coord_geo coord_geo;
101 int street_direction;
106 int connected_pref=10;
108 int offroad_limit_pref=5000;
113 tracking_init_cdf(struct cdf_data *cdf, int hist_size)
115 cdf->extrapolating = 0;
119 cdf->hist_size = hist_size;
121 cdf->pos_hist = g_new0(struct pcoord, hist_size);
122 cdf->dir_hist = g_new0(int, hist_size);
125 // Variables for finetuning the CDF
127 // Minimum average speed
128 #define CDF_MINAVG 1.f
129 // Maximum average speed
130 #define CDF_MAXAVG 6.f // only ~ 20 km/h
131 // We need a low value here because otherwise we would extrapolate whenever we are not accelerating
133 // Mininum distance (square of it..), below which we ignore gps updates
134 #define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
138 tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime)
140 struct cdf_speed *speed,*sc,*sl;
144 if (cdf->hist_size == 0) {
151 speed = g_new0(struct cdf_speed, 1);
152 speed->speed = cur_speed;
153 speed->time = fixtime;
155 speed->next = cdf->speed_hist;
156 cdf->speed_hist = speed;
162 while (sc && ((fixtime - speed->time) < 4)) { // FIXME static maxtime
164 speed_avg += sc->speed;
169 speed_avg /= (double)speed_num;
181 if (speed_avg < CDF_MINAVG) {
182 speed_avg = CDF_MINAVG;
183 } else if (speed_avg > CDF_MAXAVG) {
184 speed_avg = CDF_MAXAVG;
188 if (cur_speed >= speed_avg) {
189 if (cdf->extrapolating) {
191 cdf->extrapolating = 0;
195 if (cdf->first_pos < 0) {
196 cdf->first_pos = cdf->hist_size - 1;
199 if (cdf->poscount < cdf->hist_size) {
203 cdf->pos_hist[cdf->first_pos] = *pin;
204 cdf->dir_hist[cdf->first_pos] = dirin;
208 } else if (cdf->poscount > 0) {
210 double mx,my; // Average position's x and y values
211 double sx,sy; // Support vector
212 double dx,dy; // Difference between average and current position
213 double len; // Length of support vector
219 for (i = 0; i < cdf->poscount; i++) {
220 mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
221 my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;
225 sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x;
226 sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y;
231 if (cdf->poscount > 1) {
232 // Normalize the support vector
233 len = sqrt(sx * sx + sy * sy);
237 // Calculate the new direction
238 *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180);
240 // If we only have one position, we can't use differences of positions, but we have to use the reported
241 // direction of that position
242 sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
243 sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
244 *dirout = cdf->dir_hist[cdf->first_pos];
250 dist = dx * sx + dy * sy;
252 if (cdf->extrapolating && (dist < cdf->last_dist)) {
253 dist = cdf->last_dist;
256 cdf->last_dist = dist;
257 cdf->extrapolating = 1;
259 pout->x = (int)rint(mx + sx * dist);
260 pout->y = (int)rint(my + sy * dist);
261 pout->pro = pin->pro;
264 // We should extrapolate, but don't have an old position available
269 if (cdf->available) {
272 dx = pout->x - cdf->last_out.x;
273 dy = pout->y - cdf->last_out.y;
275 if ((dx*dx + dy*dy) < CDF_MINDIST) {
276 *pout = cdf->last_out;
277 *dirout = cdf->last_dir;
281 cdf->last_out = *pout;
282 cdf->last_dir = *dirout;
289 tracking_get_angle(struct tracking *tr)
291 return tr->curr_angle;
295 tracking_get_pos(struct tracking *tr)
297 return &tr->curr_out;
301 tracking_get_street_direction(struct tracking *tr)
303 return tr->street_direction;
307 tracking_get_segment_pos(struct tracking *tr)
313 tracking_get_street_data(struct tracking *tr)
316 return tr->curr_line->street;
321 tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter)
326 dbg(1,"enter %s\n",attr_to_name(type));
328 attr_free(_this->attr);
332 case attr_position_valid:
333 attr->u.num=_this->valid;
335 case attr_position_direction:
336 attr->u.numd=&_this->direction;
338 case attr_position_speed:
339 attr->u.numd=&_this->speed;
341 case attr_position_coord_geo:
342 if (!_this->coord_geo_valid) {
344 c.x=_this->curr_out.x;
345 c.y=_this->curr_out.y;
346 transform_to_geo(_this->pro, &c, &_this->coord_geo);
347 _this->coord_geo_valid=1;
349 attr->u.coord_geo=&_this->coord_geo;
352 if (! _this->curr_line || ! _this->curr_line->street)
354 item=&_this->curr_line->street->item;
355 mr=map_rect_new(item->map,NULL);
356 item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
357 if (item_attr_get(item, type, attr)) {
358 _this->attr=attr_dup(attr);
362 map_rect_destroy(mr);
368 tracking_get_current_item(struct tracking *_this)
370 if (! _this->curr_line || ! _this->curr_line->street)
372 return &_this->curr_line->street->item;
376 tracking_get_current_flags(struct tracking *_this)
378 if (! _this->curr_line || ! _this->curr_line->street)
380 return &_this->curr_line->street->flags;
384 tracking_get_angles(struct tracking_line *tl)
387 struct street_data *sd=tl->street;
388 for (i = 0 ; i < sd->count-1 ; i++)
389 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
393 street_data_within_selection(struct street_data *sd, struct map_selection *sel)
396 struct map_selection *curr;
403 for (i = 1 ; i < sd->count ; i++) {
404 if (r.lu.x > sd->c[i].x)
406 if (r.rl.x < sd->c[i].x)
408 if (r.rl.y > sd->c[i].y)
410 if (r.lu.y < sd->c[i].y)
415 struct coord_rect *sr=&curr->u.c_rect;
416 if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x &&
417 r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
426 tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro)
429 struct map_selection *sel;
430 struct mapset_handle *h;
434 struct street_data *street;
435 struct tracking_line *tl;
440 h=mapset_open(tr->ms);
441 while ((m=mapset_next(h,1))) {
444 if (map_projection(m) != pro) {
445 transform_to_geo(pro, &cc, &g);
446 transform_from_geo(map_projection(m), &g, &cc);
448 sel = route_rect(18, &cc, &cc, 0, max_dist);
449 mr=map_rect_new(m, sel);
452 while ((item=map_rect_get_item(mr))) {
453 if (item_get_default_flags(item->type)) {
454 street=street_get_data(item);
455 if (street_data_within_selection(street, sel)) {
456 tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
458 tracking_get_angles(tl);
462 street_data_free(street);
465 map_selection_destroy(sel);
466 map_rect_destroy(mr);
474 tracking_free_lines(struct tracking *tr)
476 struct tracking_line *tl=tr->lines,*next;
477 dbg(1,"enter(tr=%p)\n", tr);
481 street_data_free(tl->street);
486 tr->curr_line = NULL;
490 tracking_angle_diff(int a1, int a2, int full)
492 int ret=(a1-a2)%full;
501 tracking_angle_abs_diff(int a1, int a2, int full)
503 int ret=tracking_angle_diff(a1, a2, full);
510 tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags)
512 int full=180,ret=360,fwd,rev;
513 struct vehicleprofile *profile=tr->vehicleprofile;
514 fwd=((flags & profile->flags_forward_mask) == profile->flags);
515 rev=((flags & profile->flags_reverse_mask) == profile->flags);
520 street_angle=(street_angle+180)%360;
522 ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
528 tracking_is_connected(struct coord *c1, struct coord *c2)
530 if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
532 if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
534 if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
536 if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
538 return connected_pref;
542 tracking_is_no_stop(struct coord *c1, struct coord *c2)
544 if (c1->x == c2->x && c1->y == c2->y)
550 tracking_is_on_route(struct route *rt, struct item *item)
554 if (route_contains(rt, item))
560 tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
563 struct street_data *sd=t->street;
564 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);
566 struct coord c1, c2, cp;
567 c1.x = sd->c[offset].x;
568 c1.y = sd->c[offset].y;
569 c2.x = sd->c[offset+1].x;
570 c2.y = sd->c[offset+1].y;
571 cp.x = tr->curr_in.x;
572 cp.y = tr->curr_in.y;
573 value+=transform_distance_line_sq(&c1, &c2, &cp, lpnt);
578 value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags)*angle_factor>>4;
582 value += tracking_is_connected(tr->last, &sd->c[offset]);
584 value += tracking_is_no_stop(lpnt, &tr->last_out);
588 value += tracking_is_on_route(tr->rt, &sd->item);
594 tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro)
596 struct tracking_line *t;
597 int i,value,min,time;
600 struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr;
601 double speed, direction;
605 tr->vehicleprofile=vehicleprofile;
609 if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
610 valid.u.num=attr_position_valid_valid;
611 if (valid.u.num == attr_position_valid_invalid) {
612 tr->valid=valid.u.num;
615 if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) ||
616 !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) ||
617 !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) ||
618 !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) {
619 dbg(0,"failed to get position data\n");
622 time=iso8601_to_secs(time_attr.u.str);
623 speed=*speed_attr.u.numd;
624 direction=*direction_attr.u.numd;
625 tr->valid=attr_position_valid_valid;
626 transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
627 if ((speed < 3 && transform_distance(pro, &tr->last_in, &tr->curr_in) < 10 )) {
628 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);
629 tr->valid=attr_position_valid_static;
633 if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) {
636 if (time-tr->time == 1) {
637 dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed);
638 espeed=speed+(speed-tr->speed)*lag.u.num/10;
639 dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360));
640 edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10;
642 dbg(1,"no speed and direction extrapolation\n");
644 edirection=direction;
646 dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
647 dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
648 transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in);
649 dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
655 tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime);
657 tr->curr_angle=tr->direction=direction;
659 tr->last_in=tr->curr_in;
660 tr->last_out=tr->curr_out;
661 tr->last[0]=tr->curr[0];
662 tr->last[1]=tr->curr[1];
663 if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) {
665 tracking_free_lines(tr);
666 tracking_doupdate_lines(tr, &tr->curr_in, pro);
667 tr->last_updated=tr->curr_in;
668 dbg(1,"update end\n");
675 struct street_data *sd=t->street;
676 for (i = 0; i < sd->count-1 ; i++) {
677 value=tracking_value(tr,t,i,&lpnt,min,-1);
679 int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
682 tr->curr[0]=sd->c[i];
683 tr->curr[1]=sd->c[i+1];
684 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
685 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt),
686 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*angle_factor,
687 tracking_is_connected(tr->last, &sd->c[i]) ? connected_pref : 0,
688 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0,
691 tr->curr_out.x=lpnt.x;
692 tr->curr_out.y=lpnt.y;
693 tr->coord_geo_valid=0;
694 if (angle_delta < 70)
695 tr->street_direction=1;
696 else if (angle_delta > 110)
697 tr->street_direction=-1;
699 tr->street_direction=0;
705 dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
706 if (!tr->curr_line || min > offroad_limit_pref) {
707 tr->curr_out=tr->curr_in;
708 tr->coord_geo_valid=0;
710 dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
714 tracking_new(struct attr *parent, struct attr **attrs)
716 struct tracking *this=g_new0(struct tracking, 1);
717 struct attr hist_size;
719 if (! attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) {
723 tracking_init_cdf(&this->cdf, hist_size.u.num);
729 tracking_set_mapset(struct tracking *this, struct mapset *ms)
735 tracking_set_route(struct tracking *this, struct route *rt)
741 tracking_destroy(struct tracking *tr)
743 tracking_free_lines(tr);
748 tracking_get_map(struct tracking *this_)
751 this_->map=map_new(NULL, (struct attr*[]){
752 &(struct attr){attr_type,{"tracking"}},
753 &(struct attr){attr_trackingo,.u.tracking=this_},
754 &(struct attr){attr_data,{""}},
755 &(struct attr){attr_description,{"Tracking"}},
762 struct tracking *tracking;
765 struct map_rect_priv {
766 struct tracking *tracking;
768 struct tracking_line *curr,*next;
770 enum attr_type attr_next;
777 tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
779 struct map_rect_priv *this=priv_data;
783 while (this->ccount < 2 && count > 0) {
784 pro = map_projection(this->curr->street->item.map);
785 if (projection_mg != pro) {
786 transform_from_to(&this->curr->street->c[this->ccount+this->coord],
790 *c=this->curr->street->c[this->ccount+this->coord];
791 dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y);
801 tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
803 struct map_rect_priv *this_=priv_data;
804 attr->type=attr_type;
805 struct coord lpnt,*c;
806 struct tracking *tr=this_->tracking;
816 switch(this_->debug_idx) {
819 this_->str=attr->u.str=g_strdup_printf("overall: %d",tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, -1));
823 c=&this_->curr->street->c[this_->coord];
824 value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1);
825 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",
826 tr->curr_in.x, tr->curr_in.y,
827 c[0].x, c[0].y, c[1].x, c[1].y,
828 lpnt.x, lpnt.y, value);
832 this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d",
833 tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
834 tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2));
838 this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 4));
842 this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 8));
846 this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 16));
850 this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr);
853 this_->attr_next=attr_none;
857 while (this_->attr_next != attr_none) {
858 if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
863 attr->type=attr_none;
868 static struct item_methods tracking_map_item_methods = {
870 tracking_map_item_coord_get,
872 tracking_map_item_attr_get,
877 tracking_map_destroy(struct map_priv *priv)
883 tracking_map_rect_init(struct map_rect_priv *priv)
885 priv->next=priv->tracking->lines;
892 static struct map_rect_priv *
893 tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
895 struct tracking *tracking=priv->tracking;
896 struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
897 ret->tracking=tracking;
898 tracking_map_rect_init(ret);
899 ret->item.meth=&tracking_map_item_methods;
900 ret->item.priv_data=ret;
901 ret->item.type=type_tracking_100;
906 tracking_map_rect_destroy(struct map_rect_priv *priv)
912 tracking_map_get_item(struct map_rect_priv *priv)
914 struct item *ret=&priv->item;
920 if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) {
921 priv->curr=priv->next;
922 priv->next=priv->curr->next;
930 value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1);
932 priv->item.type=type_tracking_100;
933 else if (value < 128)
934 priv->item.type=type_tracking_90;
935 else if (value < 256)
936 priv->item.type=type_tracking_80;
937 else if (value < 512)
938 priv->item.type=type_tracking_70;
939 else if (value < 1024)
940 priv->item.type=type_tracking_60;
941 else if (value < 2048)
942 priv->item.type=type_tracking_50;
943 else if (value < 4096)
944 priv->item.type=type_tracking_40;
945 else if (value < 8192)
946 priv->item.type=type_tracking_30;
947 else if (value < 16384)
948 priv->item.type=type_tracking_20;
949 else if (value < 32768)
950 priv->item.type=type_tracking_10;
952 priv->item.type=type_tracking_0;
953 dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count);
955 priv->attr_next=attr_debug;
961 tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
964 tracking_map_rect_init(priv);
965 while ((ret=tracking_map_get_item(priv))) {
966 if (ret->id_hi == id_hi && ret->id_lo == id_lo)
972 static struct map_methods tracking_map_meth = {
975 tracking_map_destroy,
976 tracking_map_rect_new,
977 tracking_map_rect_destroy,
978 tracking_map_get_item,
979 tracking_map_get_item_byid,
985 static struct map_priv *
986 tracking_map_new(struct map_methods *meth, struct attr **attrs)
988 struct map_priv *ret;
989 struct attr *tracking_attr;
991 tracking_attr=attr_search(attrs, NULL, attr_trackingo);
994 ret=g_new0(struct map_priv, 1);
995 *meth=tracking_map_meth;
996 ret->tracking=tracking_attr->u.tracking;
1005 plugin_register_map_type("tracking", tracking_map_new);