Fix:Core:Made some performance critical parts a bit faster
[navit-package] / navit / track.c
1 /**
2  * Navit, a modular navigation system.
3  * Copyright (C) 2005-2008 Navit Team
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * version 2 as published by the Free Software Foundation.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the
16  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
17  * Boston, MA  02110-1301, USA.
18  */
19
20 #include <glib.h>
21 #include <string.h>
22 #include "item.h"
23 #include "attr.h"
24 #include "track.h"
25 #include "debug.h"
26 #include "transform.h"
27 #include "coord.h"
28 #include "route.h"
29 #include "projection.h"
30 #include "map.h"
31 #include "mapset.h"
32 #include "plugin.h"
33
34 struct tracking_line
35 {
36         struct street_data *street;
37 #if 0
38         long segid;
39         int linenum;
40         struct coord c[2];
41         struct coord lpnt;
42         int value;
43         int dir;
44 #endif
45         struct tracking_line *next;
46         int angle[0];
47 };
48
49
50 struct tracking {
51         struct mapset *ms;
52         struct route *rt;
53         struct map *map;
54 #if 0
55         struct transformation t;
56 #endif
57         struct pcoord last_updated;
58         struct tracking_line *lines;
59 #if 0
60         struct tracking_line **last_ptr;
61 #endif
62         struct tracking_line *curr_line;
63         int pos;
64         struct coord curr[2];
65         struct pcoord curr_in, curr_out;
66         int curr_angle;
67         struct coord last[2];
68         struct pcoord last_in, last_out;
69 };
70
71
72 int angle_factor=10;
73 int connected_pref=10;
74 int nostop_pref=10;
75 int offroad_limit_pref=5000;
76 int route_pref=300;
77
78
79 struct pcoord *
80 tracking_get_pos(struct tracking *tr)
81 {
82         return &tr->curr_out;
83 }
84
85 int
86 tracking_get_segment_pos(struct tracking *tr)
87 {
88         return tr->pos;
89 }
90
91 struct street_data *
92 tracking_get_street_data(struct tracking *tr)
93 {
94         if (tr->curr_line)
95                 return tr->curr_line->street;
96         return NULL;
97 }
98
99 int
100 tracking_get_current_attr(struct tracking *_this, enum attr_type type, struct attr *attr)
101 {
102         struct item *item;
103         struct map_rect *mr;
104         int result=0;
105         if (! _this->curr_line || ! _this->curr_line->street)
106                 return 0;
107         item=&_this->curr_line->street->item;
108         mr=map_rect_new(item->map,NULL);
109         item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
110         if (item_attr_get(item, type, attr))
111                 result=1;
112         map_rect_destroy(mr);
113         return result;
114 }
115
116 struct item *
117 tracking_get_current_item(struct tracking *_this)
118 {
119         if (! _this->curr_line || ! _this->curr_line->street)
120                 return NULL;
121         return &_this->curr_line->street->item;
122 }
123
124 static void
125 tracking_get_angles(struct tracking_line *tl)
126 {
127         int i;
128         struct street_data *sd=tl->street;
129         for (i = 0 ; i < sd->count-1 ; i++) 
130                 tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
131 }
132
133 static int
134 street_data_within_selection(struct street_data *sd, struct map_selection *sel)
135 {
136         struct coord_rect r;
137         struct map_selection *curr;
138         int i;
139
140         if (!sel)
141                 return 1;
142         r.lu=sd->c[0];
143         r.rl=sd->c[0];
144         for (i = 1 ; i < sd->count ; i++) {
145                 if (r.lu.x > sd->c[i].x)
146                         r.lu.x=sd->c[i].x;
147                 if (r.rl.x < sd->c[i].x)
148                         r.rl.x=sd->c[i].x;
149                 if (r.rl.y > sd->c[i].y)
150                         r.rl.y=sd->c[i].y;
151                 if (r.lu.y < sd->c[i].y)
152                         r.lu.y=sd->c[i].y;
153         }
154         curr=sel;
155         while (curr) {
156                 struct coord_rect *sr=&curr->u.c_rect;
157                 if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x &&
158                     r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
159                         return 1;
160                 curr=curr->next;
161         }
162         return 0;
163 }
164
165
166 static void
167 tracking_doupdate_lines(struct tracking *tr, struct pcoord *pc)
168 {
169         int max_dist=1000;
170         struct map_selection *sel;
171         struct mapset_handle *h;
172         struct map *m;
173         struct map_rect *mr;
174         struct item *item;
175         struct street_data *street;
176         struct tracking_line *tl;
177         struct coord_geo g;
178         struct coord cc;
179
180         dbg(1,"enter\n");
181         h=mapset_open(tr->ms);
182         while ((m=mapset_next(h,1))) {
183                 cc.x = pc->x;
184                 cc.y = pc->y;
185                 if (map_projection(m) != pc->pro) {
186                         transform_to_geo(pc->pro, &cc, &g);
187                         transform_from_geo(map_projection(m), &g, &cc);
188                 }
189                 sel = route_rect(18, &cc, &cc, 0, max_dist);
190                 mr=map_rect_new(m, sel);
191                 if (!mr)
192                         continue;
193                 while ((item=map_rect_get_item(mr))) {
194                         if (item->type >= type_street_0 && item->type <= type_ferry) {
195                                 street=street_get_data(item);
196                                 if (street_data_within_selection(street, sel)) {
197                                         tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
198                                         tl->street=street;
199                                         tracking_get_angles(tl);
200                                         tl->next=tr->lines;
201                                         tr->lines=tl;
202                                 } else
203                                         street_data_free(street);
204                         }
205                 }
206                 map_selection_destroy(sel);
207                 map_rect_destroy(mr);
208         }
209         mapset_close(h);
210         dbg(1, "exit\n");
211 #if 0
212
213         struct transformation t;
214
215         tr->last_ptr=&tr->lines;
216         transform_setup_source_rect_limit(&t,c,1000);
217         transform_setup_source_rect_limit(&tr->t,c,1000);
218
219
220         profile_timer(NULL);
221         street_get_block(tr->ma,&t,tst_callback,tr);
222         profile_timer("end");
223 #endif
224 }
225
226
227 static void
228 tracking_free_lines(struct tracking *tr)
229 {
230         struct tracking_line *tl=tr->lines,*next;
231         dbg(1,"enter(tr=%p)\n", tr);
232
233         while (tl) {
234                 next=tl->next;
235                 street_data_free(tl->street);
236                 g_free(tl);
237                 tl=next;
238         }
239         tr->lines=NULL;
240 }
241
242 static int
243 tracking_angle_abs_diff(int a1, int a2, int full)
244 {
245         int ret;
246
247         if (a2 > a1)
248                 ret=(a2-a1)%full;
249         else
250                 ret=(a1-a2)%full;
251         if (ret > full/2)
252                 ret=full-ret;
253         return ret;
254 }
255
256 static int
257 tracking_angle_delta(int vehicle_angle, int street_angle, int flags)
258 {
259         int full=180;
260         int ret;
261         if (flags) {
262                 full=360;
263                 if (flags & 2) {
264                         street_angle=(street_angle+180)%360;
265                         if (flags & 1)
266                                 return 360*360;
267                 }
268         }
269         ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
270         
271         return ret*ret;
272 }
273
274 static int
275 tracking_is_connected(struct coord *c1, struct coord *c2)
276 {
277         if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
278                 return 0;
279         if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
280                 return 0;
281         if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
282                 return 0;
283         if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
284                 return 0;
285         return connected_pref;
286 }
287
288 static int
289 tracking_is_no_stop(struct coord *c1, struct pcoord *c2)
290 {
291         if (c1->x == c2->x && c1->y == c2->y)
292                 return nostop_pref;
293         return 0;
294 }
295
296 static int
297 tracking_is_on_route(struct route *rt, struct item *item)
298 {
299         if (! rt)
300                 return 0;
301         if (route_pos_contains(rt, item))
302                 return 0;
303         if (route_contains(rt, item))
304                 return 0;
305         return route_pref;
306 }
307
308 static int
309 tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
310 {
311         int value=0;
312         struct street_data *sd=t->street;
313         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);
314         if (flags & 1) {
315                 struct coord c1, c2, cp;
316                 c1.x = sd->c[offset].x;
317                 c1.y = sd->c[offset].y;
318                 c2.x = sd->c[offset+1].x;
319                 c2.y = sd->c[offset+1].y;
320                 cp.x = tr->curr_in.x;
321                 cp.y = tr->curr_in.y;
322                 value+=transform_distance_line_sq(&c1, &c2, &cp, lpnt);
323         }
324         if (value >= min)
325                 return value;
326         if (flags & 2) 
327                 value += tracking_angle_delta(tr->curr_angle, t->angle[offset], sd->flags)*angle_factor>>4;
328         if (value >= min)
329                 return value;
330         if (flags & 4) 
331                 value += tracking_is_connected(tr->last, &sd->c[offset]);
332         if (flags & 8) 
333                 value += tracking_is_no_stop(lpnt, &tr->last_out);
334         if (value >= min)
335                 return value;
336         if (flags & 16)
337                 value += tracking_is_on_route(tr->rt, &sd->item);
338         return value;
339 }
340
341
342 int
343 tracking_update(struct tracking *tr, struct pcoord *pc, int angle)
344 {
345         struct tracking_line *t;
346         int i,value,min;
347         struct coord lpnt;
348         struct coord cin;
349 #if 0
350         int min,dist;
351         int debug=0;
352 #endif
353         dbg(1,"enter(%p,%p,%d)\n", tr, pc, angle);
354         dbg(1,"c=%d:0x%x,0x%x\n", pc->pro, pc->x, pc->y);
355
356         if (pc->x == tr->curr_in.x && pc->y == tr->curr_in.y) {
357                 if (tr->curr_out.x && tr->curr_out.y)
358                         *pc=tr->curr_out;
359                 return 0;
360         }
361         tr->last_in=tr->curr_in;
362         tr->last_out=tr->curr_out;
363         tr->last[0]=tr->curr[0];
364         tr->last[1]=tr->curr[1];
365         tr->curr_in=*pc;
366         tr->curr_angle=angle;
367         cin.x = pc->x;
368         cin.y = pc->y;
369         if (!tr->lines || transform_distance_sq_pc(&tr->last_updated, pc) > 250000) {
370                 dbg(1, "update\n");
371                 tracking_free_lines(tr);
372                 tracking_doupdate_lines(tr, pc);
373                 tr->last_updated=*pc;
374                 dbg(1,"update end\n");
375         }
376                 
377         t=tr->lines;
378         if (! t)
379                 return 0;
380         tr->curr_line=NULL;
381         min=INT_MAX/2;
382         while (t) {
383                 struct street_data *sd=t->street;
384                 if ((sd->flags & 3) == 3) {
385                         t=t->next;
386                         continue;
387                 }
388                 for (i = 0; i < sd->count-1 ; i++) {
389                         value=tracking_value(tr,t,i,&lpnt,min,-1);
390                         if (value < min) {
391                                 tr->curr_line=t;
392                                 tr->pos=i;
393                                 tr->curr[0]=sd->c[i];
394                                 tr->curr[1]=sd->c[i+1];
395                                 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i, 
396                                         transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt),
397                                         tracking_angle_delta(angle, t->angle[i], 0)*angle_factor,
398                                         tracking_is_connected(tr->last, &sd->c[i]) ? connected_pref : 0,
399                                         lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0,
400                                         value
401                                 );
402                                 tr->curr_out.x=lpnt.x;
403                                 tr->curr_out.y=lpnt.y;
404                                 tr->curr_out.pro = pc->pro;
405                                 min=value;
406                         }
407                 }
408                 t=t->next;
409         }
410         dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
411         if (!tr->curr_line || min > offroad_limit_pref)
412                 return 0;
413         dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
414         *pc=tr->curr_out;
415         return 1;       
416 }
417
418 struct tracking *
419 tracking_new(struct attr *parent, struct attr **attrs)
420 {
421         struct tracking *this=g_new0(struct tracking, 1);
422
423         return this;
424 }
425
426 void
427 tracking_set_mapset(struct tracking *this, struct mapset *ms)
428 {
429         this->ms=ms;
430 }
431
432 void
433 tracking_set_route(struct tracking *this, struct route *rt)
434 {
435         this->rt=rt;
436 }
437
438 void
439 tracking_destroy(struct tracking *tr)
440 {
441         tracking_free_lines(tr);
442         g_free(tr);
443 }
444
445 struct map *
446 tracking_get_map(struct tracking *this_)
447 {
448         if (! this_->map)
449                 this_->map=map_new(NULL, (struct attr*[]){
450                         &(struct attr){attr_type,{"tracking"}},
451                         &(struct attr){attr_trackingo,.u.tracking=this_},
452                         &(struct attr){attr_data,{""}},
453                         &(struct attr){attr_description,{"Tracking"}},
454                         NULL});
455         return this_->map;
456 }
457
458
459 struct map_priv {
460         struct tracking *tracking;
461 };
462
463 struct map_rect_priv {
464         struct tracking *tracking;
465         struct item item;
466         struct tracking_line *curr,*next;
467         int coord;
468         enum attr_type attr_next;
469         int ccount;
470         int debug_idx;
471         char *str;
472 };
473
474 static int
475 tracking_map_item_coord_get(void *priv_data, struct coord *c, int count)
476 {
477         struct map_rect_priv *this=priv_data;
478         enum projection pro;
479         int ret=0;
480         dbg(1,"enter\n");
481         while (this->ccount < 2 && count > 0) {
482                 pro = map_projection(this->curr->street->item.map);
483                 if (projection_mg != pro) {
484                         transform_from_to(&this->curr->street->c[this->ccount+this->coord],
485                                 pro,
486                                 c ,projection_mg);
487                 } else
488                 *c=this->curr->street->c[this->ccount+this->coord];
489                 dbg(1,"coord %d 0x%x,0x%x\n",this->ccount,c->x,c->y);
490                 this->ccount++;
491                 ret++;
492                 c++;
493                 count--;
494         }
495         return ret;
496 }
497
498 static int
499 tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr)
500 {
501         struct map_rect_priv *this_=priv_data;
502         attr->type=attr_type;
503         struct coord lpnt,*c;
504         struct tracking *tr=this_->tracking;
505         int value;
506
507         if (this_->str) {
508                 g_free(this_->str);
509                 this_->str=NULL;
510         }
511
512         switch(attr_type) {
513         case attr_debug:
514                 switch(this_->debug_idx) {
515                 case 0:
516                         this_->debug_idx++;
517                         this_->str=attr->u.str=g_strdup_printf("overall: %d",tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, -1));
518                         return 1;
519                 case 1:
520                         this_->debug_idx++;
521                         c=&this_->curr->street->c[this_->coord];
522                         value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1);
523                         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",
524                                 tr->curr_in.x, tr->curr_in.y,
525                                 c[0].x, c[0].y, c[1].x, c[1].y,
526                                 lpnt.x, lpnt.y, value);
527                         return 1;
528                 case 2:
529                         this_->debug_idx++;
530                         this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d",
531                                 tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
532                                 tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2));
533                         return 1;
534                 case 3:
535                         this_->debug_idx++;
536                         this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 4));
537                         return 1;
538                 case 4:
539                         this_->debug_idx++;
540                         this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 8));
541                         return 1;
542                 case 5:
543                         this_->debug_idx++;
544                         this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 16));
545                         return 1;
546                 case 6:
547                         this_->debug_idx++;
548                         this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr);
549                         return 1;
550                 default:
551                         this_->attr_next=attr_none;
552                         return 0;
553                 }
554         case attr_any:
555                 while (this_->attr_next != attr_none) {
556                         if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
557                                 return 1;
558                 }
559                 return 0;
560         default:
561                 attr->type=attr_none;
562                 return 0;
563         }
564 }
565
566 static struct item_methods tracking_map_item_methods = {
567         NULL,
568         tracking_map_item_coord_get,
569         NULL,
570         tracking_map_item_attr_get,
571 };
572
573
574 static void
575 tracking_map_destroy(struct map_priv *priv)
576 {
577         g_free(priv);
578 }
579
580 static void
581 tracking_map_rect_init(struct map_rect_priv *priv)
582 {
583         priv->next=priv->tracking->lines;
584         priv->curr=NULL;
585         priv->coord=0;
586         priv->item.id_lo=0;
587         priv->item.id_hi=0;
588 }
589
590 static struct map_rect_priv *
591 tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel)
592 {
593         struct tracking *tracking=priv->tracking;
594         struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
595         ret->tracking=tracking;
596         tracking_map_rect_init(ret);
597         ret->item.meth=&tracking_map_item_methods;
598         ret->item.priv_data=ret;
599         ret->item.type=type_tracking_100;
600         return ret;
601 }
602
603 static void
604 tracking_map_rect_destroy(struct map_rect_priv *priv)
605 {
606         g_free(priv);
607 }
608
609 static struct item *
610 tracking_map_get_item(struct map_rect_priv *priv)
611 {
612         struct item *ret=&priv->item;
613         int value;
614         struct coord lpnt;
615
616         if (!priv->next)
617                 return NULL;
618         if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) {
619                 priv->curr=priv->next;
620                 priv->next=priv->curr->next;
621                 priv->coord=0;
622                 priv->item.id_lo=0;
623                 priv->item.id_hi++;
624         } else {
625                 priv->coord++;
626                 priv->item.id_lo++;
627         }
628         value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1);
629         if (value < 64) 
630                 priv->item.type=type_tracking_100;
631         else if (value < 128)
632                 priv->item.type=type_tracking_90;
633         else if (value < 256)
634                 priv->item.type=type_tracking_80;
635         else if (value < 512)
636                 priv->item.type=type_tracking_70;
637         else if (value < 1024)
638                 priv->item.type=type_tracking_60;
639         else if (value < 2048)
640                 priv->item.type=type_tracking_50;
641         else if (value < 4096)
642                 priv->item.type=type_tracking_40;
643         else if (value < 8192)
644                 priv->item.type=type_tracking_30;
645         else if (value < 16384)
646                 priv->item.type=type_tracking_20;
647         else if (value < 32768)
648                 priv->item.type=type_tracking_10;
649         else
650                 priv->item.type=type_tracking_0;
651         dbg(1,"item %d %d points\n", priv->coord, priv->curr->street->count);
652         priv->ccount=0;
653         priv->attr_next=attr_debug;
654         priv->debug_idx=0;
655         return ret;
656 }
657
658 static struct item *
659 tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo)
660 {
661         struct item *ret;
662         tracking_map_rect_init(priv);
663         while ((ret=tracking_map_get_item(priv))) {
664                 if (ret->id_hi == id_hi && ret->id_lo == id_lo) 
665                         return ret;
666         }
667         return NULL;
668 }
669
670 static struct map_methods tracking_map_meth = {
671         projection_mg,
672         "utf-8",
673         tracking_map_destroy,
674         tracking_map_rect_new,
675         tracking_map_rect_destroy,
676         tracking_map_get_item,
677         tracking_map_get_item_byid,
678         NULL,
679         NULL,
680         NULL,
681 };
682
683 static struct map_priv *
684 tracking_map_new(struct map_methods *meth, struct attr **attrs)
685 {
686         struct map_priv *ret;
687         struct attr *tracking_attr;
688
689         tracking_attr=attr_search(attrs, NULL, attr_trackingo);
690         if (! tracking_attr)
691                 return NULL;
692         ret=g_new0(struct map_priv, 1);
693         *meth=tracking_map_meth;
694         ret->tracking=tracking_attr->u.tracking;
695
696         return ret;
697 }
698
699
700 void
701 tracking_init(void)
702 {
703         plugin_register_map_type("tracking", tracking_map_new);
704 }