trunk/src/optimiser.c

/***************************************
 $Header: /home/amb/CVS/routino/src/optimiser.c,v 1.5 2009-01-04 17:51:23 amb Exp $

 Routing optimiser.
 ******************/ /******************
 Written by Andrew M. Bishop

 This file Copyright 2008,2009 Andrew M. Bishop
 It may be distributed under the GNU Public License, version 2, or
 any higher version.  See section COPYING of the GNU Public license
 for conditions under which this file may be redistributed.
 ***************************************/


#include <string.h>
#include <stdlib.h>

#include "functions.h"
#include "types.h"

#define INCREMENT 10*1024
#define NBINS     64

/*+ One item in the results. +*/
typedef struct _Result
{
 node_t     node;               /*+ The end node. +*/
 Node       *Node;              /*+ The end Node. +*/
 Node       *shortest_Prev;     /*+ The previous Node following the shortest path. +*/
 distance_t shortest_distance;  /*+ The distance travelled to the node following the shortest path. +*/
 duration_t shortest_duration;  /*+ The time taken to the node following the shortest path. +*/
 Node       *quickest_Prev;     /*+ The previous Node following the quickest path. +*/
 distance_t quickest_distance;  /*+ The distance travelled to the node following the quickest path. +*/
 duration_t quickest_duration;  /*+ The time taken to the node following the quickest path. +*/
}
 Result;

/*+ A list of results. +*/
typedef struct _Results
{
 uint32_t alloced;              /*+ The amount of space allocated for results in the array +*/
 uint32_t number[NBINS];        /*+ The number of occupied results in the array +*/
 Result **results[NBINS];       /*+ An array of pointers to arrays of results +*/
}
 Results;


/*+ A queue results. +*/
typedef struct _Queue
{
 uint32_t alloced;              /*+ The amount of space allocated for results in the array +*/
 uint32_t number;               /*+ The number of occupied results in the array +*/
 Result *queue[1024];           /*+ An array of results whose size is not
                                    necessarily limited to 1024 (i.e. may
                                    overflow the end of this structure). +*/
}
 Queue;


/*+ The queue of nodes. +*/
Queue *OSMQueue=NULL;

/*+ The list of results. +*/
Results *OSMResults=NULL;

/* Functions */

static void insert_in_queue(Result *result);
static Result *find_insert_result(node_t node);
static Result *find_result(node_t node);


/*++++++++++++++++++++++++++++++++++++++
  Find the optimum route between two nodes.

  node_t start The start node.

  node_t finish The finish node.
  ++++++++++++++++++++++++++++++++++++++*/

