unlabeled-1.7.1/src/optimiser.c

/***************************************
 $Header: /home/amb/CVS/routino/src/optimiser.c,v 1.7.1.1 2009-01-06 18:21:54 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 <assert.h>
#include <string.h>
#include <stdlib.h>

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

#define INCREMENT 1024
#define NBINS     256

/*+ One part of the result for a node. +*/
typedef struct _HalfResult
{
 Node       *Prev;              /*+ The previous Node following the shortest path. +*/
 distance_t  distance;          /*+ The distance travelled to the node following the shortest path. +*/
 duration_t  duration;          /*+ The time taken to the node following the shortest path. +*/
}
 HalfResult;

/*+ One complete result for a node. +*/
typedef struct _Result
{
 node_t     node;               /*+ The end node. +*/
 Node      *Node;               /*+ The end Node. +*/
 HalfResult shortest;           /*+ The result for the shortest path. +*/
 HalfResult quickest;           /*+ The result for 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 of results. +*/
typedef struct _Queue
{
 int        firstbin;           /*+ The distance associated to the first bin. +*/
 distance_t minimum;            /*+ The distance associated to the first bin. +*/
 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. +*/
}
 Queue;


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

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

/* Functions */

static void remove_from_queue(Result *result);
static void insert_in_queue(Result *result);
static Result *pop_from_queue(void);

static Result *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 node2;
 Node *Node1,*Node2;
 HalfResult shortest1,quickest1;
 HalfResult shortest2,quickest2;
 HalfResult shortestfinish,quickestfinish;
 distance_t totalcrow,crow;
 Result *result1,*result2;
 Segment *segment;
 int nresults=0;

 /* Set up the finish conditions */

 shortestfinish.distance=~0;
 shortestfinish.duration=~0;
 quickestfinish.distance=~0;
 quickestfinish.duration=~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=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((result1=pop_from_queue()))
   {
    Node1=result1->Node;
    shortest1.distance=result1->shortest.distance;
    shortest1.duration=result1->shortest.duration;
    quickest1.distance=result1->quickest.distance;
    quickest1.duration=result1->quickest.duration;

    segment=FindFirstSegment(Node1->id);

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

       shortest2.distance=shortest1.distance+segment->distance;
       shortest2.duration=shortest1.duration+segment->duration;
       quickest2.distance=quickest1.distance+segment->distance;
       quickest2.duration=quickest1.duration+segment->duration;

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

       crow=Distance(Node2,Finish);

       if((crow+shortest2.distance)>(km_to_distance(10)+1.4*totalcrow))
          goto endloop;

       if(shortest2.distance>shortestfinish.distance && quickest2.duration>quickestfinish.duration)
          goto endloop;

       if(!result2)                         /* New end node */
         {
          result2=insert_result(node2);
          result2->node=node2;
          result2->Node=Node2;
          result2->shortest.Prev=Node1;
          result2->shortest.distance=shortest2.distance;
          result2->shortest.duration=shortest2.duration;
          result2->quickest.Prev=Node1;
          result2->quickest.distance=quickest2.distance;
          result2->quickest.duration=quickest2.duration;

          nresults++;

          if(node2==finish)
            {
             shortestfinish.distance=shortest2.distance;
             shortestfinish.duration=shortest2.duration;
             quickestfinish.distance=quickest2.distance;
             quickestfinish.duration=quickest2.duration;
            }
          else
             insert_in_queue(result2);
         }
       else
         {
          if(shortest2.distance<result2->shortest.distance ||
             (shortest2.distance==result2->shortest.distance &&
              shortest2.duration<result2->shortest.duration)) /* New end node is shorter */
            {
             remove_from_queue(result2);

             result2->shortest.Prev=Node1;
             result2->shortest.distance=shortest2.distance;
             result2->shortest.duration=shortest2.duration;

             if(node2==finish)
               {
                shortestfinish.distance=shortest2.distance;
                shortestfinish.duration=shortest2.duration;
               }
             else
                insert_in_queue(result2);
            }

          if(quickest2.duration<result2->quickest.duration ||
             (quickest2.duration==result2->quickest.duration &&
              quickest2.distance<result2->quickest.distance)) /* New end node is quicker */
            {
             remove_from_queue(result2);

             result2->quickest.Prev=Node1;
             result2->quickest.distance=quickest2.distance;
             result2->quickest.duration=quickest2.duration;

             if(node2==finish)
               {
                quickestfinish.distance=quickest2.distance;
                quickestfinish.duration=quickest2.duration;
               }
             else
                insert_in_queue(result2);
            }
         }

      endloop:

       segment=FindNextSegment(segment);
      }

    if(!(nresults%1000))
      {
       printf("\rRouting: End Nodes=%d Queue=%d Journey=%.1fkm,%.0fmin  ",nresults,0,
              distance_to_km(shortest2.distance),duration_to_minutes(quickest2.duration));
//       printf("%3d %d -> %d = %d\n",OSMQueue->number,
//              (int)(10*distance_to_km(OSMQueue->queue[0]->shortest.distance)),
//              (int)(10*distance_to_km(OSMQueue->queue[OSMQueue->number-1]->shortest.distance)),
//              (int)(10*distance_to_km(OSMQueue->queue[0]->shortest.distance))-
//              (int)(10*distance_to_km(OSMQueue->queue[OSMQueue->number-1]->shortest.distance)));
       fflush(stdout);
      }
   }

 printf("\rRouted: End Nodes=%d Shortest=%.1fkm,%.0fmin Quickest=%.1fkm,%.0fmin\n",nresults,
        distance_to_km(shortestfinish.distance),duration_to_minutes(shortestfinish.duration),
        distance_to_km(quickestfinish.distance),duration_to_minutes(quickestfinish.duration));
 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 a result into the queue in the right order.

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

