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

Contents of /trunk/src/optimiser.c

Parent Directory Parent Directory | Revision Log Revision Log

Revision 236 - (show annotations) (download) (as text)
Sat Aug 15 14:18:23 2009 UTC (15 years, 7 months ago) by amb
File MIME type: text/x-csrc
File size: 21727 byte(s) 
Optimise the priority queue used for routing.
1 /***************************************
2 $Header: /home/amb/CVS/routino/src/optimiser.c,v 1.75 2009-08-15 14:18:23 amb Exp $
3
4 Routing optimiser.
5
6 Part of the Routino routing software.
7 ******************/ /******************
8 This file Copyright 2008,2009 Andrew M. Bishop
9
10 This program is free software: you can redistribute it and/or modify
11 it under the terms of the GNU Affero General Public License as published by
12 the Free Software Foundation, either version 3 of the License, or
13 (at your option) any later version.
14
15 This program is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU Affero General Public License for more details.
19
20 You should have received a copy of the GNU Affero General Public License
21 along with this program. If not, see <http://www.gnu.org/licenses/>.
22 ***************************************/
23
24
25 #include <stdio.h>
26
27 #include "types.h"
28 #include "functions.h"
29 #include "nodes.h"
30 #include "segments.h"
31 #include "ways.h"
32 #include "results.h"
33
34
35 /*+ The option not to print any progress information. +*/
36 extern int option_quiet;
37
38 /*+ The option to calculate the quickest route insted of the shortest. +*/
39 extern int option_quickest;
40
41
42 /*++++++++++++++++++++++++++++++++++++++
43 Find the optimum route between two nodes.
44
45 Results *FindRoute Returns a set of results.
46
47 Nodes *nodes The set of nodes to use.
48
49 Segments *segments The set of segments to use.
50
51 Ways *ways The set of ways to use.
52
53 index_t start The start node.
54
55 index_t finish The finish node.
56
57 Profile *profile The profile containing the transport type, speeds and allowed highways.
58
59 int all A flag to indicate that a big results structure is required.
60 ++++++++++++++++++++++++++++++++++++++*/
61
62 Results *FindRoute(Nodes *nodes,Segments *segments,Ways *ways,index_t start,index_t finish,Profile *profile,int all)
63 {
64 Results *results;
65 Queue *queue;
66 index_t node1,node2;
67 score_t finish_score;
68 double finish_lat,finish_lon;
69 Result *result1,*result2;
70 Segment *segment;
71 Way *way;
72
73 if(!option_quiet)
74 {
75 printf("Routing: End Nodes=0");
76 fflush(stdout);
77 }
78
79 /* Set up the finish conditions */
80
81 finish_score=INF_SCORE;
82
83 GetLatLong(nodes,finish,&finish_lat,&finish_lon);
84
85 /* Create the list of results and insert the first node into the queue */
86
87 if(all)
88 results=NewResultsList(65536);
89 else
90 results=NewResultsList(8);
91
92 results->start=start;
93 results->finish=finish;
94
95 result1=InsertResult(results,start);
96
97 ZeroResult(result1);
98
99 queue=NewQueueList();
100
101 InsertInQueue(queue,result1);
102
103 /* Loop across all nodes in the queue */
104
105 while((result1=PopFromQueue(queue)))
106 {
107 if(result1->sortby>finish_score)
108 continue;
109
110 node1=result1->node;
111
112 segment=FirstSegment(segments,nodes,node1);
113
114 while(segment)
115 {
116 score_t segment_score,cumulative_score;
117
118 if(!IsNormalSegment(segment))
119 goto endloop;
120
121 if(profile->oneway && IsOnewayTo(segment,node1))
122 goto endloop;
123
124 node2=OtherNode(segment,node1);
125
126 if(result1->prev==node2)
127 goto endloop;
128
129 way=LookupWay(ways,segment->way);
130
131 if(!(way->allow&profile->allow))
132 goto endloop;
133
134 if(!profile->highway[HIGHWAY(way->type)])
135 goto endloop;
136
137 if(way->weight && way->weight<profile->weight)
138 goto endloop;
139
140 if((way->height && way->height<profile->height) ||
141 (way->width && way->width <profile->width ) ||
142 (way->length && way->length<profile->length))
143 goto endloop;
144
145 if(option_quickest==0)
146 segment_score=(score_t)DISTANCE(segment->distance)/profile->highway[HIGHWAY(way->type)];
147 else
148 segment_score=(score_t)Duration(segment,way,profile)/profile->highway[HIGHWAY(way->type)];
149
150 cumulative_score=result1->score+segment_score;
151
152 if(cumulative_score>finish_score)
153 goto endloop;
154
155 result2=FindResult(results,node2);
156
157 if(!result2) /* New end node */
158 {
159 result2=InsertResult(results,node2);
160 result2->prev=node1;
161 result2->next=NO_NODE;
162 result2->score=cumulative_score;
163 result2->segment=segment;
164
165 if(node2==finish)
166 {
167 finish_score=cumulative_score;
168 }
169 else
170 {
171 double lat,lon;
172 distance_t direct;
173
174 GetLatLong(nodes,node2,&lat,&lon);
175 direct=Distance(lat,lon,finish_lat,finish_lon);
176
177 if(option_quickest==0)
178 result2->sortby=result2->score+(score_t)direct/profile->max_pref;
179 else
180 result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
181
182 InsertInQueue(queue,result2);
183 }
184 }
185 else if(cumulative_score<result2->score) /* New end node is better */
186 {
187 result2->prev=node1;
188 result2->score=cumulative_score;
189 result2->segment=segment;
190
191 if(node2==finish)
192 {
193 finish_score=cumulative_score;
194 }
195 else if(!all)
196 {
197 InsertInQueue(queue,result2);
198 }
199 else
200 {
201 double lat,lon;
202 distance_t direct;
203
204 GetLatLong(nodes,node2,&lat,&lon);
205 direct=Distance(lat,lon,finish_lat,finish_lon);
206
207 if(option_quickest==0)
208 result2->sortby=result2->score+(score_t)direct/profile->max_pref;
209 else
210 result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
211
212 if(result2->sortby<finish_score)
213 InsertInQueue(queue,result2);
214 }
215 }
216
217 endloop:
218
219 if(!option_quiet && !(results->number%10000))
220 {
221 printf("\rRouting: End Nodes=%d",results->number);
222 fflush(stdout);
223 }
224
225 segment=NextSegment(segments,segment,node1);
226 }
227 }
228
229 if(!option_quiet)
230 {
231 printf("\rRouted: End Nodes=%d\n",results->number);
232 fflush(stdout);
233 }
234
235 FreeQueueList(queue);
236
237 /* Check it worked */
238
239 result2=FindResult(results,finish);
240
241 if(!result2)
242 {
243 FreeResultsList(results);
244 return(NULL);
245 }
246
247 /* Create the forward links for the optimum path */
248
249 result2=FindResult(results,finish);
250
251 do
252 {
253 if(result2->prev!=NO_NODE)
254 {
255 index_t node1=result2->prev;
256
257 result1=FindResult(results,node1);
258
259 result1->next=result2->node;
260
261 result2=result1;
262 }
263 else
264 result2=NULL;
265 }
266 while(result2);
267
268 return(results);
269 }
270
271
272 /*++++++++++++++++++++++++++++++++++++++
273 Find the optimum route between two nodes.
274
275 Results *FindRoute3 Returns a set of results.
276
277 Nodes *nodes The set of nodes to use.
278
279 Segments *segments The set of segments to use.
280
281 Ways *ways The set of ways to use.
282
283 Results *begin The initial portion of the route.
284
285 Results *end The final portion of the route.
286
287 Profile *profile The profile containing the transport type, speeds and allowed highways.
288 ++++++++++++++++++++++++++++++++++++++*/
289
290 Results *FindRoute3(Nodes *nodes,Segments *segments,Ways *ways,Results *begin,Results *end,Profile *profile)
291 {
292 Results *results;
293 Queue *queue;
294 index_t node1,node2;
295 score_t finish_score;
296 double finish_lat,finish_lon;
297 Result *result1,*result2,*result3;
298 Segment *segment;
299 Way *way;
300
301 if(!option_quiet)
302 {
303 printf("Routing: End Nodes=0");
304 fflush(stdout);
305 }
306
307 /* Set up the finish conditions */
308
309 finish_score=~(distance_t)0;
310
311 GetLatLong(nodes,end->finish,&finish_lat,&finish_lon);
312
313 /* Create the list of results and insert the first node into the queue */
314
315 results=NewResultsList(65536);
316
317 results->start=begin->start;
318 results->finish=end->finish;
319
320 result1=InsertResult(results,begin->start);
321 result3=FindResult(begin,begin->start);
322
323 *result1=*result3;
324
325 queue=NewQueueList();
326
327 /* Insert the finish points of the beginning part of the path into the queue */
328
329 result3=FirstResult(begin);
330
331 while(result3)
332 {
333 if(IsSuperNode(nodes,result3->node))
334 {
335 if(!(result2=FindResult(results,result3->node)))
336 {
337 result2=InsertResult(results,result3->node);
338
339 *result2=*result3;
340
341 result2->prev=begin->start;
342
343 result2->sortby=result2->score;
344 }
345
346 InsertInQueue(queue,result2);
347 }
348
349 result3=NextResult(begin,result3);
350 }
351
352 /* Loop across all nodes in the queue */
353
354 while((result1=PopFromQueue(queue)))
355 {
356 if(result1->sortby>finish_score)
357 continue;
358
359 node1=result1->node;
360
361 segment=FirstSegment(segments,nodes,node1);
362
363 while(segment)
364 {
365 score_t segment_score,cumulative_score;
366
367 if(!IsSuperSegment(segment))
368 goto endloop;
369
370 if(profile->oneway && IsOnewayTo(segment,node1))
371 goto endloop;
372
373 node2=OtherNode(segment,node1);
374
375 if(result1->prev==node2)
376 goto endloop;
377
378 way=LookupWay(ways,segment->way);
379
380 if(!(way->allow&profile->allow))
381 goto endloop;
382
383 if(!profile->highway[HIGHWAY(way->type)])
384 goto endloop;
385
386 if(way->weight && way->weight<profile->weight)
387 goto endloop;
388
389 if((way->height && way->height<profile->height) ||
390 (way->width && way->width <profile->width ) ||
391 (way->length && way->length<profile->length))
392 goto endloop;
393
394 if(option_quickest==0)
395 segment_score=(score_t)DISTANCE(segment->distance)/profile->highway[HIGHWAY(way->type)];
396 else
397 segment_score=(score_t)Duration(segment,way,profile)/profile->highway[HIGHWAY(way->type)];
398
399 cumulative_score=result1->score+segment_score;
400
401 if(cumulative_score>finish_score)
402 goto endloop;
403
404 result2=FindResult(results,node2);
405
406 if(!result2) /* New end node */
407 {
408 result2=InsertResult(results,node2);
409 result2->prev=node1;
410 result2->next=NO_NODE;
411 result2->score=cumulative_score;
412 result2->segment=segment;
413
414 if((result3=FindResult(end,node2)))
415 {
416 finish_score=result2->score+result3->score;
417 }
418 else
419 {
420 double lat,lon;
421 distance_t direct;
422
423 GetLatLong(nodes,node2,&lat,&lon);
424 direct=Distance(lat,lon,finish_lat,finish_lon);
425
426 if(option_quickest==0)
427 result2->sortby=result2->score+(score_t)direct/profile->max_pref;
428 else
429 result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
430
431 InsertInQueue(queue,result2);
432 }
433 }
434 else if(cumulative_score<result2->score) /* New end node is better */
435 {
436 result2->prev=node1;
437 result2->score=cumulative_score;
438 result2->segment=segment;
439
440 if((result3=FindResult(end,node2)))
441 {
442 if((result2->score+result3->score)<finish_score)
443 {
444 finish_score=result2->score+result3->score;
445 }
446 }
447 else
448 {
449 double lat,lon;
450 distance_t direct;
451
452 GetLatLong(nodes,node2,&lat,&lon);
453 direct=Distance(lat,lon,finish_lat,finish_lon);
454
455 if(option_quickest==0)
456 result2->sortby=result2->score+(score_t)direct/profile->max_pref;
457 else
458 result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
459
460 if(result2->sortby<finish_score)
461 InsertInQueue(queue,result2);
462 }
463 }
464
465 endloop:
466
467 if(!option_quiet && !(results->number%10000))
468 {
469 printf("\rRouting: End Nodes=%d",results->number);
470 fflush(stdout);
471 }
472
473 segment=NextSegment(segments,segment,node1);
474 }
475 }
476
477 if(!option_quiet)
478 {
479 printf("\rRouted: End Super-Nodes=%d\n",results->number);
480 fflush(stdout);
481 }
482
483 /* Finish off the end part of the route. */
484
485 if(!FindResult(results,end->finish))
486 {
487 result2=InsertResult(results,end->finish);
488 result3=FindResult(end,end->finish);
489
490 *result2=*result3;
491
492 result2->score=~(distance_t)0;
493
494 result3=FirstResult(end);
495
496 while(result3)
497 {
498 if(IsSuperNode(nodes,result3->node))
499 if((result1=FindResult(results,result3->node)))
500 if((result1->score+result3->score)<result2->score)
501 {
502 result2->score=result1->score+result3->score;
503 result2->prev=result1->node;
504 }
505
506 result3=NextResult(end,result3);
507 }
508 }
509
510 FreeQueueList(queue);
511
512 /* Check it worked */
513
514 result2=FindResult(results,end->finish);
515
516 if(!result2)
517 {
518 FreeResultsList(results);
519 return(NULL);
520 }
521
522 /* Create the forward links for the optimum path */
523
524 do
525 {
526 if(result2->prev!=NO_NODE)
527 {
528 index_t node1=result2->prev;
529
530 result1=FindResult(results,node1);
531
532 result1->next=result2->node;
533
534 result2=result1;
535 }
536 else
537 result2=NULL;
538 }
539 while(result2);
540
541 return(results);
542 }
543
544
545 /*++++++++++++++++++++++++++++++++++++++
546 Find all routes from a specified node to any super-node.
547
548 Results *FindStartRoutes Returns a set of results.
549
550 Nodes *nodes The set of nodes to use.
551
552 Segments *segments The set of segments to use.
553
554 Ways *ways The set of ways to use.
555
556 index_t start The start node.
557
558 Profile *profile The profile containing the transport type, speeds and allowed highways.
559 ++++++++++++++++++++++++++++++++++++++*/
560
561 Results *FindStartRoutes(Nodes *nodes,Segments *segments,Ways *ways,index_t start,Profile *profile)
562 {
563 Results *results;
564 Queue *queue;
565 index_t node1,node2;
566 Result *result1,*result2;
567 Segment *segment;
568 Way *way;
569
570 /* Insert the first node into the queue */
571
572 results=NewResultsList(8);
573
574 results->start=start;
575
576 result1=InsertResult(results,start);
577
578 ZeroResult(result1);
579
580 queue=NewQueueList();
581
582 InsertInQueue(queue,result1);
583
584 /* Loop across all nodes in the queue */
585
586 while((result1=PopFromQueue(queue)))
587 {
588 node1=result1->node;
589
590 segment=FirstSegment(segments,nodes,node1);
591
592 while(segment)
593 {
594 score_t segment_score,cumulative_score;
595
596 if(!IsNormalSegment(segment))
597 goto endloop;
598
599 if(profile->oneway && IsOnewayTo(segment,node1))
600 goto endloop;
601
602 node2=OtherNode(segment,node1);
603
604 if(result1->prev==node2)
605 goto endloop;
606
607 way=LookupWay(ways,segment->way);
608
609 if(!(way->allow&profile->allow))
610 goto endloop;
611
612 if(!profile->highway[HIGHWAY(way->type)])
613 goto endloop;
614
615 if(way->weight && way->weight<profile->weight)
616 goto endloop;
617
618 if((way->height && way->height<profile->height) ||
619 (way->width && way->width <profile->width ) ||
620 (way->length && way->length<profile->length))
621 goto endloop;
622
623 if(option_quickest==0)
624 segment_score=(score_t)DISTANCE(segment->distance)/profile->highway[HIGHWAY(way->type)];
625 else
626 segment_score=(score_t)Duration(segment,way,profile)/profile->highway[HIGHWAY(way->type)];
627
628 cumulative_score=result1->score+segment_score;
629
630 result2=FindResult(results,node2);
631
632 if(!result2) /* New end node */
633 {
634 result2=InsertResult(results,node2);
635 result2->prev=node1;
636 result2->next=NO_NODE;
637 result2->score=cumulative_score;
638 result2->segment=segment;
639
640 if(!IsSuperNode(nodes,node2))
641 {
642 result2->sortby=result2->score;
643 InsertInQueue(queue,result2);
644 }
645 }
646 else if(cumulative_score<result2->score) /* New end node is better */
647 {
648 result2->prev=node1;
649 result2->score=cumulative_score;
650 result2->segment=segment;
651
652 if(!IsSuperNode(nodes,node2))
653 {
654 result2->sortby=result2->score;
655 InsertInQueue(queue,result2);
656 }
657 }
658
659 endloop:
660
661 segment=NextSegment(segments,segment,node1);
662 }
663 }
664
665 FreeQueueList(queue);
666
667 /* Check it worked */
668
669 if(results->number==1)
670 {
671 FreeResultsList(results);
672 return(NULL);
673 }
674
675 return(results);
676 }
677
678
679 /*++++++++++++++++++++++++++++++++++++++
680 Find all routes from any super-node to a specific node.
681
682 Results *FindFinishRoutes Returns a set of results.
683
684 Nodes *nodes The set of nodes to use.
685
686 Segments *segments The set of segments to use.
687
688 Ways *ways The set of ways to use.
689
690 index_t finish The finishing node.
691
692 Profile *profile The profile containing the transport type, speeds and allowed highways.
693 ++++++++++++++++++++++++++++++++++++++*/
694
695 Results *FindFinishRoutes(Nodes *nodes,Segments *segments,Ways *ways,index_t finish,Profile *profile)
696 {
697 Results *results;
698 Queue *queue;
699 index_t node1,node2;
700 Result *result1,*result2;
701 Segment *segment;
702 Way *way;
703
704 /* Insert the first node into the queue */
705
706 results=NewResultsList(8);
707
708 results->finish=finish;
709
710 result1=InsertResult(results,finish);
711
712 ZeroResult(result1);
713
714 queue=NewQueueList();
715
716 InsertInQueue(queue,result1);
717
718 /* Loop across all nodes in the queue */
719
720 while((result1=PopFromQueue(queue)))
721 {
722 node1=result1->node;
723
724 segment=FirstSegment(segments,nodes,node1);
725
726 while(segment)
727 {
728 score_t segment_score,cumulative_score;
729
730 if(!IsNormalSegment(segment))
731 goto endloop;
732
733 if(profile->oneway && IsOnewayFrom(segment,node1))
734 goto endloop;
735
736 node2=OtherNode(segment,node1);
737
738 if(result1->next==node2)
739 goto endloop;
740
741 way=LookupWay(ways,segment->way);
742
743 if(!(way->allow&profile->allow))
744 goto endloop;
745
746 if(!profile->highway[HIGHWAY(way->type)])
747 goto endloop;
748
749 if(way->weight && way->weight<profile->weight)
750 goto endloop;
751
752 if((way->height && way->height<profile->height) ||
753 (way->width && way->width <profile->width ) ||
754 (way->length && way->length<profile->length))
755 goto endloop;
756
757 if(option_quickest==0)
758 segment_score=(score_t)DISTANCE(segment->distance)/profile->highway[HIGHWAY(way->type)];
759 else
760 segment_score=(score_t)Duration(segment,way,profile)/profile->highway[HIGHWAY(way->type)];
761
762 cumulative_score=result1->score+segment_score;
763
764 result2=FindResult(results,node2);
765
766 if(!result2) /* New end node */
767 {
768 result2=InsertResult(results,node2);
769 result2->prev=NO_NODE;
770 result2->next=node1;
771 result2->score=cumulative_score;
772 result2->segment=segment;
773
774 if(!IsSuperNode(nodes,node2))
775 {
776 result2->sortby=result2->score;
777 InsertInQueue(queue,result2);
778 }
779 }
780 else if(cumulative_score<result2->score) /* New end node is better */
781 {
782 result2->next=node1;
783 result2->score=cumulative_score;
784 result2->segment=segment;
785
786 if(!IsSuperNode(nodes,node2))
787 {
788 result2->sortby=result2->score;
789 InsertInQueue(queue,result2);
790 }
791 }
792
793 endloop:
794
795 segment=NextSegment(segments,segment,node1);
796 }
797 }
798
799 FreeQueueList(queue);
800
801 /* Check it worked */
802
803 if(results->number==1)
804 {
805 FreeResultsList(results);
806 return(NULL);
807 }
808
809 return(results);
810 }
811
812
813 /*++++++++++++++++++++++++++++++++++++++
814 Create an optimum route given the set of super-nodes to follow.
815
816 Results *CombineRoutes Returns the results from joining the super-nodes.
817
818 Results *results The set of results from the super-nodes.
819
820 Nodes *nodes The list of nodes.
821
822 Segments *segments The set of segments to use.
823
824 Ways *ways The list of ways.
825
826 Profile *profile The profile containing the transport type, speeds and allowed highways.
827 ++++++++++++++++++++++++++++++++++++++*/
828
829 Results *CombineRoutes(Results *results,Nodes *nodes,Segments *segments,Ways *ways,Profile *profile)
830 {
831 Result *result1,*result2,*result3,*result4;
832 Results *combined;
833 int quiet=option_quiet;
834
835 combined=NewResultsList(64);
836
837 combined->start=results->start;
838 combined->finish=results->finish;
839
840 /* Don't print any output for this part */
841
842 option_quiet=1;
843
844 /* Sort out the combined route */
845
846 result1=FindResult(results,results->start);
847
848 result3=InsertResult(combined,results->start);
849
850 ZeroResult(result3);
851
852 do
853 {
854 if(result1->next!=NO_NODE)
855 {
856 Results *results2=FindRoute(nodes,segments,ways,result1->node,result1->next,profile,0);
857
858 result2=FindResult(results2,result1->node);
859
860 result3->next=result2->next;
861
862 result2=FindResult(results2,result2->next);
863
864 do
865 {
866 result4=InsertResult(combined,result2->node);
867
868 *result4=*result2;
869 result4->score+=result3->score;
870
871 if(result2->next!=NO_NODE)
872 result2=FindResult(results2,result2->next);
873 else
874 result2=NULL;
875 }
876 while(result2);
877
878 FreeResultsList(results2);
879
880 result1=FindResult(results,result1->next);
881
882 result3=result4;
883 }
884 else
885 result1=NULL;
886 }
887 while(result1);
888
889 /* Now print out the result normally */
890
891 option_quiet=quiet;
892
893 return(combined);
894 }

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.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