Routino SVN Repository Browser

Check out the latest version of Routino: svn co http://routino.org/svn/trunk routino

/[routino]/trunk/src/optimiser.c
ViewVC logotype

Annotation of /trunk/src/optimiser.c

Parent Directory Parent Directory | Revision Log Revision Log

Revision 47 - (hide annotations) (download) (as text)
Sun Jan 18 09:06:29 2009 UTC (16 years, 2 months ago) by amb
File MIME type: text/x-csrc
File size: 34915 byte(s) 
Fix bugs when start and/or finish nodes are supernodes.
1 amb 2 /***************************************
2 amb 47 $Header: /home/amb/CVS/routino/src/optimiser.c,v 1.17 2009-01-18 09:06:29 amb Exp $
3 amb 2
4     Routing optimiser.
5     ******************/ /******************
6     Written by Andrew M. Bishop
7    
8 amb 3 This file Copyright 2008,2009 Andrew M. Bishop
9 amb 2 It may be distributed under the GNU Public License, version 2, or
10     any higher version. See section COPYING of the GNU Public license
11     for conditions under which this file may be redistributed.
12     ***************************************/
13    
14    
15 amb 11 #include <assert.h>
16 amb 2 #include <string.h>
17     #include <stdlib.h>
18    
19 amb 26 #include "nodes.h"
20     #include "ways.h"
21     #include "segments.h"
22 amb 28 #include "results.h"
23 amb 2 #include "functions.h"
24    
25 amb 26
26 amb 11 #define INCREMENT 1024
27 amb 2
28 amb 10
29 amb 2 /*+ A queue results. +*/
30     typedef struct _Queue
31     {
32 amb 45 uint32_t alloced; /*+ The amount of space allocated for results in the array. +*/
33     uint32_t number; /*+ The number of occupied results in the array. +*/
34     uint32_t *queue; /*+ An array of offsets into the results array. +*/
35 amb 2 }
36     Queue;
37    
38    
39     /*+ The queue of nodes. +*/
40 amb 45 Queue OSMQueue={0,0,NULL};
41 amb 2
42 amb 31 /*+ Print the progress? +*/
43     int print_progress=1;
44 amb 2
45 amb 31
46 amb 2 /* Functions */
47    
48 amb 45 static void insert_in_queue(Results *results,Result *result);
49 amb 2
50    
51     /*++++++++++++++++++++++++++++++++++++++
52     Find the optimum route between two nodes.
53    
54 amb 26 Results *FindRoute Returns a set of results.
55    
56     Nodes *nodes The set of nodes to use.
57    
58     Segments *segments The set of segments to use.
59    
60 amb 2 node_t start The start node.
61    
62     node_t finish The finish node.
63     ++++++++++++++++++++++++++++++++++++++*/
64    
65 amb 31 Results *FindRoute(Nodes *nodes,Segments *segments,node_t start,node_t finish)
66 amb 2 {
67 amb 31 Results *results;
68 amb 43 node_t node1,node2;
69 amb 10 HalfResult shortest1,quickest1;
70     HalfResult shortest2,quickest2;
71     HalfResult shortestfinish,quickestfinish;
72 amb 2 Result *result1,*result2;
73     Segment *segment;
74     int nresults=0;
75    
76 amb 10 /* Set up the finish conditions */
77    
78 amb 36 shortestfinish.distance=INVALID_DISTANCE;
79     shortestfinish.duration=INVALID_DURATION;
80     quickestfinish.distance=INVALID_DISTANCE;
81     quickestfinish.duration=INVALID_DURATION;
82 amb 10
83 amb 2 /* Insert the first node into the queue */
84    
85 amb 31 results=NewResultsList();
86 amb 2
87 amb 31 result1=InsertResult(results,start);
88 amb 28
89 amb 2 result1->node=start;
90 amb 43 result1->shortest.prev=0;
91     result1->shortest.next=0;
92 amb 10 result1->shortest.distance=0;
93     result1->shortest.duration=0;
94 amb 43 result1->quickest.prev=0;
95     result1->quickest.next=0;
96 amb 10 result1->quickest.distance=0;
97     result1->quickest.duration=0;
98 amb 2
99 amb 45 insert_in_queue(results,result1);
100 amb 2
101     /* Loop across all nodes in the queue */
102    
103 amb 45 while(OSMQueue.number>0)
104 amb 2 {
105 amb 45 result1=LookupResult(results,OSMQueue.queue[--OSMQueue.number]);
106 amb 43 node1=result1->node;
107 amb 10 shortest1.distance=result1->shortest.distance;
108     shortest1.duration=result1->shortest.duration;
109     quickest1.distance=result1->quickest.distance;
110     quickest1.duration=result1->quickest.duration;
111 amb 2
112 amb 43 segment=FindFirstSegment(segments,node1);
113 amb 2
114     while(segment)
115     {
116     node2=segment->node2;
117    
118 amb 36 if(segment->distance==INVALID_SHORT_DISTANCE ||
119     segment->duration==INVALID_SHORT_DURATION)
120 amb 31 goto endloop;
121    
122 amb 10 shortest2.distance=shortest1.distance+segment->distance;
123     shortest2.duration=shortest1.duration+segment->duration;
124     quickest2.distance=quickest1.distance+segment->distance;
125     quickest2.duration=quickest1.duration+segment->duration;
126 amb 2
127 amb 10 if(shortest2.distance>shortestfinish.distance && quickest2.duration>quickestfinish.duration)
128 amb 2 goto endloop;
129    
130 amb 42 result2=FindResult(results,node2);
131    
132 amb 2 if(!result2) /* New end node */
133     {
134 amb 31 result2=InsertResult(results,node2);
135 amb 2 result2->node=node2;
136 amb 43 result2->shortest.prev=node1;
137     result2->shortest.next=0;
138 amb 10 result2->shortest.distance=shortest2.distance;
139     result2->shortest.duration=shortest2.duration;
140 amb 43 result2->quickest.prev=node1;
141     result2->quickest.next=0;
142 amb 10 result2->quickest.distance=quickest2.distance;
143     result2->quickest.duration=quickest2.duration;
144 amb 2
145     nresults++;
146    
147     if(node2==finish)
148     {
149 amb 10 shortestfinish.distance=shortest2.distance;
150     shortestfinish.duration=shortest2.duration;
151     quickestfinish.distance=quickest2.distance;
152     quickestfinish.duration=quickest2.duration;
153 amb 2 }
154     else
155 amb 45 insert_in_queue(results,result2);
156 amb 2 }
157     else
158     {
159 amb 10 if(shortest2.distance<result2->shortest.distance ||
160     (shortest2.distance==result2->shortest.distance &&
161     shortest2.duration<result2->shortest.duration)) /* New end node is shorter */
162 amb 2 {
163 amb 43 result2->shortest.prev=node1;
164 amb 10 result2->shortest.distance=shortest2.distance;
165     result2->shortest.duration=shortest2.duration;
166 amb 2
167     if(node2==finish)
168     {
169 amb 10 shortestfinish.distance=shortest2.distance;
170     shortestfinish.duration=shortest2.duration;
171 amb 2 }
172     else
173 amb 45 insert_in_queue(results,result2);
174 amb 2 }
175    
176 amb 10 if(quickest2.duration<result2->quickest.duration ||
177     (quickest2.duration==result2->quickest.duration &&
178     quickest2.distance<result2->quickest.distance)) /* New end node is quicker */
179 amb 2 {
180 amb 43 result2->quickest.prev=node1;
181 amb 10 result2->quickest.distance=quickest2.distance;
182     result2->quickest.duration=quickest2.duration;
183 amb 2
184     if(node2==finish)
185     {
186 amb 10 quickestfinish.distance=quickest2.distance;
187     quickestfinish.duration=quickest2.duration;
188 amb 2 }
189     else
190 amb 45 insert_in_queue(results,result2);
191 amb 2 }
192     }
193    
194     endloop:
195    
196 amb 26 segment=FindNextSegment(segments,segment);
197 amb 2 }
198    
199 amb 31 if(print_progress && !(nresults%1000))
200 amb 2 {
201 amb 45 printf("\rRouting: End Nodes=%d Queue=%d Journey=%.1fkm,%.0fmin ",nresults,OSMQueue.number,
202 amb 10 distance_to_km(shortest2.distance),duration_to_minutes(quickest2.duration));
203 amb 2 fflush(stdout);
204     }
205     }
206    
207 amb 31 if(print_progress)
208     {
209     printf("\rRouted: End Nodes=%d Shortest=%.1fkm,%.0fmin Quickest=%.1fkm,%.0fmin\n",nresults,
210     distance_to_km(shortestfinish.distance),duration_to_minutes(shortestfinish.duration),
211     distance_to_km(quickestfinish.distance),duration_to_minutes(quickestfinish.duration));
212     fflush(stdout);
213     }
214    
215     /* Reverse the results */
216    
217     result2=FindResult(results,finish);
218    
219     do
220     {
221 amb 43 if(result2->shortest.prev)
222 amb 31 {
223 amb 43 node_t node1=result2->shortest.prev;
224 amb 31
225     result1=FindResult(results,node1);
226    
227 amb 43 result1->shortest.next=result2->node;
228 amb 31
229     result2=result1;
230     }
231     else
232     result2=NULL;
233     }
234     while(result2);
235    
236     result2=FindResult(results,finish);
237    
238     do
239     {
240 amb 43 if(result2->quickest.prev)
241 amb 31 {
242 amb 43 node_t node1=result2->quickest.prev;
243 amb 31
244     result1=FindResult(results,node1);
245    
246 amb 43 result1->quickest.next=result2->node;
247 amb 31
248     result2=result1;
249     }
250     else
251     result2=NULL;
252     }
253     while(result2);
254    
255     return(results);
256 amb 2 }
257    
258    
259     /*++++++++++++++++++++++++++++++++++++++
260 amb 34 Find the optimum route between two nodes.
261    
262     Results *FindRoute3 Returns a set of results.
263    
264     Nodes *nodes The set of nodes to use.
265    
266     Segments *segments The set of segments to use.
267    
268     node_t start The start node.
269    
270     node_t finish The finish node.
271    
272     Results *begin The initial portion of the route.
273    
274     Results *end The final portion of the route.
275     ++++++++++++++++++++++++++++++++++++++*/
276    
277     Results *FindRoute3(Nodes *nodes,Segments *segments,node_t start,node_t finish,Results *begin,Results *end)
278     {
279     Results *results;
280 amb 43 node_t node1,node2;
281 amb 34 HalfResult shortest1,quickest1;
282     HalfResult shortest2,quickest2;
283     HalfResult shortestfinish,quickestfinish;
284     Result *result1,*result2,*result3;
285     Segment *segment;
286     int nresults=0;
287 amb 45 int j;
288 amb 34
289     /* Set up the finish conditions */
290    
291 amb 36 shortestfinish.distance=INVALID_DISTANCE;
292     shortestfinish.duration=INVALID_DURATION;
293     quickestfinish.distance=INVALID_DISTANCE;
294     quickestfinish.duration=INVALID_DURATION;
295 amb 34
296 amb 47 /* Insert the start nodes */
297 amb 34
298     results=NewResultsList();
299    
300     result1=InsertResult(results,start);
301     result2=FindResult(begin,start);
302    
303     *result1=*result2;
304    
305     /* Insert the finish points of the beginning part of the path into the queue */
306    
307 amb 45 for(j=0;j<begin->number;j++)
308     if(FindNode(nodes,begin->results[j].node))
309     {
310     if(!(result1=FindResult(results,begin->results[j].node)))
311 amb 34 {
312 amb 45 result1=InsertResult(results,begin->results[j].node);
313 amb 34
314 amb 45 *result1=begin->results[j];
315 amb 34
316 amb 45 result1->shortest.prev=start;
317     result1->quickest.prev=start;
318 amb 47 }
319 amb 34
320 amb 47 insert_in_queue(results,result1);
321 amb 45 }
322 amb 34
323     /* Loop across all nodes in the queue */
324    
325 amb 45 while(OSMQueue.number>0)
326 amb 34 {
327 amb 45 result1=LookupResult(results,OSMQueue.queue[--OSMQueue.number]);
328 amb 43 node1=result1->node;
329 amb 34 shortest1.distance=result1->shortest.distance;
330     shortest1.duration=result1->shortest.duration;
331     quickest1.distance=result1->quickest.distance;
332     quickest1.duration=result1->quickest.duration;
333    
334 amb 43 segment=FindFirstSegment(segments,node1);
335 amb 34
336     while(segment)
337     {
338     node2=segment->node2;
339    
340 amb 36 if(segment->distance==INVALID_SHORT_DISTANCE ||
341     segment->duration==INVALID_SHORT_DURATION)
342 amb 34 goto endloop;
343    
344     shortest2.distance=shortest1.distance+segment->distance;
345     shortest2.duration=shortest1.duration+segment->duration;
346     quickest2.distance=quickest1.distance+segment->distance;
347     quickest2.duration=quickest1.duration+segment->duration;
348    
349     if(shortest2.distance>shortestfinish.distance && quickest2.duration>quickestfinish.duration)
350     goto endloop;
351    
352 amb 42 result2=FindResult(results,node2);
353    
354 amb 34 if(!result2) /* New end node */
355     {
356     result2=InsertResult(results,node2);
357     result2->node=node2;
358 amb 43 result2->shortest.prev=node1;
359     result2->shortest.next=0;
360 amb 34 result2->shortest.distance=shortest2.distance;
361     result2->shortest.duration=shortest2.duration;
362 amb 43 result2->quickest.prev=node1;
363     result2->quickest.next=0;
364 amb 34 result2->quickest.distance=quickest2.distance;
365     result2->quickest.duration=quickest2.duration;
366    
367     nresults++;
368    
369     if((result3=FindResult(end,node2)))
370     {
371     if((shortest2.distance+result3->shortest.distance)<shortestfinish.distance ||
372     ((shortest2.distance+result3->shortest.distance)==shortestfinish.distance &&
373     (shortest2.duration+result3->shortest.duration)<shortestfinish.duration))
374     {
375     shortestfinish.distance=shortest2.distance+result3->shortest.distance;
376     shortestfinish.duration=shortest2.duration+result3->shortest.duration;
377     }
378     if((quickest2.duration+result3->quickest.duration)<quickestfinish.duration ||
379     ((quickest2.duration+result3->quickest.duration)==quickestfinish.duration &&
380     (quickest2.distance+result3->quickest.distance)<quickestfinish.distance))
381     {
382     quickestfinish.distance=quickest2.distance+result3->quickest.distance;
383     quickestfinish.duration=quickest2.duration+result3->quickest.duration;
384     }
385     }
386     else
387 amb 45 insert_in_queue(results,result2);
388 amb 34 }
389     else
390     {
391     if(shortest2.distance<result2->shortest.distance ||
392     (shortest2.distance==result2->shortest.distance &&
393     shortest2.duration<result2->shortest.duration)) /* New end node is shorter */
394     {
395 amb 43 result2->shortest.prev=node1;
396 amb 34 result2->shortest.distance=shortest2.distance;
397     result2->shortest.duration=shortest2.duration;
398    
399     if((result3=FindResult(end,node2)))
400     {
401     if((shortest2.distance+result3->shortest.distance)<shortestfinish.distance ||
402     ((shortest2.distance+result3->shortest.distance)==shortestfinish.distance &&
403     (shortest2.duration+result3->shortest.duration)<shortestfinish.duration))
404     {
405     shortestfinish.distance=shortest2.distance+result3->shortest.distance;
406     shortestfinish.duration=shortest2.duration+result3->shortest.duration;
407     }
408     if((quickest2.duration+result3->quickest.duration)<quickestfinish.duration ||
409     ((quickest2.duration+result3->quickest.duration)==quickestfinish.duration &&
410     (quickest2.distance+result3->quickest.distance)<quickestfinish.distance))
411     {
412     quickestfinish.distance=quickest2.distance+result3->quickest.distance;
413     quickestfinish.duration=quickest2.duration+result3->quickest.duration;
414     }
415     }
416     else
417 amb 45 insert_in_queue(results,result2);
418 amb 34 }
419    
420     if(quickest2.duration<result2->quickest.duration ||
421     (quickest2.duration==result2->quickest.duration &&
422     quickest2.distance<result2->quickest.distance)) /* New end node is quicker */
423     {
424 amb 43 result2->quickest.prev=node1;
425 amb 34 result2->quickest.distance=quickest2.distance;
426     result2->quickest.duration=quickest2.duration;
427    
428     if((result3=FindResult(end,node2)))
429     {
430     if((shortest2.distance+result3->shortest.distance)<shortestfinish.distance ||
431     ((shortest2.distance+result3->shortest.distance)==shortestfinish.distance &&
432     (shortest2.duration+result3->shortest.duration)<shortestfinish.duration))
433     {
434     shortestfinish.distance=shortest2.distance+result3->shortest.distance;
435     shortestfinish.duration=shortest2.duration+result3->shortest.duration;
436     }
437     if((quickest2.duration+result3->quickest.duration)<quickestfinish.duration ||
438     ((quickest2.duration+result3->quickest.duration)==quickestfinish.duration &&
439     (quickest2.distance+result3->quickest.distance)<quickestfinish.distance))
440     {
441     quickestfinish.distance=quickest2.distance+result3->quickest.distance;
442     quickestfinish.duration=quickest2.duration+result3->quickest.duration;
443     }
444     }
445     else
446 amb 45 insert_in_queue(results,result2);
447 amb 34 }
448     }
449    
450     endloop:
451    
452     segment=FindNextSegment(segments,segment);
453     }
454    
455     if(print_progress && !(nresults%1000))
456     {
457 amb 45 printf("\rRouting: End Nodes=%d Queue=%d Journey=%.1fkm,%.0fmin ",nresults,OSMQueue.number,
458 amb 34 distance_to_km(shortest2.distance),duration_to_minutes(quickest2.duration));
459     fflush(stdout);
460     }
461     }
462    
463     if(print_progress)
464     {
465     printf("\rRouted: End Nodes=%d Shortest=%.1fkm,%.0fmin Quickest=%.1fkm,%.0fmin\n",nresults,
466     distance_to_km(shortestfinish.distance),duration_to_minutes(shortestfinish.duration),
467     distance_to_km(quickestfinish.distance),duration_to_minutes(quickestfinish.duration));
468     fflush(stdout);
469     }
470    
471     /* Finish off the end part of the route. */
472    
473 amb 47 if(!(result2=FindResult(results,finish)))
474     {
475     result1=InsertResult(results,finish);
476     result2=FindResult(end,finish);
477 amb 34
478 amb 47 *result1=*result2;
479    
480     for(j=0;j<end->number;j++)
481     if(FindNode(nodes,end->results[j].node))
482     if((result1=FindResult(results,end->results[j].node)))
483 amb 34 {
484 amb 47 if((result1->shortest.distance+end->results[j].shortest.distance)<result2->shortest.distance ||
485     ((result1->shortest.distance+end->results[j].shortest.distance)==result2->shortest.distance &&
486     (result1->shortest.duration+end->results[j].shortest.duration)<result2->shortest.duration))
487     {
488     result2->shortest.distance=result1->shortest.distance+end->results[j].shortest.distance;
489     result2->shortest.duration=result1->shortest.duration+end->results[j].shortest.duration;
490     result2->shortest.prev=result1->node;
491     }
492     if((result1->quickest.duration+end->results[j].quickest.duration)<result2->quickest.duration ||
493     ((result1->quickest.duration+end->results[j].quickest.duration)==result2->quickest.duration &&
494     (result1->quickest.distance+end->results[j].quickest.distance)<result2->quickest.distance))
495     {
496     result2->quickest.distance=result1->quickest.distance+end->results[j].quickest.distance;
497     result2->quickest.duration=result1->quickest.duration+end->results[j].quickest.duration;
498     result2->quickest.prev=result1->node;
499     }
500 amb 34 }
501 amb 47 }
502 amb 34
503     /* Reverse the results */
504    
505     result2=FindResult(results,finish);
506    
507     do
508     {
509 amb 43 if(result2->shortest.prev)
510 amb 34 {
511 amb 43 node_t node1=result2->shortest.prev;
512 amb 34
513     result1=FindResult(results,node1);
514    
515 amb 43 result1->shortest.next=result2->node;
516 amb 34
517     result2=result1;
518     }
519     else
520     result2=NULL;
521     }
522     while(result2);
523    
524     result2=FindResult(results,finish);
525    
526     do
527     {
528 amb 43 if(result2->quickest.prev)
529 amb 34 {
530 amb 43 node_t node1=result2->quickest.prev;
531 amb 34
532     result1=FindResult(results,node1);
533    
534 amb 43 result1->quickest.next=result2->node;
535 amb 34
536     result2=result1;
537     }
538     else
539     result2=NULL;
540     }
541     while(result2);
542    
543     return(results);
544     }
545    
546    
547     /*++++++++++++++++++++++++++++++++++++++
548 amb 2 Print the optimum route between two nodes.
549    
550 amb 31 Results *Results The set of results to print.
551    
552     Nodes *nodes The list of nodes.
553    
554 amb 26 Segments *segments The set of segments to use.
555    
556     Ways *ways The list of ways.
557    
558 amb 2 node_t start The start node.
559    
560     node_t finish The finish node.
561     ++++++++++++++++++++++++++++++++++++++*/
562    
563 amb 31 void PrintRoute(Results *results,Nodes *nodes,Segments *segments,Ways *ways,node_t start,node_t finish)
564 amb 2 {
565     FILE *file;
566     Result *result;
567    
568     /* Print the result for the shortest route */
569    
570     file=fopen("shortest.txt","w");
571    
572 amb 31 result=FindResult(results,start);
573 amb 2
574     do
575     {
576 amb 43 Node *node=FindNode(nodes,result->node);
577    
578     if(result->shortest.prev)
579 amb 2 {
580 amb 6 Segment *segment;
581     Way *way;
582 amb 2
583 amb 43 segment=FindFirstSegment(segments,result->shortest.prev);
584 amb 34 while(segment->node2!=result->node)
585 amb 26 segment=FindNextSegment(segments,segment);
586 amb 2
587 amb 26 way=FindWay(ways,segment->way);
588 amb 6
589 amb 43 fprintf(file,"%9.5f %9.5f %5.3f %5.2f %6.1f %5.1f %3d %s\n",node->latitude,node->longitude,
590 amb 5 distance_to_km(segment->distance),duration_to_minutes(segment->duration),
591 amb 34 distance_to_km(result->shortest.distance),duration_to_minutes(result->shortest.duration),
592 amb 31 (way->limit?way->limit:way->speed),WayName(ways,way));
593 amb 34 }
594     else
595 amb 43 fprintf(file,"%9.5f %9.5f %5.3f %5.2f %6.1f %5.1f\n",node->latitude,node->longitude,
596 amb 34 0.0,0.0,0.0,0.0);
597 amb 2
598 amb 43 if(result->shortest.next)
599     result=FindResult(results,result->shortest.next);
600 amb 2 else
601     result=NULL;
602     }
603     while(result);
604    
605     fclose(file);
606    
607     /* Print the result for the quickest route */
608    
609     file=fopen("quickest.txt","w");
610    
611 amb 31 result=FindResult(results,start);
612 amb 2
613     do
614     {
615 amb 43 Node *node=FindNode(nodes,result->node);
616    
617     if(result->quickest.prev)
618 amb 2 {
619 amb 6 Segment *segment;
620     Way *way;
621 amb 2
622 amb 43 segment=FindFirstSegment(segments,result->quickest.prev);
623 amb 34 while(segment->node2!=result->node)
624 amb 26 segment=FindNextSegment(segments,segment);
625 amb 2
626 amb 26 way=FindWay(ways,segment->way);
627 amb 6
628 amb 43 fprintf(file,"%9.5f %9.5f %5.3f %5.2f %6.1f %5.1f %3d %s\n",node->latitude,node->longitude,
629 amb 5 distance_to_km(segment->distance),duration_to_minutes(segment->duration),
630 amb 34 distance_to_km(result->quickest.distance),duration_to_minutes(result->quickest.duration),
631 amb 31 (way->limit?way->limit:way->speed),WayName(ways,way));
632 amb 34 }
633     else
634 amb 43 fprintf(file,"%9.5f %9.5f %5.3f %5.2f %6.1f %5.1f\n",node->latitude,node->longitude,
635 amb 34 0.0,0.0,0.0,0.0);
636 amb 2
637 amb 43 if(result->quickest.next)
638     result=FindResult(results,result->quickest.next);
639 amb 2 else
640     result=NULL;
641     }
642     while(result);
643    
644 amb 31 fclose(file);
645     }
646 amb 3
647    
648 amb 31 /*++++++++++++++++++++++++++++++++++++++
649     Find all routes from a specified node to any node in the specified list.
650 amb 3
651 amb 31 Results *FindRoute2 Returns a set of results.
652    
653     Nodes *nodes The set of nodes to use.
654    
655     Segments *segments The set of segments to use.
656    
657     node_t start The start node.
658    
659     Nodes *finish The finishing nodes.
660     ++++++++++++++++++++++++++++++++++++++*/
661    
662     Results *FindRoutes(Nodes *nodes,Segments *segments,node_t start,Nodes *finish)
663     {
664     Results *results;
665 amb 43 node_t node1,node2;
666 amb 31 HalfResult shortest1,quickest1;
667     HalfResult shortest2,quickest2;
668     Result *result1,*result2;
669     Segment *segment;
670     int nresults=0;
671    
672     /* Insert the first node into the queue */
673    
674     results=NewResultsList();
675    
676     result1=InsertResult(results,start);
677    
678     result1->node=start;
679 amb 43 result1->shortest.prev=0;
680     result1->shortest.next=0;
681 amb 31 result1->shortest.distance=0;
682     result1->shortest.duration=0;
683 amb 43 result1->quickest.prev=0;
684     result1->quickest.next=0;
685 amb 31 result1->quickest.distance=0;
686     result1->quickest.duration=0;
687    
688 amb 45 insert_in_queue(results,result1);
689 amb 31
690     /* Loop across all nodes in the queue */
691    
692 amb 45 while(OSMQueue.number>0)
693 amb 31 {
694 amb 45 result1=LookupResult(results,OSMQueue.queue[--OSMQueue.number]);
695 amb 43 node1=result1->node;
696 amb 31 shortest1.distance=result1->shortest.distance;
697     shortest1.duration=result1->shortest.duration;
698     quickest1.distance=result1->quickest.distance;
699     quickest1.duration=result1->quickest.duration;
700    
701 amb 43 segment=FindFirstSegment(segments,node1);
702 amb 31
703     while(segment)
704     {
705     node2=segment->node2;
706    
707 amb 36 if(segment->distance==INVALID_SHORT_DISTANCE ||
708     segment->duration==INVALID_SHORT_DURATION)
709 amb 31 goto endloop;
710    
711     shortest2.distance=shortest1.distance+segment->distance;
712     shortest2.duration=shortest1.duration+segment->duration;
713     quickest2.distance=quickest1.distance+segment->distance;
714     quickest2.duration=quickest1.duration+segment->duration;
715    
716     result2=FindResult(results,node2);
717    
718     if(!result2) /* New end node */
719     {
720     result2=InsertResult(results,node2);
721     result2->node=node2;
722 amb 43 result2->shortest.prev=node1;
723     result2->shortest.next=0;
724 amb 31 result2->shortest.distance=shortest2.distance;
725     result2->shortest.duration=shortest2.duration;
726 amb 43 result2->quickest.prev=node1;
727     result2->quickest.next=0;
728 amb 31 result2->quickest.distance=quickest2.distance;
729     result2->quickest.duration=quickest2.duration;
730    
731     nresults++;
732    
733     if(!FindNode(finish,node2))
734 amb 45 insert_in_queue(results,result2);
735 amb 31 }
736     else
737     {
738     if(shortest2.distance<result2->shortest.distance ||
739     (shortest2.distance==result2->shortest.distance &&
740     shortest2.duration<result2->shortest.duration)) /* New end node is shorter */
741     {
742 amb 43 result2->shortest.prev=node1;
743 amb 31 result2->shortest.distance=shortest2.distance;
744     result2->shortest.duration=shortest2.duration;
745    
746     if(!FindNode(finish,node2))
747 amb 45 insert_in_queue(results,result2);
748 amb 31 }
749    
750     if(quickest2.duration<result2->quickest.duration ||
751     (quickest2.duration==result2->quickest.duration &&
752     quickest2.distance<result2->quickest.distance)) /* New end node is quicker */
753     {
754 amb 43 result2->quickest.prev=node1;
755 amb 31 result2->quickest.distance=quickest2.distance;
756     result2->quickest.duration=quickest2.duration;
757    
758     if(!FindNode(finish,node2))
759 amb 45 insert_in_queue(results,result2);
760 amb 31 }
761     }
762    
763     endloop:
764    
765     segment=FindNextSegment(segments,segment);
766     }
767     }
768    
769     return(results);
770 amb 2 }
771    
772    
773     /*++++++++++++++++++++++++++++++++++++++
774 amb 47 Find all routes from any node in the specified list to a specific node.
775 amb 34
776 amb 47 Results *FindReverseRoute Returns a set of results.
777 amb 34
778     Nodes *nodes The set of nodes to use.
779    
780     Segments *segments The set of segments to use.
781    
782     Nodes *start The starting nodes.
783    
784     node_t finish The finishing node.
785     ++++++++++++++++++++++++++++++++++++++*/
786    
787     Results *FindReverseRoutes(Nodes *nodes,Segments *segments,Nodes *start,node_t finish)
788     {
789     Results *results;
790 amb 43 node_t node1,node2;
791 amb 34 HalfResult shortest1,quickest1;
792     HalfResult shortest2,quickest2;
793     Result *result1,*result2;
794     Segment *segment;
795     int nresults=0;
796    
797     /* Insert the first node into the queue */
798    
799     results=NewResultsList();
800    
801     result1=InsertResult(results,finish);
802    
803     result1->node=finish;
804 amb 43 result1->shortest.prev=0;
805     result1->shortest.next=0;
806 amb 34 result1->shortest.distance=0;
807     result1->shortest.duration=0;
808 amb 43 result1->quickest.prev=0;
809     result1->quickest.next=0;
810 amb 34 result1->quickest.distance=0;
811     result1->quickest.duration=0;
812    
813 amb 45 insert_in_queue(results,result1);
814 amb 34
815     /* Loop across all nodes in the queue */
816    
817 amb 45 while(OSMQueue.number>0)
818 amb 34 {
819 amb 45 result1=LookupResult(results,OSMQueue.queue[--OSMQueue.number]);
820 amb 43 node1=result1->node;
821 amb 34 shortest1.distance=result1->shortest.distance;
822     shortest1.duration=result1->shortest.duration;
823     quickest1.distance=result1->quickest.distance;
824     quickest1.duration=result1->quickest.duration;
825    
826 amb 43 segment=FindFirstSegment(segments,node1);
827 amb 34
828     while(segment)
829     {
830     /* Reverse the segment and check it exists */
831    
832     node_t reversenode;
833     Segment *reversesegment;
834    
835     reversenode=segment->node2;
836     reversesegment=FindFirstSegment(segments,reversenode);
837    
838 amb 43 while(reversesegment && reversesegment->node2!=node1)
839 amb 34 reversesegment=FindNextSegment(segments,reversesegment);
840    
841     if(!reversesegment)
842     goto endloop;
843    
844     node2=reversesegment->node1;
845    
846 amb 36 if(reversesegment->distance==INVALID_SHORT_DISTANCE ||
847     reversesegment->duration==INVALID_SHORT_DURATION)
848 amb 34 goto endloop;
849    
850     shortest2.distance=shortest1.distance+reversesegment->distance;
851     shortest2.duration=shortest1.duration+reversesegment->duration;
852     quickest2.distance=quickest1.distance+reversesegment->distance;
853     quickest2.duration=quickest1.duration+reversesegment->duration;
854    
855     result2=FindResult(results,node2);
856    
857     if(!result2) /* New end node */
858     {
859     result2=InsertResult(results,node2);
860     result2->node=node2;
861 amb 43 result2->shortest.prev=0;
862     result2->shortest.next=node1;
863 amb 34 result2->shortest.distance=shortest2.distance;
864     result2->shortest.duration=shortest2.duration;
865 amb 43 result2->quickest.prev=0;
866     result2->quickest.next=node1;
867 amb 34 result2->quickest.distance=quickest2.distance;
868     result2->quickest.duration=quickest2.duration;
869    
870     nresults++;
871    
872     if(!FindNode(start,node2))
873 amb 45 insert_in_queue(results,result2);
874 amb 34 }
875     else
876     {
877     if(shortest2.distance<result2->shortest.distance ||
878     (shortest2.distance==result2->shortest.distance &&
879     shortest2.duration<result2->shortest.duration)) /* New end node is shorter */
880     {
881 amb 43 result2->shortest.next=node1;
882 amb 34 result2->shortest.distance=shortest2.distance;
883     result2->shortest.duration=shortest2.duration;
884    
885     if(!FindNode(start,node2))
886 amb 45 insert_in_queue(results,result2);
887 amb 34 }
888    
889     if(quickest2.duration<result2->quickest.duration ||
890     (quickest2.duration==result2->quickest.duration &&
891     quickest2.distance<result2->quickest.distance)) /* New end node is quicker */
892     {
893 amb 43 result2->quickest.next=node1;
894 amb 34 result2->quickest.distance=quickest2.distance;
895     result2->quickest.duration=quickest2.duration;
896    
897     if(!FindNode(start,node2))
898 amb 45 insert_in_queue(results,result2);
899 amb 34 }
900     }
901    
902     endloop:
903    
904     segment=FindNextSegment(segments,segment);
905     }
906     }
907    
908     return(results);
909     }
910    
911    
912     /*++++++++++++++++++++++++++++++++++++++
913 amb 31 Print the optimum route between two nodes.
914    
915 amb 34 Results *results The set of results.
916 amb 31
917     Nodes *nodes The list of nodes.
918    
919     Segments *segments The set of segments to use.
920    
921     Ways *ways The list of ways.
922    
923 amb 33 Nodes *supernodes The list of supernodes.
924 amb 31
925     Segments *supersegments The list of supersegments.
926    
927     node_t start The start node.
928    
929     node_t finish The finish node.
930     ++++++++++++++++++++++++++++++++++++++*/
931    
932 amb 33 void PrintRoutes(Results *results,Nodes *nodes,Segments *segments,Ways *ways,Nodes *supernodes,Segments *supersegments,node_t start,node_t finish)
933 amb 31 {
934     Result *result1,*result2,*result3,*result4;
935     Results *combined;
936    
937     combined=NewResultsList();
938    
939     print_progress=0;
940    
941     /* Sort out the shortest */
942    
943     result1=FindResult(results,start);
944 amb 34 result4=NULL;
945 amb 31
946     do
947     {
948     result3=InsertResult(combined,result1->node);
949    
950     result3->node=result1->node;
951 amb 36
952 amb 31 result3->shortest=result1->shortest;
953 amb 43 result3->shortest.next=0;
954 amb 34 if(result4)
955 amb 43 result3->shortest.prev=result4->node;
956 amb 36 else
957 amb 43 result3->shortest.prev=0;
958 amb 31
959 amb 34 result4=result3;
960    
961 amb 43 if(result1->shortest.next)
962 amb 31 {
963 amb 43 Results *results2=FindRoute(nodes,segments,result1->node,result1->shortest.next);
964 amb 31
965     result2=FindResult(results2,result1->node);
966    
967 amb 43 result3->shortest.next=result2->shortest.next;
968 amb 36
969 amb 43 result2=FindResult(results2,result2->shortest.next);
970 amb 36
971 amb 31 do
972     {
973 amb 43 if(result2->shortest.prev && result2->shortest.next)
974 amb 31 {
975     result4=InsertResult(combined,result2->node);
976    
977     result4->node=result2->node;
978 amb 36
979     result4->shortest=result2->shortest;
980     result4->shortest.distance+=result3->shortest.distance;
981     result4->shortest.duration+=result3->shortest.duration;
982 amb 31 }
983    
984 amb 43 if(result2->shortest.next)
985     result2=FindResult(results2,result2->shortest.next);
986 amb 31 else
987     result2=NULL;
988     }
989     while(result2);
990    
991     FreeResultsList(results2);
992    
993 amb 43 result1=FindResult(results,result1->shortest.next);
994 amb 31 }
995     else
996     result1=NULL;
997     }
998     while(result1);
999    
1000     /* Sort out the quickest */
1001    
1002     result1=FindResult(results,start);
1003 amb 34 result4=NULL;
1004 amb 31
1005     do
1006     {
1007     result3=FindResult(combined,result1->node);
1008    
1009     if(!result3)
1010 amb 34 {
1011 amb 31 result3=InsertResult(combined,result1->node);
1012    
1013 amb 34 result3->node=result1->node;
1014     }
1015    
1016 amb 31 result3->quickest=result1->quickest;
1017 amb 43 result3->quickest.next=0;
1018 amb 34 if(result4)
1019 amb 43 result3->quickest.prev=result4->node;
1020 amb 36 else
1021 amb 43 result3->quickest.prev=0;
1022 amb 31
1023 amb 34 result4=result3;
1024    
1025 amb 43 if(result1->quickest.next)
1026 amb 31 {
1027 amb 43 Results *results2=FindRoute(nodes,segments,result1->node,result1->quickest.next);
1028 amb 31
1029     result2=FindResult(results2,result1->node);
1030    
1031 amb 43 result3->quickest.next=result2->quickest.next;
1032 amb 36
1033 amb 43 result2=FindResult(results2,result2->quickest.next);
1034 amb 36
1035 amb 31 do
1036     {
1037 amb 43 if(result2->quickest.prev && result2->quickest.next)
1038 amb 31 {
1039     result4=FindResult(combined,result2->node);
1040    
1041     if(!result4)
1042 amb 36 {
1043 amb 31 result4=InsertResult(combined,result2->node);
1044    
1045 amb 36 result4->node=result2->node;
1046     }
1047    
1048     result4->quickest=result2->quickest;
1049     result4->quickest.distance+=result3->quickest.distance;
1050     result4->quickest.duration+=result3->quickest.duration;
1051 amb 31 }
1052    
1053 amb 43 if(result2->quickest.next)
1054     result2=FindResult(results2,result2->quickest.next);
1055 amb 31 else
1056     result2=NULL;
1057     }
1058     while(result2);
1059    
1060     FreeResultsList(results2);
1061    
1062 amb 43 result1=FindResult(results,result1->quickest.next);
1063 amb 31 }
1064     else
1065     result1=NULL;
1066     }
1067     while(result1);
1068    
1069     /* Now print out the result normally */
1070    
1071     print_progress=1;
1072    
1073     PrintRoute(combined,nodes,segments,ways,start,finish);
1074     }
1075    
1076    
1077     /*++++++++++++++++++++++++++++++++++++++
1078 amb 2 Insert an item into the queue in the right order.
1079    
1080 amb 45 Results *results The set of results.
1081    
1082 amb 2 Result *result The result to insert into the queue.
1083     ++++++++++++++++++++++++++++++++++++++*/
1084    
1085 amb 45 static void insert_in_queue(Results *results,Result *result)
1086 amb 2 {
1087 amb 45 int start=0;
1088     int end=OSMQueue.number-1;
1089 amb 2 int mid;
1090     int insert=-1;
1091    
1092     /* Check that the whole thing is allocated. */
1093    
1094 amb 45 if(!OSMQueue.queue)
1095 amb 2 {
1096 amb 45 OSMQueue.alloced=INCREMENT;
1097     OSMQueue.number=0;
1098     OSMQueue.queue=(uint32_t*)malloc(OSMQueue.alloced*sizeof(uint32_t));
1099 amb 2 }
1100    
1101     /* Check that the arrays have enough space. */
1102    
1103 amb 45 if(OSMQueue.number==OSMQueue.alloced)
1104 amb 2 {
1105 amb 45 OSMQueue.alloced+=INCREMENT;
1106     OSMQueue.queue=(uint32_t*)realloc((void*)OSMQueue.queue,OSMQueue.alloced*sizeof(uint32_t));
1107 amb 2 }
1108    
1109     /* Binary search - search key may not match, new insertion point required
1110     *
1111     * # <- start | Check mid and move start or end if it doesn't match
1112     * # |
1113     * # | Since there may not be an exact match we must set end=mid
1114     * # <- mid | or start=mid because we know that mid doesn't match.
1115     * # |
1116     * # | Eventually end=start+1 and the insertion point is before
1117     * # <- end | end (since it cannot be before the initial start or end).
1118     */
1119    
1120 amb 45 if(OSMQueue.number==0) /* There is nothing in the queue */
1121     insert=0;
1122     else if(result->shortest.distance>results->results[OSMQueue.queue[start]].shortest.distance) /* Check key is not before start */
1123 amb 2 insert=start;
1124 amb 45 else if(result->shortest.distance<results->results[OSMQueue.queue[end]].shortest.distance) /* Check key is not after end */
1125 amb 2 insert=end+1;
1126     else
1127     {
1128     do
1129     {
1130 amb 45 mid=(start+end)/2; /* Choose mid point */
1131 amb 2
1132 amb 45 if(results->results[OSMQueue.queue[mid]].shortest.distance>result->shortest.distance) /* Mid point is too low */
1133 amb 2 start=mid;
1134 amb 45 else if(results->results[OSMQueue.queue[mid]].shortest.distance<result->shortest.distance) /* Mid point is too high */
1135 amb 2 end=mid;
1136 amb 45 else /* Mid point is correct */
1137 amb 2 {
1138 amb 45 if(&results->results[OSMQueue.queue[mid]]==result)
1139 amb 2 return;
1140    
1141     insert=mid;
1142     break;
1143     }
1144     }
1145     while((end-start)>1);
1146    
1147     if(insert==-1)
1148     insert=end;
1149     }
1150    
1151     /* Shuffle the array up */
1152    
1153 amb 45 if(insert!=OSMQueue.number)
1154     memmove(&OSMQueue.queue[insert+1],&OSMQueue.queue[insert],(OSMQueue.number-insert)*sizeof(uint32_t));
1155 amb 2
1156     /* Insert the new entry */
1157    
1158 amb 45 OSMQueue.queue[insert]=result-results->results;
1159 amb 2
1160 amb 45 OSMQueue.number++;
1161 amb 2 }