void FindRoute(node_t start,node_t finish)
{
 Node *Start,*Finish;
 node_t node1,node2;
 Node *Node1,*Node2;
 distance_t shortest_distance1,shortest_distance2=0;
 duration_t shortest_duration1,shortest_duration2=0;
 distance_t quickest_distance1,quickest_distance2=0;
 duration_t quickest_duration1,quickest_duration2=0;
 distance_short_t deltadistance;
 duration_short_t deltaduration;
 distance_t totalcrow,crow;
 distance_t shortest_distancefinish=~0,quickest_distancefinish=~0;
 duration_t shortest_durationfinish=~0,quickest_durationfinish=~0;
 Result *result1,*result2;
 Segment *segment;
 int nresults=0;

 /* Work out the distance as the crow flies */

 Start=FindNode(start);
 Finish=FindNode(finish);

 totalcrow=Distance(Start,Finish);

 /* Insert the first node into the queue */

 result1=find_insert_result(start);

 result1->node=start;
 result1->Node=Start;
 result1->shortest_Prev=NULL;
 result1->shortest_distance=0;
 result1->shortest_duration=0;
 result1->quickest_Prev=NULL;
 result1->quickest_distance=0;
 result1->quickest_duration=0;

 insert_in_queue(result1);

 /* Loop across all nodes in the queue */

 while(OSMQueue->number>0)
   {
    result1=OSMQueue->queue[--OSMQueue->number];
    node1=result1->node;
    Node1=result1->Node;
    shortest_distance1=result1->shortest_distance;
    shortest_duration1=result1->shortest_duration;
    quickest_distance1=result1->quickest_distance;
    quickest_duration1=result1->quickest_duration;

    segment=FindFirstSegment(node1);

    while(segment)
      {
       node2=segment->node2;

       deltadistance=segment->distance;
       deltaduration=segment->duration;

       shortest_distance2=shortest_distance1+deltadistance;
       shortest_duration2=shortest_duration1+deltaduration;
       quickest_distance2=quickest_distance1+deltadistance;
       quickest_duration2=quickest_duration1+deltaduration;

       result2=find_result(node2);
       if(result2)
          Node2=result2->Node;
       else
          Node2=FindNode(node2);

       crow=Distance(Node2,Finish);

       if((crow+shortest_distance2)>(km_to_distance(10)+1.4*totalcrow))
          goto endloop;

       if(shortest_distance2>shortest_distancefinish && quickest_duration2>quickest_durationfinish)
          goto endloop;

       if(!result2)                         /* New end node */
         {
          result2=find_insert_result(node2);
          result2->node=node2;
          result2->Node=Node2;
          result2->shortest_Prev=Node1;
          result2->shortest_distance=shortest_distance2;
          result2->shortest_duration=shortest_duration2;
          result2->quickest_Prev=Node1;
          result2->quickest_distance=quickest_distance2;
          result2->quickest_duration=quickest_duration2;

          nresults++;

          if(node2==finish)
            {
             shortest_distancefinish=shortest_distance2;
             shortest_durationfinish=shortest_duration2;
             quickest_distancefinish=quickest_distance2;
             quickest_durationfinish=quickest_duration2;
            }
          else
             insert_in_queue(result2);
         }
       else
         {
          if(shortest_distance2<result2->shortest_distance ||
             (shortest_distance2==result2->shortest_distance &&
              shortest_duration2<result2->shortest_duration)) /* New end node is shorter */
            {
             result2->shortest_Prev=Node1;
             result2->shortest_distance=shortest_distance2;
             result2->shortest_duration=shortest_duration2;

             if(node2==finish)
               {
                shortest_distancefinish=shortest_distance2;
                shortest_durationfinish=shortest_duration2;
               }
             else
                insert_in_queue(result2);
            }

          if(quickest_duration2<result2->quickest_duration ||
             (quickest_duration2==result2->quickest_duration &&
              quickest_distance2<result2->quickest_distance)) /* New end node is quicker */
            {
             result2->quickest_Prev=Node1;
             result2->quickest_distance=quickest_distance2;
             result2->quickest_duration=quickest_duration2;

             if(node2==finish)
               {
                quickest_distancefinish=quickest_distance2;
                quickest_durationfinish=quickest_duration2;
               }
             else
                insert_in_queue(result2);
            }
         }

      endloop:

       segment=FindNextSegment(segment);
      }

    if(!(nresults%1000))
      {
       printf("\rRouting: End Nodes=%d Queue=%d Journey=%.1fkm,%.0fmin  ",nresults,OSMQueue->number,
              distance_to_km(shortest_distance2),duration_to_minutes(quickest_duration2));
       fflush(stdout);
      }
   }

 printf("\rRouted: End Nodes=%d Shortest=%.1fkm,%.0fmin Quickest=%.1fkm,%.0fmin\n",nresults,
        distance_to_km(shortest_distancefinish),duration_to_minutes(shortest_durationfinish),
        distance_to_km(quickest_distancefinish),duration_to_minutes(quickest_durationfinish));
 fflush(stdout);
}


/*++++++++++++++++++++++++++++++++++++++
  Print the optimum route between two nodes.

  node_t start The start node.

  node_t finish The finish node.
  ++++++++++++++++++++++++++++++++++++++*/

void PrintRoute(node_t start,node_t finish)
{
 FILE *file;
 Result *result;
// int i,j;

 /* Print the result for the shortest route */

 file=fopen("shortest.txt","w");

 result=find_result(finish);

 do
   {
    if(result->shortest_Prev)
      {
       Segment *segment;
       Way *way;

       segment=FindFirstSegment(result->shortest_Prev->id);
       while(segment->node2!=result->Node->id)
          segment=FindNextSegment(segment);

       way=FindWay(segment->way);

       fprintf(file,"%9.5f %9.5f %9d %5.3f %5.2f %3.0f %s\n",result->Node->latitude,result->Node->longitude,result->node,
               distance_to_km(segment->distance),duration_to_minutes(segment->duration),
               distance_to_km(segment->distance)/duration_to_hours(segment->duration),
               WayName(way));

       result=find_result(result->shortest_Prev->id);
      }
    else
      {
       fprintf(file,"%9.5f %9.5f %9d\n",result->Node->latitude,result->Node->longitude,result->node);

       result=NULL;
      }
   }
 while(result);

 fclose(file);

 /* Print the result for the quickest route */

 file=fopen("quickest.txt","w");

 result=find_result(finish);

 do
   {
    if(result->quickest_Prev)
      {
       Segment *segment;
       Way *way;

       segment=FindFirstSegment(result->quickest_Prev->id);
       while(segment->node2!=result->Node->id)
          segment=FindNextSegment(segment);

       way=FindWay(segment->way);

       fprintf(file,"%9.5f %9.5f %9d %5.3f %5.2f %3.0f %s\n",result->Node->latitude,result->Node->longitude,result->node,
               distance_to_km(segment->distance),duration_to_minutes(segment->duration),
               distance_to_km(segment->distance)/duration_to_hours(segment->duration),
               WayName(way));

       result=find_result(result->quickest_Prev->id);
      }
    else
      {
       fprintf(file,"%9.5f %9.5f %9d\n",result->Node->latitude,result->Node->longitude,result->node);

       result=NULL;
      }
   }
 while(result);

 /* Print all the distance results. */

// file=fopen("distance.txt","w");
//
// for(i=0;i<NBINS;i++)
//    for(j=0;j<OSMResults->number[i];j++)
//      {
//       result=OSMResults->results[i][j];
//
//       fprintf(file,"%9.5f %9.5f 0 %5.3f\n",result->Node->latitude,result->Node->longitude,
//               distance_to_km(result->shortest_distance));
//      }
//
// fclose(file);

 /* Print all the duration results. */

// file=fopen("duration.txt","w");
//
// for(i=0;i<NBINS;i++)
//    for(j=0;j<OSMResults->number[i];j++)
//      {
//       result=OSMResults->results[i][j];
//
//       fprintf(file,"%9.5f %9.5f 0 %5.3f\n",result->Node->latitude,result->Node->longitude,
//               duration_to_minutes(result->quickest_duration));
//      }
//
// fclose(file);
}


