trunk/src/optimiser.c

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