static void insert_in_queue(Result *result)
{
 static int biggest=0;
 int bin;

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

 if(!OSMQueue)
   {
    int i;

    OSMQueue=(Queue*)calloc(1,sizeof(Queue));
    OSMQueue->alloced=(INCREMENT/8);

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

 /* Choose the bin */

 bin=(int)(result->shortest.distance>>5)-OSMQueue->minimum;

 if(bin<0)
    bin=0;

 if(bin>=NBINS)
    bin=NBINS-1;

 if(bin>biggest)
   {printf("bin=%d\n",bin);biggest=bin;}

 bin=(bin+OSMQueue->firstbin)%NBINS;

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

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

    OSMQueue->alloced+=(INCREMENT/8);

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

 /* No search - just add result to appropriate bin */

 OSMQueue->results[bin][OSMQueue->number[bin]]=result;
 OSMQueue->number[bin]++;
}


/*++++++++++++++++++++++++++++++++++++++
  Remove a result from the queue.

  Result *result The result to remove from the queue.
  ++++++++++++++++++++++++++++++++++++++*/

static void remove_from_queue(Result *result)
{
 int i;
 int bin=(result->shortest.distance>>5)-OSMQueue->minimum;

 if(bin<0)
    return;

 if(bin>=NBINS)
    bin=NBINS-1;

 bin=(bin+OSMQueue->firstbin)%NBINS;

 /* Linear search in correct bin */

 for(i=0;i<OSMQueue->number[bin];i++)
    if(OSMQueue->results[bin][i]==result)
       OSMQueue->results[bin][i]=NULL;
}


/*++++++++++++++++++++++++++++++++++++++
  Pop a result from the queue.
  ++++++++++++++++++++++++++++++++++++++*/

static Result *pop_from_queue(void)
{
 int i,j;

 /* No search - just take first result in first non-empty bin */

 for(j=0;j<NBINS;j++)
   {
    int bin=OSMQueue->firstbin;

    for(i=OSMQueue->number[bin]-1;i>=0;i--)
       if(OSMQueue->results[bin][i])
         {
          OSMQueue->number[bin]=i;
          return(OSMQueue->results[bin][i]);
         }

    OSMQueue->firstbin++;
    OSMQueue->firstbin%=NBINS;
    OSMQueue->minimum++;
   }

 return(NULL);
}


/*++++++++++++++++++++++++++++++++++++++
  Insert a new result into the results data structure in the right order.

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

static Result *insert_result(node_t node)
{
 int start;
 int end;
 int mid;
 int insert=-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
          assert(0);
      }
    while((end-start)>1);

    insert=end;
   }

 /* Shuffle the array up */

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

 /* Insert the new entry */

 OSMResults->number[bin]++;

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

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


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