/*++++++++++++++++++++++++++++++++++++++
  Insert an item into the queue in the right order.

  Result *result The result to insert into the queue.
  ++++++++++++++++++++++++++++++++++++++*/

static void insert_in_queue(Result *result)
{
 int start;
 int end;
 int mid;
 int insert=-1;

 /* Check that the whole thing is allocated. */

 if(!OSMQueue)
   {
    OSMQueue=(Queue*)calloc(1,sizeof(Queue));
    OSMQueue->alloced=sizeof(OSMQueue->queue)/sizeof(OSMQueue->queue[0]);
   }

 /* Check that the arrays have enough space. */

 if(OSMQueue->number==OSMQueue->alloced)
   {
    OSMQueue=(Queue*)realloc((void*)OSMQueue,sizeof(Queue)-sizeof(OSMQueue->queue)+(OSMQueue->alloced+INCREMENT)*sizeof(Result*));

    OSMQueue->alloced+=INCREMENT;
   }

 /* Binary search - search key may not match, new insertion point required
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  Since there may not be an exact match we must set end=mid
  *  # <- mid    |  or start=mid because we know that mid doesn't match.
  *  #           |
  *  #           |  Eventually end=start+1 and the insertion point is before
  *  # <- end    |  end (since it cannot be before the initial start or end).
  */

 start=0;
 end=OSMQueue->number-1;

 if(OSMQueue->number==0)                                                      /* There is nothing in the queue */
    insert=start;
 else if(result->shortest_distance>OSMQueue->queue[start]->shortest_distance) /* Check key is not before start */
    insert=start;
 else if(result->shortest_distance<OSMQueue->queue[end]->shortest_distance)   /* Check key is not after end */
    insert=end+1;
 else
   {
    do
      {
       mid=(start+end)/2;                                                         /* Choose mid point */

       if(OSMQueue->queue[mid]->shortest_distance>result->shortest_distance)      /* Mid point is too low */
          start=mid;
       else if(OSMQueue->queue[mid]->shortest_distance<result->shortest_distance) /* Mid point is too high */
          end=mid;
       else                                                                       /* Mid point is correct */
         {
          if(OSMQueue->queue[mid]==result)
             return;

          insert=mid;
          break;
         }
      }
    while((end-start)>1);

    if(insert==-1)
       insert=end;
   }

 /* Shuffle the array up */

 if(insert!=OSMQueue->number)
    memmove(&OSMQueue->queue[insert+1],&OSMQueue->queue[insert],(OSMQueue->number-insert)*sizeof(Result*));

 /* Insert the new entry */

 OSMQueue->queue[insert]=result;

 OSMQueue->number++;
}


/*++++++++++++++++++++++++++++++++++++++
  Find an existing result or insert a new one into the results in the right order.

  node_t node The node that is to be inserted into the results.
  ++++++++++++++++++++++++++++++++++++++*/

