Ticket #1258: route.c_bidirDijkstra.diff

File route.c_bidirDijkstra.diff, 48.5 KB (added by marcus püschel, 5 years ago)
Line 
179,93d78
2< /* =================================================================
3<  * These part of the Header is used by multithreaded implementations
4<  */
5< #include "util.h"
6< #include <pthread.h>
7< #include <glib.h>
8<
9< /**@brief varaibles used for enabling and disabling functions
10<  *
11<  * These variables are influenced by the menue and control if the bidirectional routing
12<  * and also the parallel graph buildup are used or not.
13<  */
14< int useBidirectionalRouting = 0;
15< int useBidirectionalMultiThread = 0;
16<
1796,126d80
18< /**@brief varaibles used for time measurement
19<  */
20< double timestamp_update = (double)0.0;
21< double timestamp_build = (double)0.0;
22< double timestamp_binextract = (double)0.0;
23< double timestamp_flood = (double)0.0;
24< double timestamp_floodbidirectional = (double)0.0;
25< double timestamp_build_next_map = (double)0.0;
26<
27< /* Needed for bidirectional Search */
28< struct route_graph_common_point {
29<       struct route_graph_point *pointref;     /* reference to common point with lowest costs mu */
30<       int mu;                                                         /* costs mu with path over this common point */
31<       int value_p_min;                                        /* costs of actual point within target to source flooding */
32<       int value_p_min_back;                           /* costs of actual point within source to target flooding */
33< };
34<
35< struct route_graph_flood_bidirectional_threadstruct {
36<       struct route_graph *graph;
37<       struct route_info *src;
38<       struct route_info *dst;
39<       struct vehicleprofile *profile;
40<       struct route_graph_point *initial_point;
41<       struct route_graph_common_point *common_point;
42< };
43<
44< pthread_mutex_t lock_common_point;
45< // translation for Windows and other OS's needed!
46<
47< /* ================================================================= */
48<
49148,151d101
50<
51<       /* Extension to the route_graph_point used for bidirectional dijkstra */
52<       struct route_graph_segment *fromseg; /**< Pointer to the segment used to reach this point */
53<
54154,155d103
55<       struct fibheap_el *el_back;                              /**< When this point is put on a Fibonacci heap, this is a pointer
56<                                                                                 *  to this point's heap-element */
57157d104
58<       int value_common;                                                /**< The cost at which one can reach the destination from this point on */
59159c106
60<       int flags;                                                      /**< Flags for this point (eg traffic distortion) */
61---
62>       int flags;                                              /**< Flags for this point (eg traffic distortion) */
63366,373c313
64<
65< static void route_graph_flood(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb, struct route_info *src);
66< static void route_graph_flood_standard(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb);
67< static void route_graph_flood_bidirectional(struct route_graph *this, struct route_info *src, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb);
68< static void route_graph_flood_bidirectional2Threads(struct route_graph *this, struct route_info *src, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb);
69< static int flood_bidirectional_target_to_source(void *attr);
70< static int flood_bidirectional_source_to_target(void *attr);
71< static int calculate_costs_and_set_refs(struct vehicleprofile *profile, struct route_graph_point *src_point, struct route_info *src, struct route_graph_point * common_point ,struct route_graph_point * dst_point, struct route_info *dst);
72---
73> static void route_graph_flood(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb);
74375a316
75>
76869,870c810
77<                       //route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb);
78<                       route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb, this->pos);
79---
80>                       route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb);
811394c1334
82<               route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb, this->pos);
83---
84>               route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb);
851535d1474
86<                       curr->fromseg=NULL;
871537,1538d1475
88<                       curr->el_back=NULL;
89<                       curr->value_common=INT_MAX;
902118c2055
91<       if (!route_through_traffic_allowed(profile, over) && from && from->seg && route_through_traffic_allowed(profile, from->seg))
92---
93>       if (!route_through_traffic_allowed(profile, over) && from && route_through_traffic_allowed(profile, from->seg))
942384,2407d2320
95<  * @brief Switch between different Dijkstra implementations
96<  */
97< static void
98< route_graph_flood(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb, struct route_info *src)
99< {
100<
101<       if(useBidirectionalRouting)
102<       {
103<               if(useBidirectionalMultiThread)
104<               {
105<                       route_graph_flood_bidirectional2Threads(this, src, dst, profile, cb);
106<               }
107<               else
108<               {
109<                       route_graph_flood_bidirectional(this, src, dst, profile, cb);
110<               }
111<       }
112<       else
113<       {
114<               route_graph_flood_standard(this, dst, profile, cb);
115<       }
116< }
117<
118< /**
1192419c2332
120< route_graph_flood_standard(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb)
121---
122> route_graph_flood(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb)
1232421,2422d2333
124<       timestamp_flood = now_ms();
125<
1262426c2337
127<       struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs from target to source*/
128---
129>       struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs */
1302428,2429c2339
131<       // heap for Target - Source
132<       heap = fh_makekeyheap();
133---
134>       heap = fh_makekeyheap();   
1352431,2432d2340
136<       // Fill Heap initial with first segment(s) of source
137<       // and get local costs for this segment(s)
1382449d2356
139<
1402454d2360
141<
1422529,3240d2434
143<
144<       dbg(0,"route_graph_flood() took: %.3f ms\n", now_ms() - timestamp_flood);
145<
146<       callback_call_0(cb);
147<       dbg(1,"return\n");
148< }
149<
150< /**
151<  * @brief Calculates the routing costs for each point,  starting from 2 points
152<  *
153<  * This function is the heart of routing. It assigns each point in the route graph a
154<  * cost at which one can reach the destination from this point on. Additionally it assigns
155<  * each point a segment one should follow from this point on to reach the destination at the
156<  * stated costs.
157<  *
158<  * This function uses Dijkstra's algorithm to do the routing. To understand it you should have a look
159<  * at this algorithm.
160<  */
161< static void
162< route_graph_flood_bidirectional(struct route_graph *this, struct route_info *src, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb)
163< {
164<
165<       //const int dbg_level = 0;
166<       //debug_route = 1;
167<       timestamp_floodbidirectional = now_ms();
168<
169<       struct route_graph_point *p_min = NULL;
170<       struct route_graph_segment *s=NULL;
171<       int min, new,val;
172<       struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs from target to source*/
173<
174<       // Values for Source - Target search (Back)
175<       struct route_graph_point *p_min_back = NULL;
176<       struct route_graph_segment *s_back = NULL;
177<       int min_back, new_back, val_back, mu, mu_tmp;
178<       struct fibheap *heap_back; /* This heap will hold all points with "temporarily" calculated costs from source to target*/
179<
180<       //struct route_graph_point *curr;
181<       struct route_graph_point *common_point;
182<       struct route_graph_point *source_point;
183<       struct route_graph_point *destination_point;
184<
185<       mu = INT_MAX;
186<       mu_tmp = INT_MAX;
187<       min = INT_MAX;
188<       min_back = INT_MAX;
189<       // heap for Target to CommonPoint
190<       heap = fh_makekeyheap();
191<       // heap for Source to CommonPoint
192<       heap_back = fh_makekeyheap();
193<
194<       // Fill Heap initial with first points reachable by initial segment
195<       // where target is located on and get local costs for this partial segment(s) / Position within this Segment
196<       while ((s=route_graph_get_segment(this, dst->street, s))) {
197<               val=route_value_seg(profile, NULL, s, -1);
198<               if (val != INT_MAX)
199<               {
200<                       val=val*(100-dst->percent)/100;
201<                       s->end->seg=s;
202<                       s->end->value=val;
203<                       s->end->el=fh_insertkey(heap, s->end->value, s->end);
204<                       destination_point = s->end;
205<                       min = val;
206<               }
207<               val=route_value_seg(profile, NULL, s, 1);
208<               if (val != INT_MAX)
209<               {
210<                       val=val*dst->percent/100;
211<                       s->start->seg=s;
212<                       s->start->value=val;
213<                       s->start->el=fh_insertkey(heap, s->start->value, s->start);
214<
215<                       if(val < min)
216<                       {
217<                               destination_point = s->start;
218<                       }
219<               }
220<       }
221<
222<       // Fill Heap initial with first points reachable by initial segment
223<       // where source (tp) is located on and get local costs for this partial segment(s) / Position within this Segment
224<
225<       while ((s_back=route_graph_get_segment(this, src->street, s_back))) {
226<               val_back = route_value_seg(profile, NULL, s_back, 1);
227<               if (val_back != INT_MAX)
228<               {
229<                       val_back = val_back*(100-src->percent)/100;
230<                       s_back->end->fromseg = s_back;
231<                       // set costs for point start of initial segment
232<                       s_back->end->value = val_back;
233<                       s_back->end->el_back = fh_insertkey(heap_back, s_back->end->value, s_back->end);
234<                       source_point = s_back->end;
235<                       min_back = val_back;
236<               }
237<               val_back = route_value_seg(profile, NULL, s_back, -1);
238<               if (val_back != INT_MAX)
239<               {
240<                       val_back = val_back*src->percent/100;
241<                       s_back->start->fromseg = s_back;
242<                       // set costs for point end of initial segment
243<                       s_back->start->value = val_back;
244<                       s_back->start->el_back = fh_insertkey(heap_back, s_back->start->value, s_back->start);
245<                       if(val_back < min_back)
246<                       {
247<                               source_point = s_back->start;
248<                       }
249<               }
250<       }
251<
252<       min = INT_MAX;
253<       min_back = INT_MAX;
254<
255<       //=====================================
256<
257<       for (;;) {
258<
259<               p_min = fh_extractmin(heap); /* Starting Dijkstra by selecting the point with the minimum costs on the heap */
260<               //dbg(dbg_level,"extract p=%p min=%d, 0x%x, 0x%x\n", p_min, min, p_min->c.x, p_min->c.y);
261<
262<               p_min_back = fh_extractmin(heap_back);
263<               //dbg(dbg_level,"extract_back p=%p min=%d, 0x%x, 0x%x\n", p_min_back, min_back, p_min_back->c.x, p_min_back->c.y);
264<
265<               // termination consition: both heaps are empty
266<               if (!p_min && !p_min_back )
267<               {
268<                       dbg(1,"both heaps empty\n");
269<                       break;
270<               }
271<
272<               if(p_min_back)
273<               {
274<                       // Costs to reach this point:
275<                       min_back = p_min_back->value;
276<               }
277<               if(p_min)
278<               {
279<                       // Costs to reach this point:
280<                       min = p_min->value;
281<               }
282<
283<               // termination condition: found shortest possible route from source to target
284<               if ( mu < (min + min_back))
285<               {
286<                       dbg(1,"%d < %d\n", mu, (min + min_back));
287<                       break;
288<               }
289<
290<               // executing dijkstra starting from destination
291<               if ( p_min ){
292<                       // Forward Step (flood direction is from target to source/common point)
293<                       if (debug_route)
294<                       {
295<                               printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
296<                       }
297<                       p_min->el = NULL; /* This point is permanently calculated now, we've taken it out of the heap */
298<                       s = p_min->start;
299<
300<                       //dbg(0,"forward step: outgoing edges\n");
301<                       // Outgoing Egdes:
302<                       while (s) { /* Iterating all the segments leading away from our point to update the points at their ends */
303<                               val = route_value_seg(profile, p_min, s, -1);
304<                               if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
305<                                       if (profile->turn_around_penalty2)
306<                                               val+=profile->turn_around_penalty2;
307<                                       else
308<                                               val=INT_MAX;
309<                               }
310<                               if (val != INT_MAX) {
311<                                       new=min+val;
312<                                       if (debug_route)
313<                                               printf("begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
314<                                       if(s->end->fromseg != NULL)
315<                                       {
316<                                               //dbg(dbg_level,"found common point\n");
317<                                               mu_tmp = new + s->end->value;
318<                                               if (mu_tmp < mu)
319<                                               {
320<                                                       mu = mu_tmp;
321<                                                       s->end->seg=s;
322<                                                       common_point = s->end;
323<                                                       //dbg(dbg_level,"costs for common point: %d\n", mu);
324<                                               }
325<                                       }
326<                                       else
327<                                       {
328<                                               if (new < s->end->value) { /* We've found a less costly way to reach the end of s, update it */
329<                                                       s->end->value=new;
330<                                                       s->end->seg=s;
331<                                                       if (! s->end->el) {
332<                                                               if (debug_route)
333<                                                                       printf("insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
334<                                                               s->end->el=fh_insertkey(heap, new, s->end);
335<                                                               if (debug_route)
336<                                                                       printf("el new=%p\n", s->end->el);
337<                                                       }
338<                                                       else {
339<                                                               if (debug_route)
340<                                                                       printf("replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
341<                                                               fh_replacekey(heap, s->end->el, new);
342<                                                       }
343<                                               }
344<                                       }
345<                                       if (debug_route)
346<                                               printf("\n");
347<                               }
348<                               s=s->start_next;
349<                       }
350<                       s = p_min->end;
351<                       //dbg(dbg_level,"forward step: incoming edges\n");
352<                       // Incoming Egdes:
353<                       while (s) { /* Doing the same as above with the segments leading towards our point */
354<                               val = route_value_seg(profile, p_min, s, 1);
355<                               if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
356<                                       if (profile->turn_around_penalty2)
357<                                               val+=profile->turn_around_penalty2;
358<                                       else
359<                                               val=INT_MAX;
360<                               }
361<                               if (val != INT_MAX) {
362<                                       new = min + val;
363<                                       if (debug_route)
364<                                               printf("end %d len %d vs %d (0x%x,0x%x)\n",new,val,s->start->value,s->start->c.x, s->start->c.y);
365<                                       if(s->start->fromseg != NULL)
366<                                       {
367<                                               //dbg(dbg_level,"found common point\n");
368<                                               mu_tmp = new + s->start->value;
369<                                               if (mu_tmp < mu)
370<                                               {
371<                                                       mu = mu_tmp;
372<                                                       s->start->seg=s;
373<                                                       common_point = s->start;
374<                                                       //dbg(dbg_level,"costs for common point: %d\n", mu);
375<                                               }
376<                                       }
377<                                       else
378<                                       {
379<                                               if (new < s->start->value) {
380<                                                       s->start->value=new;
381<                                                       s->start->seg=s;
382<                                                       if (! s->start->el) {
383<                                                               if (debug_route)
384<                                                                       printf("insert_start p=%p el=%p val=%d ", s->start, s->start->el, s->start->value);
385<                                                               s->start->el=fh_insertkey(heap, new, s->start);
386<                                                               if (debug_route)
387<                                                                       printf("el new=%p\n", s->start->el);
388<                                                       }
389<                                                       else {
390<                                                               if (debug_route)
391<                                                                       printf("replace_start p=%p el=%p val=%d\n", s->start, s->start->el, s->start->value);
392<                                                               fh_replacekey(heap, s->start->el, new);
393<                                                       }
394<                                               }
395<                                       }
396<                                       if (debug_route)
397<                                               printf("\n");
398<                               }
399<                               s=s->end_next;
400<                       }
401<               }
402<
403<               // executing dijkstra starting from source
404<               if(p_min_back){
405<                       // ===============================================================================================================================
406<                       // Backward Step (flood direction is from source to target/common point)
407<                       // set ->fromseg  when common_point point is found for constructing the
408<                       // final route path with ->ref afterwards
409<
410<                       if (debug_route)
411<                       {
412<                               printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min_back, p_min_back->el, min_back, p_min_back->c.x, p_min_back->c.y);
413<                       }
414<                       p_min_back->el_back=NULL; /* This point is permanently calculated now, we've taken it out of the heap */
415<                       s_back = p_min_back->start;
416<                       //dbg(dbg_level,"backward step: outgoing edges\n");
417<
418<                       while (s_back) { /* Iterating all the segments leading away from our point to update the points at their ends */
419<                               val_back = route_value_seg(profile, p_min_back, s_back, 1);
420<                               if (val_back != INT_MAX && item_is_equal(s_back->data.item, p_min_back->fromseg->data.item)) {
421<                                       if (profile->turn_around_penalty2)
422<                                               val_back += profile->turn_around_penalty2;
423<                                       else
424<                                               val_back = INT_MAX;
425<                               }
426<                               if (val_back != INT_MAX)
427<                               {
428<                                       new_back = min_back + val_back;
429<                                       if(s_back->end->seg != NULL)
430<                                       {
431<                                               //dbg(dbg_level,"found common point\n");
432<                                               mu_tmp = new_back + s_back->end->value;
433<                                               //s_back->end->value_common = mu_tmp;
434<                                               if (mu_tmp < mu)
435<                                               {
436<                                                       mu = mu_tmp;
437<                                                       common_point = s_back->end;
438<                                                       s_back->end->fromseg = s_back;
439<                                                       //dbg(dbg_level,"costs for common point: %d\n", mu);
440<                                               }
441<                                       }
442<                                       else
443<                                       {
444<                                               if (new_back < s_back->end->value) { /* We've found a less costly way to reach the end of s, update it */
445<                                                       s_back->end->value = new_back;
446<                                                       s_back->end->fromseg = s_back;
447<
448<                                                       if (! s_back->end->el_back)
449<                                                       {
450<                                                               if (debug_route)
451<                                                                       printf("insert_end p_back=%p el_back=%p val_back=%d ", s_back->end, s_back->end->el_back, s_back->end->value);
452<                                                               s_back->end->el_back = fh_insertkey(heap_back, new_back, s_back->end);
453<                                                       }
454<                                                       else
455<                                                       {
456<                                                               if (debug_route)
457<                                                                       printf("replaced_end p_back=%p el_back=%p val_back=%d ", s_back->end, s_back->end->el_back, s_back->end->value);
458<                                                               fh_replacekey(heap_back, s_back->end->el_back, new_back);
459<                                                       }
460<                                               }
461<                                       }
462<                               }
463<                               s_back=s_back->start_next;
464<                       }
465<
466<                       //dbg(dbg_level,"backward step: incoming edges\n");
467<                       s_back=p_min_back->end;
468<                       while (s_back) { /* Doing the same as above with the segments leading towards our point */
469<                               val_back = route_value_seg(profile, p_min_back, s_back, -1);
470<                               if (val_back != INT_MAX && item_is_equal(s_back->data.item, p_min_back->fromseg->data.item)) {
471<                                       if (profile->turn_around_penalty2)
472<                                               val_back += profile->turn_around_penalty2;
473<                                       else
474<                                               val_back = INT_MAX;
475<                               }
476<                               if (val_back != INT_MAX) {
477<                                       new_back = min_back + val_back;
478<                                       if(s_back->start->seg != NULL)
479<                                       {
480<                                               //dbg(dbg_level,"found common point\n");
481<                                               mu_tmp = new_back + s_back->start->value;
482<                                               if (mu_tmp < mu)
483<                                               {
484<                                                       mu = mu_tmp;
485<                                                       s_back->start->fromseg = s_back;
486<                                                       common_point = s_back->start;
487<                                                       //dbg(dbg_level,"costs for common point: %d\n", mu);
488<                                               }
489<                                       }
490<                                       else
491<                                       {
492<                                               if (new_back < s_back->start->value) {
493<                                                       s_back->start->value = new_back;
494<                                                       s_back->start->fromseg = s_back;
495<
496<                                                       if (! s_back->start->el_back)
497<                                                       {
498<                                                               if (debug_route)
499<                                                                       printf("insert_start p_back=%p el_back=%p val_back=%d ", s_back->start, s_back->start->el_back, s_back->start->value);
500<                                                               s_back->start->el_back = fh_insertkey(heap_back, new_back, s_back->start);
501<                                                       }
502<                                                       else
503<                                                       {
504<                                                               if (debug_route)
505<                                                                       printf("replaced_start p_back=%p el_back=%p val_back=%d ", s_back->start, s_back->start->el_back, s_back->start->value);
506<                                                               fh_replacekey(heap_back, s_back->start->el_back, new_back);
507<                                                       }
508<                                               }
509<                                       }
510<                               }
511<                               s_back=s_back->end_next;
512<                       }
513<               }
514<       } // end loop
515<
516<       /*
517<        * following start->fromseg, which indicates the least costly way
518<        * to reach the source from the common point and save taken path
519<        * by setting ->seg
520<        */
521< //    dbg(dbg_level,"backward step: traversing from common point to source\n");
522< //
523< //    s = common_point->fromseg;
524< //    if(s->start == common_point)
525< //    {
526< //            curr = s->end;
527< //    }else{
528< //            curr = s->start;
529< //    }
530< //
531< //    s->start->seg = s;
532< //    s->end->seg = s;
533< //    //curr = s->start;
534< //
535< //    for (;;) {
536< //
537< //            printf("curr %p - s->seg %p [ S: %p, E: %p ]\n", curr, curr->fromseg, curr->fromseg->start, curr->fromseg->end);
538< //
539< //            if(curr == s->start )
540< //            {
541< //                    curr = s->end;
542< //                    s->end->seg = s;
543< //
544< //
545< //            }else{
546< //                    curr = s->start;
547< //                    s->start->seg = s;
548< //            }
549< //
550< //            // reached source?
551< //            if( item_is_equal(src->street->item, s->data.item) )
552< //            {
553< //                    //s_back->end->seg = s_back;
554< //                    dbg(dbg_level, "reached Source\n");
555< //                    break;
556< //            }
557< //
558< //            // avoid loop:
559< //            if( item_is_equal(curr->fromseg->data.item, s->data.item) )
560< //            {
561< //                    dbg(dbg_level, "loop detected!\n");
562< //                    break;
563< //            }
564< //
565< //            s = curr->fromseg;
566< //
567< //    }
568<
569< // bad idea, confuses References!
570< //                    timestamp_reverse = now_ms();
571< //                    struct route_graph_point *curr;
572< //                    for (i = 0 ; i < HASH_SIZE ; i++) {
573< //                            curr=this->hash[i];
574< //                            while (curr) {
575< //                                    if (curr->fromseg)
576< //                                    {
577< //                                            curr->fromseg->start->seg = curr->fromseg;
578< //                                    }
579< //                                    curr=curr->hash_next;
580< //                            }
581< //                    }
582< //                    dbg(dbg_level, "reverse took %.2f ms\n", now_ms() - timestamp_reverse);
583< //
584<
585<       calculate_costs_and_set_refs(profile, source_point, src, common_point , destination_point, dst);
586<       //dbg(dbg_level,"result costs %d\n", mu_tmp);
587<
588<       fh_deleteheap(heap);
589<       fh_deleteheap(heap_back);
590<
591<
592<       dbg(0,"route_graph_flood_bidirectional() took: %.3f ms\n", now_ms() - timestamp_floodbidirectional);
593<
594<       callback_call_0(cb);
595<       dbg(1,"return\n");
596< }
597<
598< /**
599<  * @brief Calculates the routing costs for each point,  starting from 2 points with 2 threads
600<  *
601<  * This function is the heart of routing. It assigns each point in the route graph a
602<  * cost at which one can reach the destination from this point on. Additionally it assigns
603<  * each point a segment one should follow from this point on to reach the destination at the
604<  * stated costs.
605<  *
606<  * This function uses Dijkstra's algorithm to do the routing. To understand it you should have a look
607<  * at this algorithm.
608<  */
609< static void
610< route_graph_flood_bidirectional2Threads(struct route_graph *this, struct route_info *src, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb)
611< {
612< //    const int dbg_level = 0;
613< //    debug_route = 1;
614<       timestamp_floodbidirectional = now_ms();
615<       struct route_graph_common_point *common_point = g_new0(struct route_graph_common_point, 1);
616<       // initialization of common point variables
617<       common_point->pointref = NULL;
618<       common_point->mu = INT_MAX;
619<
620<       pthread_t st;
621< //    pthread_t ts;
622<       int st_ret;
623< //    int ts_ret;
624<       struct route_graph_flood_bidirectional_threadstruct st_attr;
625< //    struct route_graph_flood_bidirectional_threadstruct ts_attr;
626<
627<       // initialize thread parameter structs
628<       st_attr.graph = this;
629<       st_attr.common_point = common_point;
630<       st_attr.initial_point = NULL; /* set by thread-function*/
631<       st_attr.profile = profile;
632<       st_attr.src = src;
633<       st_attr.dst = dst;
634<
635< //    ts_attr.graph = this;
636< //    ts_attr.common_point = common_point;
637< //    ts_attr.initial_point = NULL; /* set by thread-function*/
638< //    ts_attr.profile = profile;
639< //    ts_attr.src = src;
640< //    ts_attr.dst = dst;
641<
642<       // start two threads for bidirectional flooding
643<       if( pthread_create( &st, NULL, &flood_bidirectional_source_to_target, (void*)&st_attr)!=0)
644<       {
645<               dbg(0,"Error creating Thread for flood_bidirectional_source_to_target()");
646<       }
647< //    if( pthread_create( &st, NULL, &flood_bidirectional_target_to_source, (void*)&ts_attr)!=0)
648< //    {
649< //            dbg(0,"Error creating Thread for flood_bidirectional_target_to_source()");
650< //    }
651<
652<       // target to source flooding within this function:
653<       struct route_graph_point *source_point = NULL;
654<       struct route_graph_point *p_min = NULL;
655<       struct route_graph_segment *s=NULL;
656<       int min, new, val, mu_tmp, mu;
657<       struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs from target to source*/
658<
659<       mu_tmp = INT_MAX;
660<       mu = INT_MAX;
661<       min = INT_MAX;
662<       // heap for Target to CommonPoint
663<       heap = fh_makekeyheap();
664<
665<       // Fill Heap initial with first points reachable by initial segment
666<       // where target is located on and get local costs for this partial segment(s) / Position within this Segment
667<       while ((s=route_graph_get_segment(this, dst->street, s))) {
668<               val=route_value_seg(profile, NULL, s, -1);
669<               if (val != INT_MAX)
670<               {
671<                       val=val*(100-dst->percent)/100;
672<                       s->end->seg=s;
673<                       s->end->value=val;
674<                       s->end->el=fh_insertkey(heap, s->end->value, s->end);
675<                       source_point = s->end;
676<                       min = val;
677<               }
678<               val=route_value_seg(profile, NULL, s, 1);
679<               if (val != INT_MAX)
680<               {
681<                       val=val*dst->percent/100;
682<                       s->start->seg=s;
683<                       s->start->value=val;
684<                       s->start->el=fh_insertkey(heap, s->start->value, s->start);
685<
686<                       if(val < min)
687<                       {
688<                               source_point = s->start;
689<                       }
690<               }
691<       }
692<
693<       min = INT_MAX;
694<       /* infinite loop */
695<       for (;;) {
696<
697<               p_min = fh_extractmin(heap); /* Starting Dijkstra by selecting the point with the minimum costs on the heap */
698<               //dbg(dbg_level,"extract p=%p min=%d, 0x%x, 0x%x\n", p_min, min, p_min->c.x, p_min->c.y);
699<
700<               // termination consition: both heaps are empty
701<               if (!p_min)
702<               {
703<                       dbg(1,"t-s heap empty\n");
704<                       break;
705<               }
706<
707<               min=p_min->value;
708<
709<               common_point->value_p_min = min;
710<               //pthread_mutex_lock(&lock_common_point);
711<               // termination condition: found shortest possible route from source to target
712<               mu = common_point->mu;
713<               if ( mu < (common_point->value_p_min_back + min))
714<               {
715<                       dbg(1,"%d < %d\n", mu, (common_point->value_p_min_back + min));
716<                       //pthread_mutex_unlock(&lock_common_point);
717<                       break;
718<               }
719<               //pthread_mutex_unlock(&lock_common_point);
720<
721<
722< //            if (debug_route)
723< //            {
724< //                    printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
725< //            }
726<
727<               p_min->el = NULL; /* This point is permanently calculated now, we've taken it out of the heap */
728<               s = p_min->start;
729<
730<               // Outgoing Egdes:
731<               while (s) { /* Iterating all the segments leading away from our point to update the points at their ends */
732<                       val = route_value_seg(profile, p_min, s, -1);
733<                       if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
734<                               if (profile->turn_around_penalty2)
735<                                       val+=profile->turn_around_penalty2;
736<                               else
737<                                       val=INT_MAX;
738<                       }
739<                       if (val != INT_MAX) {
740<                               new=min+val;
741< //                            if (debug_route)
742< //                                    printf("begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
743<                               if(s->end->fromseg != NULL)
744<                               {
745<                                       //dbg(dbg_level,"found common point\n");
746<                                       mu_tmp = new + s->end->value;
747<                                       pthread_mutex_lock(&lock_common_point);
748<                                       if (mu_tmp < common_point->mu)
749<                                       {
750<                                               common_point->mu = mu_tmp;
751<                                               s->end->seg=s;
752<                                               common_point->pointref = s->end;
753<                                               //dbg(dbg_level,"costs for common point: %d\n", mu);
754<                                       }
755<                                       pthread_mutex_unlock(&lock_common_point);
756<                               }
757<                               else
758<                               {
759<                                       if (new < s->end->value) { /* We've found a less costly way to reach the end of s, update it */
760<                                               s->end->seg=s;
761<                                               s->end->value=new;
762<
763<                                               if (! s->end->el) {
764< //                                                    if (debug_route)
765< //                                                            printf("insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
766<                                                       s->end->el=fh_insertkey(heap, new, s->end);
767< //                                                    if (debug_route)
768< //                                                            printf("el new=%p\n", s->end->el);
769<                                               }
770<                                               else {
771< //                                                    if (debug_route)
772< //                                                            printf("replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
773<                                                       fh_replacekey(heap, s->end->el, new);
774<                                               }
775<                                       }
776<                               }
777< //                            if (debug_route)
778< //                                    printf("\n");
779<                       }
780<                       s=s->start_next;
781<               } // end while loop
782<
783<               s = p_min->end;
784<               // Incoming Egdes:
785<               while (s) { /* Doing the same as above with the segments leading towards our point */
786<                       val = route_value_seg(profile, p_min, s, 1);
787<                       if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
788<                               if (profile->turn_around_penalty2)
789<                                       val+=profile->turn_around_penalty2;
790<                               else
791<                                       val=INT_MAX;
792<                       }
793<                       if (val != INT_MAX) {
794<                               new = min + val;
795< //                            if (debug_route)
796< //                                    printf("end %d len %d vs %d (0x%x,0x%x)\n",new,val,s->start->value,s->start->c.x, s->start->c.y);
797<                               if(s->start->fromseg != NULL)
798<                               {
799<                                       //dbg(dbg_level,"found common point\n");
800<                                       mu_tmp = new + s->start->value;
801<                                       pthread_mutex_lock(&lock_common_point);
802<                                       if (mu_tmp < common_point->mu)
803<                                       {
804<                                               common_point->mu = mu_tmp;
805<                                               s->start->seg=s;
806<                                               common_point->pointref = s->start;
807<                                               //dbg(dbg_level,"costs for common point: %d\n", mu);
808<                                       }
809<                                       pthread_mutex_unlock(&lock_common_point);
810<                               }
811<                               else
812<                               {
813<                                       if (new < s->start->value) {
814<                                               s->start->value=new;
815<                                               s->start->seg=s;
816<                                               if (! s->start->el) {
817< //                                                    if (debug_route)
818< //                                                            printf("insert_start p=%p el=%p val=%d ", s->start, s->start->el, s->start->value);
819<                                                       s->start->el=fh_insertkey(heap, new, s->start);
820< //                                                    if (debug_route)
821< //                                                            printf("el new=%p\n", s->start->el);
822<                                               }
823<                                               else {
824< //                                                    if (debug_route)
825< //                                                            printf("replace_start p=%p el=%p val=%d\n", s->start, s->start->el, s->start->value);
826<                                                       fh_replacekey(heap, s->start->el, new);
827<                                               }
828<                                       }
829<                               }
830< //                            if (debug_route)
831< //                                    printf("\n");
832<                       }
833<                       s=s->end_next;
834<               } // end while loop
835<       } // end for loop
836<
837<       //=====================================
838<
839<       fh_deleteheap(heap);
840<
841<
842<       // blocking wait for Thread:
843<       pthread_join(st, &st_ret);
844<       //pthread_join(ts, &ts_ret);
845<
846<       /*
847<        * following start->fromseg, which indicates the least costly way
848<        * to reach the source from the common point and save taken path
849<        * by setting ->seg
850<        */
851<       calculate_costs_and_set_refs(profile, st_attr.initial_point, src, common_point->pointref , source_point, dst);
852<       //dbg(dbg_level,"result costs %d\n", mu_tmp);
853<
854<       dbg(0," took: %.3f ms\n", now_ms() - timestamp_floodbidirectional);
8553242d2435
856<       g_free(common_point);
8573246,3822d2438
858< static int
859< flood_bidirectional_source_to_target(void *attr)
860< {
861<       struct route_graph_flood_bidirectional_threadstruct *st_attr = (struct route_graph_flood_bidirectional_threadstruct*)attr;
862<       struct route_graph *this = st_attr->graph;
863<       struct route_info *src = st_attr->src;
864< //    struct route_info *dst = st_attr->dst;
865<       struct vehicleprofile *profile = st_attr->profile;
866<       struct route_graph_common_point *common_point = st_attr->common_point;
867<
868<       struct route_graph_point *p_min_back = NULL;
869<       struct route_graph_segment *s_back = NULL;
870<       int min_back, new_back, val_back, mu_tmp, mu;
871<       struct fibheap *heap_back; /* This heap will hold all points with "temporarily" calculated costs from source to target*/
872<
873<       mu_tmp = INT_MAX;
874<       mu = INT_MAX;
875<       min_back = INT_MAX;
876<       // heap for Source to CommonPoint
877<       heap_back = fh_makekeyheap();
878<
879<       // Fill Heap initial with first points reachable by initial segment
880<       // where source (tp) is located on and get local costs for this partial segment(s) / Position within this Segment
881<       while ((s_back=route_graph_get_segment(this, src->street, s_back))) {
882<               val_back = route_value_seg(profile, NULL, s_back, 1);
883<               if (val_back != INT_MAX)
884<               {
885<                       val_back = val_back*(100-src->percent)/100;
886<                       s_back->end->fromseg = s_back;
887<                       // set costs for point start of initial segment
888<                       s_back->end->value = val_back;
889<                       s_back->end->el_back = fh_insertkey(heap_back, s_back->end->value, s_back->end);
890<                       st_attr->initial_point = s_back->end;
891<                       min_back = val_back;
892<               }
893<               val_back = route_value_seg(profile, NULL, s_back, -1);
894<               if (val_back != INT_MAX)
895<               {
896<                       val_back = val_back*src->percent/100;
897<                       s_back->start->fromseg = s_back;
898<                       // set costs for point end of initial segment
899<                       s_back->start->value = val_back;
900<                       s_back->start->el_back = fh_insertkey(heap_back, s_back->start->value, s_back->start);
901<                       if(val_back < min_back)
902<                       {
903<                               st_attr->initial_point = s_back->start;
904<                       }
905<               }
906<       }
907<       min_back = INT_MAX;
908<       //=====================================
909<       for (;;) {
910<               p_min_back = fh_extractmin(heap_back);
911<               //dbg(dbg_level,"extract_back p=%p min=%d, 0x%x, 0x%x\n", p_min_back, min_back, p_min_back->c.x, p_min_back->c.y);
912<
913<               // termination condition: heap is empty
914<               if (!p_min_back )
915<               {
916<                       dbg(1,"s-t heap empty\n");
917<                       break;
918<               }
919<
920<               // Costs to reach this point:
921<               min_back = p_min_back->value;
922<
923<               common_point->value_p_min_back = min_back;
924<               //pthread_mutex_lock(&lock_common_point);
925<               // termination condition: found shortest possible route from source to target
926<               mu = common_point->mu;
927<               if ( mu < (common_point->value_p_min + min_back))
928<               {
929<                       dbg(1,"%d < %d\n", mu, (common_point->value_p_min + min_back));
930<                       //pthread_mutex_unlock(&lock_common_point);
931<                       break;
932<               }
933<               //pthread_mutex_unlock(&lock_common_point);
934<
935< //            if (debug_route)
936< //            {
937< //                    printf("extract p_back=%p free el=%p min_back=%d, 0x%x, 0x%x\n", p_min_back, p_min_back->el, min_back, p_min_back->c.x, p_min_back->c.y);
938< //            }
939<
940<               p_min_back->el_back=NULL; /* This point is permanently calculated now, we've taken it out of the heap */
941<               s_back = p_min_back->start;
942<               //dbg(dbg_level,"backward step: outgoing edges\n");
943<
944<               while (s_back) { /* Iterating all the segments leading away from our point to update the points at their ends */
945<                       val_back = route_value_seg(profile, p_min_back, s_back, 1);
946<                       if (val_back != INT_MAX && item_is_equal(s_back->data.item, p_min_back->fromseg->data.item)) {
947<                               if (profile->turn_around_penalty2)
948<                                       val_back += profile->turn_around_penalty2;
949<                               else
950<                                       val_back = INT_MAX;
951<                       }
952<                       if (val_back != INT_MAX)
953<                       {
954<                               new_back = min_back + val_back;
955<                               if(s_back->end->seg != NULL)
956<                               {
957<                                       //dbg(dbg_level,"found common point\n");
958<                                       mu_tmp = new_back + s_back->end->value;
959<                                       pthread_mutex_lock(&lock_common_point);
960<                                       if (mu_tmp <  common_point->mu)
961<                                       {
962<                                               common_point->mu = mu_tmp;
963<                                               common_point->pointref = s_back->end;
964<                                               s_back->end->fromseg = s_back;
965<                                               //dbg(dbg_level,"costs for common point: %d\n", mu);
966<                                       }
967<                                       pthread_mutex_unlock(&lock_common_point);
968<                               }
969<                               else
970<                               {
971<                                       if (new_back < s_back->end->value) { /* We've found a less costly way to reach the end of s, update it */
972<                                               s_back->end->value = new_back;
973<                                               s_back->end->fromseg = s_back;
974<
975<                                               if (! s_back->end->el_back)
976<                                               {
977< //                                                    if (debug_route)
978< //                                                            printf("insert_end p_back=%p el_back=%p val_back=%d ", s_back->end, s_back->end->el_back, s_back->end->value);
979<                                                       s_back->end->el_back = fh_insertkey(heap_back, new_back, s_back->end);
980<                                               }
981<                                               else
982<                                               {
983< //                                                    if (debug_route)
984< //                                                            printf("replaced_end p_back=%p el_back=%p val_back=%d ", s_back->end, s_back->end->el_back, s_back->end->value);
985<                                                       fh_replacekey(heap_back, s_back->end->el_back, new_back);
986<                                               }
987<                                       }
988<                               }
989<                       }
990<                       s_back=s_back->start_next;
991<               } // end while loop
992<
993<               //dbg(dbg_level,"backward step: incoming edges\n");
994<               s_back=p_min_back->end;
995<               while (s_back) { /* Doing the same as above with the segments leading towards our point */
996<                       val_back = route_value_seg(profile, p_min_back, s_back, -1);
997<                       if (val_back != INT_MAX && item_is_equal(s_back->data.item, p_min_back->fromseg->data.item)) {
998<                               if (profile->turn_around_penalty2)
999<                                       val_back += profile->turn_around_penalty2;
1000<                               else
1001<                                       val_back = INT_MAX;
1002<                       }
1003<                       if (val_back != INT_MAX) {
1004<                               new_back = min_back + val_back;
1005<                               if(s_back->start->seg != NULL)
1006<                               {
1007<                                       //dbg(dbg_level,"found common point\n");
1008<                                       mu_tmp = new_back + s_back->start->value;
1009<                                       pthread_mutex_lock(&lock_common_point);
1010<                                       if (mu_tmp <  common_point->mu)
1011<                                       {
1012<                                               common_point->mu = mu_tmp;
1013<                                               common_point->pointref = s_back->start;
1014<                                               s_back->start->fromseg = s_back;
1015<                                               //dbg(dbg_level,"costs for common point: %d\n", mu);
1016<                                       }
1017<                                       pthread_mutex_unlock(&lock_common_point);
1018<
1019<                               }
1020<                               else
1021<                               {
1022<                                       if (new_back < s_back->start->value) {
1023<                                               s_back->start->fromseg = s_back;
1024<                                               s_back->start->value = new_back;
1025<
1026<                                               if (! s_back->start->el_back)
1027<                                               {
1028< //                                                    if (debug_route)
1029< //                                                            printf("insert_start p_back=%p el_back=%p val_back=%d ", s_back->start, s_back->start->el_back, s_back->start->value);
1030<                                                       s_back->start->el_back = fh_insertkey(heap_back, new_back, s_back->start);
1031<                                               }
1032<                                               else
1033<                                               {
1034< //                                                    if (debug_route)
1035< //                                                            printf("replaced_start p_back=%p el_back=%p val_back=%d ", s_back->start, s_back->start->el_back, s_back->start->value);
1036<                                                       fh_replacekey(heap_back, s_back->start->el_back, new_back);
1037<                                               }
1038<                                       }
1039<                               }
1040<                       }
1041<                       s_back=s_back->end_next;
1042<               } // end while loop
1043<       } // end for loop
1044<
1045<       //=====================================
1046<       fh_deleteheap(heap_back);
1047<
1048<       return 0;
1049< }
1050<
1051< static int
1052< flood_bidirectional_target_to_source(void *attr)
1053< {
1054<       struct route_graph_flood_bidirectional_threadstruct *st_attr = (struct route_graph_flood_bidirectional_threadstruct*)attr;
1055<       struct route_graph *this = st_attr->graph;
1056<       struct route_info *dst = st_attr->dst;
1057<       struct vehicleprofile *profile = st_attr->profile;
1058<       struct route_graph_common_point *common_point = st_attr->common_point;
1059<
1060<       struct route_graph_point *p_min = NULL;
1061<       struct route_graph_segment *s=NULL;
1062<       int min, new,val;
1063<       struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs from target to source*/
1064<       int mu_tmp, mu;
1065<
1066<       mu_tmp = INT_MAX;
1067<       mu = INT_MAX;
1068<       min = INT_MAX;
1069<       // heap for Target to CommonPoint
1070<       heap = fh_makekeyheap();
1071<
1072<       // Fill Heap initial with first points reachable by initial segment
1073<       // where target is located on and get local costs for this partial segment(s) / Position within this Segment
1074<       while ((s=route_graph_get_segment(this, dst->street, s))) {
1075<               val=route_value_seg(profile, NULL, s, -1);
1076<               if (val != INT_MAX)
1077<               {
1078<                       val=val*(100-dst->percent)/100;
1079<                       s->end->seg=s;
1080<                       s->end->value=val;
1081<                       s->end->el=fh_insertkey(heap, s->end->value, s->end);
1082<                       st_attr->initial_point = s->end;
1083<                       min = val;
1084<               }
1085<               val=route_value_seg(profile, NULL, s, 1);
1086<               if (val != INT_MAX)
1087<               {
1088<                       val=val*dst->percent/100;
1089<                       s->start->seg=s;
1090<                       s->start->value=val;
1091<                       s->start->el=fh_insertkey(heap, s->start->value, s->start);
1092<
1093<                       if(val < min)
1094<                       {
1095<                               st_attr->initial_point = s->start;
1096<                       }
1097<               }
1098<       }
1099<
1100<       min = INT_MAX;
1101<       //=====================================
1102<
1103<       /* infinite loop */
1104<       for (;;) {
1105<
1106<               p_min = fh_extractmin(heap); /* Starting Dijkstra by selecting the point with the minimum costs on the heap */
1107<               //dbg(dbg_level,"extract p=%p min=%d, 0x%x, 0x%x\n", p_min, min, p_min->c.x, p_min->c.y);
1108<
1109<               // termination consition: both heaps are empty
1110<               if (!p_min)
1111<               {
1112<                       dbg(1,"t-s heap empty\n");
1113<                       break;
1114<               }
1115<
1116<               min=p_min->value;
1117<
1118<               common_point->value_p_min = min;
1119<               //pthread_mutex_lock(&lock_common_point);
1120<               // termination condition: found shortest possible route from source to target
1121<               mu = common_point->mu;
1122<               if ( mu < (common_point->value_p_min_back + min))
1123<               {
1124<                       dbg(1,"%d < %d\n", mu, (common_point->value_p_min_back + min));
1125<                       //pthread_mutex_unlock(&lock_common_point);
1126<                       break;
1127<               }
1128<               //pthread_mutex_unlock(&lock_common_point);
1129<
1130<
1131< //            if (debug_route)
1132< //            {
1133< //                    printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
1134< //            }
1135<
1136<               p_min->el = NULL; /* This point is permanently calculated now, we've taken it out of the heap */
1137<               s = p_min->start;
1138<
1139<               // Outgoing Egdes:
1140<               while (s) { /* Iterating all the segments leading away from our point to update the points at their ends */
1141<                       val = route_value_seg(profile, p_min, s, -1);
1142<                       if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
1143<                               if (profile->turn_around_penalty2)
1144<                                       val+=profile->turn_around_penalty2;
1145<                               else
1146<                                       val=INT_MAX;
1147<                       }
1148<                       if (val != INT_MAX) {
1149<                               new=min+val;
1150< //                            if (debug_route)
1151< //                                    printf("begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
1152<                               if(s->end->fromseg != NULL)
1153<                               {
1154<                                       //dbg(dbg_level,"found common point\n");
1155<                                       mu_tmp = new + s->end->value;
1156<                                       pthread_mutex_lock(&lock_common_point);
1157<                                       if (mu_tmp < common_point->mu)
1158<                                       {
1159<                                               common_point->mu = mu_tmp;
1160<                                               s->end->seg=s;
1161<                                               common_point->pointref = s->end;
1162<                                               //dbg(dbg_level,"costs for common point: %d\n", mu);
1163<                                       }
1164<                                       pthread_mutex_unlock(&lock_common_point);
1165<                               }
1166<                               else
1167<                               {
1168<                                       if (new < s->end->value) { /* We've found a less costly way to reach the end of s, update it */
1169<                                               s->end->seg=s;
1170<                                               s->end->value=new;
1171<
1172<                                               if (! s->end->el) {
1173< //                                                    if (debug_route)
1174< //                                                            printf("insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
1175<                                                       s->end->el=fh_insertkey(heap, new, s->end);
1176< //                                                    if (debug_route)
1177< //                                                            printf("el new=%p\n", s->end->el);
1178<                                               }
1179<                                               else {
1180< //                                                    if (debug_route)
1181< //                                                            printf("replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
1182<                                                       fh_replacekey(heap, s->end->el, new);
1183<                                               }
1184<                                       }
1185<                               }
1186< //                            if (debug_route)
1187< //                                    printf("\n");
1188<                       }
1189<                       s=s->start_next;
1190<               } // end while loop
1191<
1192<               s = p_min->end;
1193<               // Incoming Egdes:
1194<               while (s) { /* Doing the same as above with the segments leading towards our point */
1195<                       val = route_value_seg(profile, p_min, s, 1);
1196<                       if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
1197<                               if (profile->turn_around_penalty2)
1198<                                       val+=profile->turn_around_penalty2;
1199<                               else
1200<                                       val=INT_MAX;
1201<                       }
1202<                       if (val != INT_MAX) {
1203<                               new = min + val;
1204< //                            if (debug_route)
1205< //                                    printf("end %d len %d vs %d (0x%x,0x%x)\n",new,val,s->start->value,s->start->c.x, s->start->c.y);
1206<                               if(s->start->fromseg != NULL)
1207<                               {
1208<                                       //dbg(dbg_level,"found common point\n");
1209<                                       mu_tmp = new + s->start->value;
1210<                                       pthread_mutex_lock(&lock_common_point);
1211<                                       if (mu_tmp < common_point->mu)
1212<                                       {
1213<                                               common_point->mu = mu_tmp;
1214<                                               s->start->seg=s;
1215<                                               common_point->pointref = s->start;
1216<                                               //dbg(dbg_level,"costs for common point: %d\n", mu);
1217<                                       }
1218<                                       pthread_mutex_unlock(&lock_common_point);
1219<                               }
1220<                               else
1221<                               {
1222<                                       if (new < s->start->value) {
1223<                                               s->start->value=new;
1224<                                               s->start->seg=s;
1225<                                               if (! s->start->el) {
1226< //                                                    if (debug_route)
1227< //                                                            printf("insert_start p=%p el=%p val=%d ", s->start, s->start->el, s->start->value);
1228<                                                       s->start->el=fh_insertkey(heap, new, s->start);
1229< //                                                    if (debug_route)
1230< //                                                            printf("el new=%p\n", s->start->el);
1231<                                               }
1232<                                               else {
1233< //                                                    if (debug_route)
1234< //                                                            printf("replace_start p=%p el=%p val=%d\n", s->start, s->start->el, s->start->value);
1235<                                                       fh_replacekey(heap, s->start->el, new);
1236<                                               }
1237<                                       }
1238<                               }
1239< //                            if (debug_route)
1240< //                                    printf("\n");
1241<                       }
1242<                       s=s->end_next;
1243<               } // end while loop
1244<       } // end for loop
1245<
1246<       //=====================================
1247<
1248<       fh_deleteheap(heap);
1249<       return 0;
1250< }
1251<
1252<
1253<
1254< /**
1255<  * @brief calculate costs from source to target over common_point and set ref
1256<  * to maintain functionality of other routines
1257<  *
1258<  * following start->fromseg, which indicates the least costly way
1259<  * to reach the source from the common point and save taken path
1260<  * by setting ->seg, summarize costs and also summarize costs from
1261<  * common point to destination by following common_point->ref
1262<  *
1263<  *
1264<  * @param src_point The starting point
1265<  * @param src position of user
1266<  * @param common_point The common point of both searches
1267<  * @param dst position of destination
1268<  * @param destination_point The destination point
1269<  * @return mu, the summarized costs
1270<  */
1271<
1272< static int
1273< calculate_costs_and_set_refs(struct vehicleprofile *profile, struct route_graph_point *src_point, struct route_info *src, struct route_graph_point * common_point ,struct route_graph_point * dst_point, struct route_info *dst)
1274< {
1275<       int dbg_level = 1;
1276<       int mu;
1277<       struct route_graph_point *  curr;
1278<       struct route_graph_segment * s;
1279<
1280<       dbg(dbg_level,"traversing from common point to source\n");
1281<
1282<       if(!common_point || !common_point->fromseg)
1283<       {
1284<               return INT_MAX;
1285<       }
1286<
1287<       if(!src_point || !dst_point)
1288<       {
1289<               dbg(dbg_level,"source or destination point not set\n");
1290<               return INT_MAX;
1291<       }
1292<
1293<       s = common_point->fromseg;
1294<       curr = common_point;
1295<
1296<       //while(curr->fromseg) {
1297<       while(s) {
1298<
1299<               //dbg(dbg_level, "curr %p - s->seg %p [ S: %p, E: %p ]\n", curr, curr->fromseg, curr->fromseg->start, curr->fromseg->end);
1300<
1301<               if(curr == s->start )
1302<               {
1303<                       curr = s->end;
1304<                       s->end->seg = s;
1305<
1306<               }else{
1307<                       curr = s->start;
1308<                       s->start->seg = s;
1309<               }
1310<
1311<               // reached source?
1312< //            if( item_is_equal(src->street->item, s->data.item) )
1313< //            {
1314< //                    src_point->seg = s;
1315< //                    dbg(dbg_level, "reached Source\n");
1316< //                    break;
1317< //            }
1318<
1319<               if( src_point == curr )
1320<               {
1321<                       dbg(dbg_level, "setting ref reached Source\n");
1322<                       break;
1323<               }
1324<
1325<               // avoid loop:
1326<               if( item_is_equal(curr->fromseg->data.item, s->data.item) )
1327<               {
1328<                       dbg(dbg_level, "setting ref detected loop!\n");
1329<                       break;
1330<               }
1331<
1332<               s = curr->fromseg;
1333<       }
1334<
1335< #if 0
1336<       // TEST Travelling from Source to Target:
1337<       curr = src_point;
1338<       s = src_point->seg;
1339<
1340<       while (s) {
1341<
1342<               //dbg(dbg_level, "curr %p - s->seg %p [ S: %p, E: %p ]\n", curr, curr->seg, curr->seg->start, curr->seg->end);
1343<
1344<               // reached Destination?
1345<               if( item_is_equal(dst->street->item, s->data.item) )
1346<               {
1347<                       dbg(dbg_level, "TEST: reached Destination\n");
1348<                       break;
1349<               }
1350<
1351<               if(curr == s->start )
1352<               {
1353<                       curr = s->end;
1354<
1355<               }else{
1356<                       curr = s->start;
1357<               }
1358<
1359<               // avoid loop:
1360<               if( item_is_equal(curr->seg->data.item, s->data.item) )
1361<               {
1362<                       dbg(dbg_level, "TEST: loop detected!\n");
1363<                       break;
1364<               }
1365<
1366<               s = curr->seg;
1367<       }
1368< #endif
1369<
1370<       // summarize costs from destination to one step before common point
1371<       // common point is always found by both searches but has costs of
1372<       // search starting from Source
1373<       if(common_point == common_point->seg->start)
1374<       {
1375<               mu = common_point->seg->end->value;
1376<       }else{
1377<               mu = common_point->seg->start->value;
1378<       }
1379<
1380<       // costs from common point one step in direction destination
1381<       mu += route_value_seg(profile, NULL, common_point->seg, 1);
1382<       //mu += route_value_seg(profile, common_point, common_point->seg, 1);
1383<
1384<       // summarize costs from source to common point
1385<       mu += common_point->value;
1386<
1387<       dbg(dbg_level, "Costs of Path mu = %d\n", mu);
1388<       return mu;
1389< }
1390<
1391< /**
1392<  * @brief sets default navit routing implementation
1393<  *
1394<  *
1395<  * @return int In every case 1
1396<  */
1397< int
1398< use_standard_routing()
1399< {
1400<       useBidirectionalRouting = 0;
1401<       useBidirectionalMultiThread = 0;
1402<       return 1;
1403< }
1404<
1405< /**
1406<  * @brief sets singlethreaded bidirectional dijkstra implementation as routing algorithm
1407<  *
1408<  *
1409<  * @return int In every case 1
1410<  */
1411< int
1412< use_bidirectional_routing_1Thread()
1413< {
1414<       useBidirectionalRouting = 1;
1415<       useBidirectionalMultiThread = 0;
1416<       return 1;
1417< }
1418<
1419< /**
1420<  * @brief sets bidirectional dijkstra implementation with two threads as routing algorithm
1421<  *
1422<  *
1423<  * @return int In every case 1
1424<  */
1425< int
1426< use_bidirectional_routing_2Threads()
1427< {
1428<       // ToDo: check if more than one Core is available
1429<       useBidirectionalRouting = 1;
1430<       useBidirectionalMultiThread = 1;
1431<       return 1;
1432< }
1433<
1434<
14353992,3993c2608
1436<                       //route_graph_flood(this, dst, profile, NULL);
1437<                       route_graph_flood(this, dst, profile, NULL, pos);
1438---
1439>                       route_graph_flood(this, dst, profile, NULL);
14404009d2623
1441<               //printf("s %p - s->seg %p [ S: %p, E: %p ]\n", start, start->fromseg, start->fromseg->start, start->fromseg->end);
14424011,4012c2625
1443<
1444<               if (s->start == start) {
1445---
1446>               if (s->start == start) {               
14474037d2649
1448<       //timestamp_build_next_map = now_ms();
14494045,4047c2657
1450<
1451<       //dbg(0, "took %.2lf\n", now_ms() - timestamp_build_next_map);
1452<
1453---
1454>               
14554190d2799
1456<       // double timestamp_restrictions = now_ms();
14574202d2810
1458<       // dbg(0," took: %.2f ms\n", now_ms()-timestamp_restrictions);
14594208,4209d2815
1460<       dbg(0,"build took: %.2f ms\n", now_ms()-timestamp_build);
1461<
14624233,4234d2838
1463<       //double tmptimestamp = now_ms();
1464<
14654239c2843
1466<               for (;;) {
1467---
1468>               for (;;) {     
14694256,4257d2859
1470<
1471<       //dbg(0," took: %.2f ms\n", now_ms()-tmptimestamp);
14724279,4281d2880
1473<
1474<       double tmptimestamp = (double)0.0;
1475<
14764283,4287d2881
1477<       dbg(1,"enter\n");
1478<
1479<       tmptimestamp = now_ms();
1480<       ret->sel=route_calc_selection(c, count, profile); // 0.01 ms
1481<       dbg(0,"route_calc_selection() took: %.2f ms\n", now_ms()-tmptimestamp);
14824288a2883
1483>       dbg(1,"enter\n");
14844290,4293c2885,2886
1485<       tmptimestamp = now_ms();
1486<       ret->h=mapset_open(ms); // 0.00 ms
1487<       dbg(0,"mapset_open() took: %.2f ms\n", now_ms()-tmptimestamp);
1488<
1489---
1490>       ret->sel=route_calc_selection(c, count, profile);
1491>       ret->h=mapset_open(ms);
14924296d2888
1493<
14944302c2894
1495<       } else {
1496---
1497>       } else
14984305,4307d2896
1499<               dbg(0," took: %.2f ms\n", now_ms()-timestamp_build);
1500<       }
1501<
15024314,4316c2903
1503<       // dbg(0," took: %.2f ms\n", now_ms()-timestamp_update);
1504<       //route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, cb);
1505<       route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, cb, this->pos);
1506---
1507>       route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, cb);
15084330,4332d2916
1509<
1510<       timestamp_update = now_ms();
1511<
15124352,4354d2935
1513<
1514<       timestamp_build = now_ms();
1515<
15164357c2938
1517<               while (this->graph->busy)
1518---
1519>               while (this->graph->busy)
15204360,4361d2940
1521<       
1522<       // dbg(0," took: %.2f ms\n", now_ms()-timestamp_update);
15235495a4075
1524>       g_free(this_);