Properties

Name Value
cvs:description Route optimiser.
Index: This Site Routino Software Download Source Code Documentation XML Files Links
Index: View SVN Development Recent changes Development (destination-access branch) Version 3.4.2 (destination-access branch) Version 3.4.2 Version 3.4.1 (destination-access branch) Version 3.4.1 Version 3.4 (destination-access branch) Version 3.4 Version 3.3.3 (destination-access branch) Version 3.3.3 Version 3.3.2 (destination-access branch) Version 3.3.2 Version 3.3.1 (destination-access branch) Version 3.3.1 Version 3.3 (destination-access branch) Version 3.3 Version 3.2 Version 3.1.1 Version 3.1 Version 3.0 Version 2.7.3 Version 2.7.2 Version 2.7.1 Version 2.7 Version 2.6 Version 2.5.1 Version 2.5 Version 2.4.1 Version 2.4 Version 2.3.2 Version 2.3.1 Version 2.3 Version 2.2 Version 2.1.2 Version 2.1.1 Version 2.1 Version 2.0.3 Version 2.0.2 Version 2.0.1 Version 2.0 Version 1.5.1 Version 1.5 Version 1.4.1 Version 1.4 Version 1.3 Version 1.2 Version 1.1 Version 1.0
Powered by ViewVC 1.3.0-dev ViewVC Help
© Andrew M. Bishop = < amb @ ... >
Root