static Result *find_insert_result(node_t node)
{
 int start;
 int end;
 int mid;
 int insert=-1,found=-1;
 int bin=node%NBINS;

 /* Check that the whole thing is allocated. */

 if(!OSMResults)
   {
    int i;

    OSMResults=(Results*)calloc(1,sizeof(Results));
    OSMResults->alloced=INCREMENT;

    for(i=0;i<NBINS;i++)
       OSMResults->results[i]=(Result**)malloc(OSMResults->alloced*sizeof(Result*));
   }

 /* Check that the arrays have enough space. */

 if(OSMResults->number[bin]==OSMResults->alloced)
   {
    int i;

    OSMResults->alloced+=INCREMENT;

    for(i=0;i<NBINS;i++)
       OSMResults->results[i]=(Result**)realloc((void*)OSMResults->results[i],OSMResults->alloced*sizeof(Result*));
   }

 /* Binary search - search key may not match, if not then insertion point required
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  Since there may not be an exact match we must set end=mid
  *  # <- mid    |  or start=mid because we know that mid doesn't match.
  *  #           |
  *  #           |  Eventually end=start+1 and the insertion point is before
  *  # <- end    |  end (since it cannot be before the initial start or end).
  */

 start=0;
 end=OSMResults->number[bin]-1;

 if(OSMResults->number[bin]==0)                      /* There are no results */
    insert=start;
 else if(node<OSMResults->results[bin][start]->node) /* Check key is not before start */
    insert=start;
 else if(node>OSMResults->results[bin][end]->node)   /* Check key is not after end */
    insert=end+1;
 else
   {
    do
      {
       mid=(start+end)/2;                                /* Choose mid point */

       if(OSMResults->results[bin][mid]->node<node)      /* Mid point is too low */
          start=mid;
       else if(OSMResults->results[bin][mid]->node>node) /* Mid point is too high */
          end=mid;
       else                                              /* Mid point is correct */
         {
          found=mid;
          break;
         }
      }
    while((end-start)>1);

    if(found==-1)
      {
       if(OSMResults->results[bin][start]->node==node)   /* Start is correct */
          found=start;
       else if(OSMResults->results[bin][end]->node==node)/* End is correct */
          found=end;
       else
          insert=end;
      }
   }

 /* Shuffle the array up */

 if(insert!=-1 && insert!=OSMResults->number[bin])
    memmove(&OSMResults->results[bin][insert+1],&OSMResults->results[bin][insert],(OSMResults->number[bin]-insert)*sizeof(Result*));

 if(insert!=-1)
    found=insert;

 /* Insert the insert entry */

 if(insert!=-1)
   {
    OSMResults->number[bin]++;

    OSMResults->results[bin][found]=(Result*)malloc(sizeof(Result));
   }

 return(OSMResults->results[bin][found]);
}


/*++++++++++++++++++++++++++++++++++++++
  Find a result, ordered by node.

  node_t node The node that is to be found.
  ++++++++++++++++++++++++++++++++++++++*/

static Result *find_result(node_t node)
{
 int start;
 int end;
 int mid;
 int bin=node%NBINS;

 /* Binary search - search key exact match only is required.
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  Since an exact match is wanted we can set end=mid-1
  *  # <- mid    |  or start=mid+1 because we know that mid doesn't match.
  *  #           |
  *  #           |  Eventually either end=start or end=start+1 and one of
  *  # <- end    |  start or end is the wanted one.
  */

 start=0;
 end=OSMResults->number[bin]-1;

 if(OSMResults->number[bin]==0)                      /* There are no results */
    return(NULL);
 else if(node<OSMResults->results[bin][start]->node) /* Check key is not before start */
    return(NULL);
 else if(node>OSMResults->results[bin][end]->node)   /* Check key is not after end */
    return(NULL);
 else
   {
    do
      {
       mid=(start+end)/2;                                /* Choose mid point */

       if(OSMResults->results[bin][mid]->node<node)      /* Mid point is too low */
          start=mid+1;
       else if(OSMResults->results[bin][mid]->node>node) /* Mid point is too high */
          end=mid-1;
       else                                              /* Mid point is correct */
          return(OSMResults->results[bin][mid]);
      }
    while((end-start)>1);

    if(OSMResults->results[bin][start]->node==node)      /* Start is correct */
       return(OSMResults->results[bin][start]);

    if(OSMResults->results[bin][end]->node==node)        /* End is correct */
       return(OSMResults->results[bin][end]);
   }

 return(NULL);
}
1	amb	2	/***************************************
2	amb	8	$Header: /home/amb/CVS/routino/src/optimiser.c,v 1.5 2009-01-04 17:51:23 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			#include <string.h>
16			#include <stdlib.h>
17
18			#include "functions.h"
19			#include "types.h"
20
21			#define INCREMENT 10*1024
22			#define NBINS 64
23
24			/+ One item in the results. +/
25			typedef struct _Result
26			{
27			node_t node; /+ The end node. +/
28			Node Node; /+ The end Node. +*/
29			Node shortest_Prev; /+ The previous Node following the shortest path. +*/
30			distance_t shortest_distance; /+ The distance travelled to the node following the shortest path. +/
31			duration_t shortest_duration; /+ The time taken to the node following the shortest path. +/
32			Node quickest_Prev; /+ The previous Node following the quickest path. +*/
33			distance_t quickest_distance; /+ The distance travelled to the node following the quickest path. +/
34			duration_t quickest_duration; /+ The time taken to the node following the quickest path. +/
35			}
36			Result;
37
38			/+ A list of results. +/
39			typedef struct _Results
40			{
41			uint32_t alloced; /+ The amount of space allocated for results in the array +/
42			uint32_t number[NBINS]; /+ The number of occupied results in the array +/
43			Result *results[NBINS]; /+ An array of pointers to arrays of results +*/
44			}
45			Results;
46
47
48			/+ A queue results. +/
49			typedef struct _Queue
50			{
51			uint32_t alloced; /+ The amount of space allocated for results in the array +/
52			uint32_t number; /+ The number of occupied results in the array +/
53			Result queue[1024]; /+ An array of results whose size is not
54			necessarily limited to 1024 (i.e. may
55			overflow the end of this structure). +*/
56			}
57			Queue;
58
59
60			/+ The queue of nodes. +/
61			Queue *OSMQueue=NULL;
62
63			/+ The list of results. +/
64			Results *OSMResults=NULL;
65
66			/* Functions */
67
68			static void insert_in_queue(Result *result);
69			static Result *find_insert_result(node_t node);
70			static Result *find_result(node_t node);
71
72
73			/*++++++++++++++++++++++++++++++++++++++
74			Find the optimum route between two nodes.
75
76			node_t start The start node.
77
78			node_t finish The finish node.
79			++++++++++++++++++++++++++++++++++++++*/
80
81			void FindRoute(node_t start,node_t finish)
82			{
83			Node Start,Finish;
84			node_t node1,node2;
85			Node Node1,Node2;
86			distance_t shortest_distance1,shortest_distance2=0;
87			duration_t shortest_duration1,shortest_duration2=0;
88			distance_t quickest_distance1,quickest_distance2=0;
89			duration_t quickest_duration1,quickest_duration2=0;
90	amb	5	distance_short_t deltadistance;
91			duration_short_t deltaduration;
92	amb	2	distance_t totalcrow,crow;
93			distance_t shortest_distancefinish=~0,quickest_distancefinish=~0;
94			duration_t shortest_durationfinish=~0,quickest_durationfinish=~0;
95			Result result1,result2;
96			Segment *segment;
97			int nresults=0;
98
99			/* Work out the distance as the crow flies */
100
101			Start=FindNode(start);
102			Finish=FindNode(finish);
103
104	amb	8	totalcrow=Distance(Start,Finish);
105	amb	2
106			/* Insert the first node into the queue */
107
108			result1=find_insert_result(start);
109
110			result1->node=start;
111			result1->Node=Start;
112			result1->shortest_Prev=NULL;
113			result1->shortest_distance=0;
114			result1->shortest_duration=0;
115			result1->quickest_Prev=NULL;
116			result1->quickest_distance=0;
117			result1->quickest_duration=0;
118
119			insert_in_queue(result1);
120
121			/* Loop across all nodes in the queue */
122
123			while(OSMQueue->number>0)
124			{
125			result1=OSMQueue->queue[--OSMQueue->number];
126			node1=result1->node;
127			Node1=result1->Node;
128			shortest_distance1=result1->shortest_distance;
129			shortest_duration1=result1->shortest_duration;
130			quickest_distance1=result1->quickest_distance;
131			quickest_duration1=result1->quickest_duration;
132
133			segment=FindFirstSegment(node1);
134
135			while(segment)
136			{
137			node2=segment->node2;
138
139			deltadistance=segment->distance;
140			deltaduration=segment->duration;
141
142			shortest_distance2=shortest_distance1+deltadistance;
143			shortest_duration2=shortest_duration1+deltaduration;
144			quickest_distance2=quickest_distance1+deltadistance;
145			quickest_duration2=quickest_duration1+deltaduration;
146
147			result2=find_result(node2);
148			if(result2)
149			Node2=result2->Node;
150			else
151			Node2=FindNode(node2);
152
153	amb	8	crow=Distance(Node2,Finish);
154	amb	2
155	amb	5	if((crow+shortest_distance2)>(km_to_distance(10)+1.4*totalcrow))
156	amb	2	goto endloop;
157
158	amb	3	if(shortest_distance2>shortest_distancefinish && quickest_duration2>quickest_durationfinish)
159	amb	2	goto endloop;
160
161			if(!result2) /* New end node */
162			{
163			result2=find_insert_result(node2);
164			result2->node=node2;
165			result2->Node=Node2;
166			result2->shortest_Prev=Node1;
167			result2->shortest_distance=shortest_distance2;
168			result2->shortest_duration=shortest_duration2;
169			result2->quickest_Prev=Node1;
170			result2->quickest_distance=quickest_distance2;
171			result2->quickest_duration=quickest_duration2;
172
173			nresults++;
174
175			if(node2==finish)
176			{
177			shortest_distancefinish=shortest_distance2;
178			shortest_durationfinish=shortest_duration2;
179			quickest_distancefinish=quickest_distance2;
180			quickest_durationfinish=quickest_duration2;
181			}
182			else
183			insert_in_queue(result2);
184			}
185			else
186			{
187			if(shortest_distance2<result2->shortest_distance \|\|
188			(shortest_distance2==result2->shortest_distance &&
189			shortest_duration2<result2->shortest_duration)) /* New end node is shorter */
190			{
191			result2->shortest_Prev=Node1;
192			result2->shortest_distance=shortest_distance2;
193			result2->shortest_duration=shortest_duration2;
194
195			if(node2==finish)
196			{
197			shortest_distancefinish=shortest_distance2;
198			shortest_durationfinish=shortest_duration2;
199			}
200			else
201			insert_in_queue(result2);
202			}
203
204			if(quickest_duration2<result2->quickest_duration \|\|
205			(quickest_duration2==result2->quickest_duration &&
206			quickest_distance2<result2->quickest_distance)) /* New end node is quicker */
207			{
208			result2->quickest_Prev=Node1;
209			result2->quickest_distance=quickest_distance2;
210			result2->quickest_duration=quickest_duration2;
211
212			if(node2==finish)
213			{
214			quickest_distancefinish=quickest_distance2;
215			quickest_durationfinish=quickest_duration2;
216			}
217			else
218			insert_in_queue(result2);
219			}
220			}
221
222			endloop:
223
224			segment=FindNextSegment(segment);
225			}
226
227			if(!(nresults%1000))
228			{
229			printf("\rRouting: End Nodes=%d Queue=%d Journey=%.1fkm,%.0fmin ",nresults,OSMQueue->number,
230	amb	5	distance_to_km(shortest_distance2),duration_to_minutes(quickest_duration2));
231	amb	2	fflush(stdout);
232			}
233			}
234
235			printf("\rRouted: End Nodes=%d Shortest=%.1fkm,%.0fmin Quickest=%.1fkm,%.0fmin\n",nresults,
236	amb	5	distance_to_km(shortest_distancefinish),duration_to_minutes(shortest_durationfinish),
237			distance_to_km(quickest_distancefinish),duration_to_minutes(quickest_durationfinish));
238	amb	2	fflush(stdout);
239			}
240
241
242			/*++++++++++++++++++++++++++++++++++++++
243			Print the optimum route between two nodes.
244
245			node_t start The start node.
246
247			node_t finish The finish node.
248			++++++++++++++++++++++++++++++++++++++*/
249
250			void PrintRoute(node_t start,node_t finish)
251			{
252			FILE *file;
253			Result *result;
254	amb	3	// int i,j;
255	amb	2
256			/* Print the result for the shortest route */
257
258			file=fopen("shortest.txt","w");
259
260			result=find_result(finish);
261
262			do
263			{
264			if(result->shortest_Prev)
265			{
266	amb	6	Segment *segment;
267			Way *way;
268	amb	2
269	amb	6	segment=FindFirstSegment(result->shortest_Prev->id);
270	amb	2	while(segment->node2!=result->Node->id)
271			segment=FindNextSegment(segment);
272
273	amb	6	way=FindWay(segment->way);
274
275			fprintf(file,"%9.5f %9.5f %9d %5.3f %5.2f %3.0f %s\n",result->Node->latitude,result->Node->longitude,result->node,
276	amb	5	distance_to_km(segment->distance),duration_to_minutes(segment->duration),
277	amb	6	distance_to_km(segment->distance)/duration_to_hours(segment->duration),
278			WayName(way));
279	amb	2
280			result=find_result(result->shortest_Prev->id);
281			}
282			else
283			{
284			fprintf(file,"%9.5f %9.5f %9d\n",result->Node->latitude,result->Node->longitude,result->node);
285
286			result=NULL;
287			}
288			}
289			while(result);
290
291			fclose(file);
292
293			/* Print the result for the quickest route */
294
295			file=fopen("quickest.txt","w");
296
297			result=find_result(finish);
298
299			do
300			{
301			if(result->quickest_Prev)
302			{
303	amb	6	Segment *segment;
304			Way *way;
305	amb	2
306	amb	6	segment=FindFirstSegment(result->quickest_Prev->id);
307	amb	2	while(segment->node2!=result->Node->id)
308			segment=FindNextSegment(segment);
309
310	amb	6	way=FindWay(segment->way);
311
312			fprintf(file,"%9.5f %9.5f %9d %5.3f %5.2f %3.0f %s\n",result->Node->latitude,result->Node->longitude,result->node,
313	amb	5	distance_to_km(segment->distance),duration_to_minutes(segment->duration),
314	amb	6	distance_to_km(segment->distance)/duration_to_hours(segment->duration),
315			WayName(way));
316	amb	2
317			result=find_result(result->quickest_Prev->id);
318			}
319			else
320			{
321			fprintf(file,"%9.5f %9.5f %9d\n",result->Node->latitude,result->Node->longitude,result->node);
322
323			result=NULL;
324			}
325			}
326			while(result);
327
328	amb	3	/* Print all the distance results. */
329
330			// file=fopen("distance.txt","w");
331			//
332			// for(i=0;i<NBINS;i++)
333			// for(j=0;j<OSMResults->number[i];j++)
334			// {
335			// result=OSMResults->results[i][j];
336			//
337			// fprintf(file,"%9.5f %9.5f 0 %5.3f\n",result->Node->latitude,result->Node->longitude,
338	amb	5	// distance_to_km(result->shortest_distance));
339	amb	3	// }
340			//
341			// fclose(file);
342
343			/* Print all the duration results. */
344
345			// file=fopen("duration.txt","w");
346			//
347			// for(i=0;i<NBINS;i++)
348			// for(j=0;j<OSMResults->number[i];j++)
349			// {
350			// result=OSMResults->results[i][j];
351			//
352			// fprintf(file,"%9.5f %9.5f 0 %5.3f\n",result->Node->latitude,result->Node->longitude,
353	amb	5	// duration_to_minutes(result->quickest_duration));
354	amb	3	// }
355			//
356			// fclose(file);
357	amb	2	}
358
359
360			/*++++++++++++++++++++++++++++++++++++++
361			Insert an item into the queue in the right order.
362
363			Result *result The result to insert into the queue.
364			++++++++++++++++++++++++++++++++++++++*/
365
366			static void insert_in_queue(Result *result)
367			{
368			int start;
369			int end;
370			int mid;
371			int insert=-1;
372
373			/* Check that the whole thing is allocated. */
374
375			if(!OSMQueue)
376			{
377			OSMQueue=(Queue*)calloc(1,sizeof(Queue));
378			OSMQueue->alloced=sizeof(OSMQueue->queue)/sizeof(OSMQueue->queue[0]);
379			}
380
381			/* Check that the arrays have enough space. */
382
383			if(OSMQueue->number==OSMQueue->alloced)
384			{
385			OSMQueue=(Queue)realloc((void)OSMQueue,sizeof(Queue)-sizeof(OSMQueue->queue)+(OSMQueue->alloced+INCREMENT)sizeof(Result));
386
387			OSMQueue->alloced+=INCREMENT;
388			}
389
390			/* Binary search - search key may not match, new insertion point required
391			*
392			* # <- start \| Check mid and move start or end if it doesn't match
393			* # \|
394			* # \| Since there may not be an exact match we must set end=mid
395			* # <- mid \| or start=mid because we know that mid doesn't match.
396			* # \|
397			* # \| Eventually end=start+1 and the insertion point is before
398			* # <- end \| end (since it cannot be before the initial start or end).
399			*/
400
401			start=0;
402			end=OSMQueue->number-1;
403
404			if(OSMQueue->number==0) /* There is nothing in the queue */
405			insert=start;
406			else if(result->shortest_distance>OSMQueue->queue[start]->shortest_distance) /* Check key is not before start */
407			insert=start;
408			else if(result->shortest_distance<OSMQueue->queue[end]->shortest_distance) /* Check key is not after end */
409			insert=end+1;
410			else
411			{
412			do
413			{
414			mid=(start+end)/2; /* Choose mid point */
415
416			if(OSMQueue->queue[mid]->shortest_distance>result->shortest_distance) /* Mid point is too low */
417			start=mid;
418			else if(OSMQueue->queue[mid]->shortest_distance<result->shortest_distance) /* Mid point is too high */
419			end=mid;
420			else /* Mid point is correct */
421			{
422			if(OSMQueue->queue[mid]==result)
423			return;
424
425			insert=mid;
426			break;
427			}
428			}
429			while((end-start)>1);
430
431			if(insert==-1)
432			insert=end;
433			}
434
435			/* Shuffle the array up */
436
437			if(insert!=OSMQueue->number)
438			memmove(&OSMQueue->queue[insert+1],&OSMQueue->queue[insert],(OSMQueue->number-insert)sizeof(Result));
439
440			/* Insert the new entry */
441
442			OSMQueue->queue[insert]=result;
443
444			OSMQueue->number++;
445			}
446
447
448			/*++++++++++++++++++++++++++++++++++++++
449			Find an existing result or insert a new one into the results in the right order.
450
451			node_t node The node that is to be inserted into the results.
452			++++++++++++++++++++++++++++++++++++++*/
453
454			static Result *find_insert_result(node_t node)
455			{
456			int start;
457			int end;
458			int mid;
459			int insert=-1,found=-1;
460			int bin=node%NBINS;
461
462			/* Check that the whole thing is allocated. */
463
464			if(!OSMResults)
465			{
466			int i;
467
468			OSMResults=(Results*)calloc(1,sizeof(Results));
469			OSMResults->alloced=INCREMENT;
470
471			for(i=0;i<NBINS;i++)
472			OSMResults->results[i]=(Result*)malloc(OSMResults->allocedsizeof(Result*));
473			}
474
475			/* Check that the arrays have enough space. */
476
477			if(OSMResults->number[bin]==OSMResults->alloced)
478			{
479			int i;
480
481			OSMResults->alloced+=INCREMENT;
482
483			for(i=0;i<NBINS;i++)
484			OSMResults->results[i]=(Result*)realloc((void)OSMResults->results[i],OSMResults->allocedsizeof(Result));
485			}
486
487			/* Binary search - search key may not match, if not then insertion point required
488			*
489			* # <- start \| Check mid and move start or end if it doesn't match
490			* # \|
491			* # \| Since there may not be an exact match we must set end=mid
492			* # <- mid \| or start=mid because we know that mid doesn't match.
493			* # \|
494			* # \| Eventually end=start+1 and the insertion point is before
495			* # <- end \| end (since it cannot be before the initial start or end).
496			*/
497
498			start=0;
499			end=OSMResults->number[bin]-1;
500
501			if(OSMResults->number[bin]==0) /* There are no results */
502			insert=start;
503			else if(node<OSMResults->results[bin][start]->node) /* Check key is not before start */
504			insert=start;
505			else if(node>OSMResults->results[bin][end]->node) /* Check key is not after end */
506			insert=end+1;
507			else
508			{
509			do
510			{
511			mid=(start+end)/2; /* Choose mid point */
512
513			if(OSMResults->results[bin][mid]->node<node) /* Mid point is too low */
514			start=mid;
515			else if(OSMResults->results[bin][mid]->node>node) /* Mid point is too high */
516			end=mid;
517			else /* Mid point is correct */
518			{
519			found=mid;
520			break;
521			}
522			}
523			while((end-start)>1);
524
525			if(found==-1)
526			{
527			if(OSMResults->results[bin][start]->node==node) /* Start is correct */
528			found=start;
529			else if(OSMResults->results[bin][end]->node==node)/* End is correct */
530			found=end;
531			else
532			insert=end;
533			}
534			}
535
536			/* Shuffle the array up */
537
538			if(insert!=-1 && insert!=OSMResults->number[bin])
539			memmove(&OSMResults->results[bin][insert+1],&OSMResults->results[bin][insert],(OSMResults->number[bin]-insert)sizeof(Result));
540
541			if(insert!=-1)
542			found=insert;
543
544			/* Insert the insert entry */
545
546			if(insert!=-1)
547			{
548			OSMResults->number[bin]++;
549
550			OSMResults->results[bin][found]=(Result*)malloc(sizeof(Result));
551			}
552
553			return(OSMResults->results[bin][found]);
554			}
555
556
557			/*++++++++++++++++++++++++++++++++++++++
558			Find a result, ordered by node.
559
560			node_t node The node that is to be found.
561			++++++++++++++++++++++++++++++++++++++*/
562
563			static Result *find_result(node_t node)
564			{
565			int start;
566			int end;
567			int mid;
568			int bin=node%NBINS;
569
570			/* Binary search - search key exact match only is required.
571			*
572			* # <- start \| Check mid and move start or end if it doesn't match
573			* # \|
574			* # \| Since an exact match is wanted we can set end=mid-1
575			* # <- mid \| or start=mid+1 because we know that mid doesn't match.
576			* # \|
577			* # \| Eventually either end=start or end=start+1 and one of
578			* # <- end \| start or end is the wanted one.
579			*/
580
581			start=0;
582			end=OSMResults->number[bin]-1;
583
584			if(OSMResults->number[bin]==0) /* There are no results */
585			return(NULL);
586			else if(node<OSMResults->results[bin][start]->node) /* Check key is not before start */
587			return(NULL);
588			else if(node>OSMResults->results[bin][end]->node) /* Check key is not after end */
589			return(NULL);
590			else
591			{
592			do
593			{
594			mid=(start+end)/2; /* Choose mid point */
595
596			if(OSMResults->results[bin][mid]->node<node) /* Mid point is too low */
597			start=mid+1;
598			else if(OSMResults->results[bin][mid]->node>node) /* Mid point is too high */
599			end=mid-1;
600			else /* Mid point is correct */
601			return(OSMResults->results[bin][mid]);
602			}
603			while((end-start)>1);
604
605			if(OSMResults->results[bin][start]->node==node) /* Start is correct */
606			return(OSMResults->results[bin][start]);
607
608			if(OSMResults->results[bin][end]->node==node) /* End is correct */
609			return(OSMResults->results[bin][end]);
610			}
611
612			return(NULL);
613			}