trunk/src/optimiser.c

/***************************************
 $Header: /home/amb/CVS/routino/src/optimiser.c,v 1.9 2009-01-10 13:39:51 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 "nodes.h"
#include "ways.h"
#include "segments.h"
#include "results.h"
#include "functions.h"


#define INCREMENT 1024
#define NBINS     256


/*+ 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);


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

  Results *FindRoute Returns a set of results.

  Nodes *nodes The set of nodes to use.

  Segments *segments The set of segments to use.

  node_t start The start node.

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

void FindRoute(Nodes *nodes,Segments *segments,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(nodes,start);
 Finish=FindNode(nodes,finish);

 totalcrow=Distance(Start,Finish);

 /* Insert the first node into the queue */

 OSMResults=NewResultsList();

 result1=InsertResult(OSMResults,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(segments,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=FindResult(OSMResults,node2);
       if(result2)
          Node2=result2->Node;
       else
          Node2=FindNode(nodes,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=InsertResult(OSMResults,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(segments,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.

  Segments *segments The set of segments to use.

  Ways *ways The list of ways.

  node_t start The start node.

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

void PrintRoute(Segments *segments,Ways *ways,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=FindResult(OSMResults,finish);

 do
   {
    if(result->shortest.Prev)
      {
       Segment *segment;
       Way *way;

       segment=FindFirstSegment(segments,result->shortest.Prev->id);
       while(segment->node2!=result->Node->id)
          segment=FindNextSegment(segments,segment);

       way=FindWay(ways,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(ways,way));

       result=FindResult(OSMResults,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=FindResult(OSMResults,finish);

 do
   {
    if(result->quickest.Prev)
      {
       Segment *segment;
       Way *way;

       segment=FindFirstSegment(segments,result->quickest.Prev->id);
       while(segment->node2!=result->Node->id)
          segment=FindNextSegment(segments,segment);

       way=FindWay(ways,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(ways,way));

       result=FindResult(OSMResults,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*)malloc(sizeof(Queue)-sizeof(OSMQueue->queue)+INCREMENT*sizeof(Result*));

    OSMQueue->alloced=INCREMENT;
    OSMQueue->number=0;
   }

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

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

 /* 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++;
}


1	/***************************************
2	$Header: /home/amb/CVS/routino/src/optimiser.c,v 1.9 2009-01-10 13:39:51 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 "nodes.h"
20	#include "ways.h"
21	#include "segments.h"
22	#include "results.h"
23	#include "functions.h"
24
25
26	#define INCREMENT 1024
27	#define NBINS 256
28
29
30	/+ A queue results. +/
31	typedef struct _Queue
32	{
33	uint32_t alloced; /+ The amount of space allocated for results in the array +/
34	uint32_t number; /+ The number of occupied results in the array +/
35	Result queue[1024]; /+ An array of results whose size is not
36	necessarily limited to 1024 (i.e. may
37	overflow the end of this structure). +*/
38	}
39	Queue;
40
41
42	/+ The queue of nodes. +/
43	Queue *OSMQueue=NULL;
44
45	/+ The list of results. +/
46	Results *OSMResults=NULL;
47
48	/* Functions */
49
50	static void insert_in_queue(Result *result);
51
52
53	/*++++++++++++++++++++++++++++++++++++++
54	Find the optimum route between two nodes.
55
56	Results *FindRoute Returns a set of results.
57
58	Nodes *nodes The set of nodes to use.
59
60	Segments *segments The set of segments to use.
61
62	node_t start The start node.
63
64	node_t finish The finish node.
65	++++++++++++++++++++++++++++++++++++++*/
66
67	void FindRoute(Nodes nodes,Segments segments,node_t start,node_t finish)
68	{
69	Node Start,Finish;
70	node_t node2;
71	Node Node1,Node2;
72	HalfResult shortest1,quickest1;
73	HalfResult shortest2,quickest2;
74	HalfResult shortestfinish,quickestfinish;
75	distance_t totalcrow,crow;
76	Result result1,result2;
77	Segment *segment;
78	int nresults=0;
79
80	/* Set up the finish conditions */
81
82	shortestfinish.distance=~0;
83	shortestfinish.duration=~0;
84	quickestfinish.distance=~0;
85	quickestfinish.duration=~0;
86
87	/* Work out the distance as the crow flies */
88
89	Start=FindNode(nodes,start);
90	Finish=FindNode(nodes,finish);
91
92	totalcrow=Distance(Start,Finish);
93
94	/* Insert the first node into the queue */
95
96	OSMResults=NewResultsList();
97
98	result1=InsertResult(OSMResults,start);
99
100	result1->node=start;
101	result1->Node=Start;
102	result1->shortest.Prev=NULL;
103	result1->shortest.distance=0;
104	result1->shortest.duration=0;
105	result1->quickest.Prev=NULL;
106	result1->quickest.distance=0;
107	result1->quickest.duration=0;
108
109	insert_in_queue(result1);
110
111	/* Loop across all nodes in the queue */
112
113	while(OSMQueue->number>0)
114	{
115	result1=OSMQueue->queue[--OSMQueue->number];
116	Node1=result1->Node;
117	shortest1.distance=result1->shortest.distance;
118	shortest1.duration=result1->shortest.duration;
119	quickest1.distance=result1->quickest.distance;
120	quickest1.duration=result1->quickest.duration;
121
122	segment=FindFirstSegment(segments,Node1->id);
123
124	while(segment)
125	{
126	node2=segment->node2;
127
128	shortest2.distance=shortest1.distance+segment->distance;
129	shortest2.duration=shortest1.duration+segment->duration;
130	quickest2.distance=quickest1.distance+segment->distance;
131	quickest2.duration=quickest1.duration+segment->duration;
132
133	result2=FindResult(OSMResults,node2);
134	if(result2)
135	Node2=result2->Node;
136	else
137	Node2=FindNode(nodes,node2);
138
139	crow=Distance(Node2,Finish);
140
141	if((crow+shortest2.distance)>(km_to_distance(10)+1.4*totalcrow))
142	goto endloop;
143
144	if(shortest2.distance>shortestfinish.distance && quickest2.duration>quickestfinish.duration)
145	goto endloop;
146
147	if(!result2) /* New end node */
148	{
149	result2=InsertResult(OSMResults,node2);
150	result2->node=node2;
151	result2->Node=Node2;
152	result2->shortest.Prev=Node1;
153	result2->shortest.distance=shortest2.distance;
154	result2->shortest.duration=shortest2.duration;
155	result2->quickest.Prev=Node1;
156	result2->quickest.distance=quickest2.distance;
157	result2->quickest.duration=quickest2.duration;
158
159	nresults++;
160
161	if(node2==finish)
162	{
163	shortestfinish.distance=shortest2.distance;
164	shortestfinish.duration=shortest2.duration;
165	quickestfinish.distance=quickest2.distance;
166	quickestfinish.duration=quickest2.duration;
167	}
168	else
169	insert_in_queue(result2);
170	}
171	else
172	{
173	if(shortest2.distance<result2->shortest.distance \|\|
174	(shortest2.distance==result2->shortest.distance &&
175	shortest2.duration<result2->shortest.duration)) /* New end node is shorter */
176	{
177	result2->shortest.Prev=Node1;
178	result2->shortest.distance=shortest2.distance;
179	result2->shortest.duration=shortest2.duration;
180
181	if(node2==finish)
182	{
183	shortestfinish.distance=shortest2.distance;
184	shortestfinish.duration=shortest2.duration;
185	}
186	else
187	insert_in_queue(result2);
188	}
189
190	if(quickest2.duration<result2->quickest.duration \|\|
191	(quickest2.duration==result2->quickest.duration &&
192	quickest2.distance<result2->quickest.distance)) /* New end node is quicker */
193	{
194	result2->quickest.Prev=Node1;
195	result2->quickest.distance=quickest2.distance;
196	result2->quickest.duration=quickest2.duration;
197
198	if(node2==finish)
199	{
200	quickestfinish.distance=quickest2.distance;
201	quickestfinish.duration=quickest2.duration;
202	}
203	else
204	insert_in_queue(result2);
205	}
206	}
207
208	endloop:
209
210	segment=FindNextSegment(segments,segment);
211	}
212
213	if(!(nresults%1000))
214	{
215	printf("\rRouting: End Nodes=%d Queue=%d Journey=%.1fkm,%.0fmin ",nresults,OSMQueue->number,
216	distance_to_km(shortest2.distance),duration_to_minutes(quickest2.duration));
217	fflush(stdout);
218	}
219	}
220
221	printf("\rRouted: End Nodes=%d Shortest=%.1fkm,%.0fmin Quickest=%.1fkm,%.0fmin\n",nresults,
222	distance_to_km(shortestfinish.distance),duration_to_minutes(shortestfinish.duration),
223	distance_to_km(quickestfinish.distance),duration_to_minutes(quickestfinish.duration));
224	fflush(stdout);
225	}
226
227
228	/*++++++++++++++++++++++++++++++++++++++
229	Print the optimum route between two nodes.
230
231	Segments *segments The set of segments to use.
232
233	Ways *ways The list of ways.
234
235	node_t start The start node.
236
237	node_t finish The finish node.
238	++++++++++++++++++++++++++++++++++++++*/
239
240	void PrintRoute(Segments segments,Ways ways,node_t start,node_t finish)
241	{
242	FILE *file;
243	Result *result;
244	// int i,j;
245
246	/* Print the result for the shortest route */
247
248	file=fopen("shortest.txt","w");
249
250	result=FindResult(OSMResults,finish);
251
252	do
253	{
254	if(result->shortest.Prev)
255	{
256	Segment *segment;
257	Way *way;
258
259	segment=FindFirstSegment(segments,result->shortest.Prev->id);
260	while(segment->node2!=result->Node->id)
261	segment=FindNextSegment(segments,segment);
262
263	way=FindWay(ways,segment->way);
264
265	fprintf(file,"%9.5f %9.5f %9d %5.3f %5.2f %3.0f %s\n",result->Node->latitude,result->Node->longitude,result->node,
266	distance_to_km(segment->distance),duration_to_minutes(segment->duration),
267	distance_to_km(segment->distance)/duration_to_hours(segment->duration),
268	WayName(ways,way));
269
270	result=FindResult(OSMResults,result->shortest.Prev->id);
271	}
272	else
273	{
274	fprintf(file,"%9.5f %9.5f %9d\n",result->Node->latitude,result->Node->longitude,result->node);
275
276	result=NULL;
277	}
278	}
279	while(result);
280
281	fclose(file);
282
283	/* Print the result for the quickest route */
284
285	file=fopen("quickest.txt","w");
286
287	result=FindResult(OSMResults,finish);
288
289	do
290	{
291	if(result->quickest.Prev)
292	{
293	Segment *segment;
294	Way *way;
295
296	segment=FindFirstSegment(segments,result->quickest.Prev->id);
297	while(segment->node2!=result->Node->id)
298	segment=FindNextSegment(segments,segment);
299
300	way=FindWay(ways,segment->way);
301
302	fprintf(file,"%9.5f %9.5f %9d %5.3f %5.2f %3.0f %s\n",result->Node->latitude,result->Node->longitude,result->node,
303	distance_to_km(segment->distance),duration_to_minutes(segment->duration),
304	distance_to_km(segment->distance)/duration_to_hours(segment->duration),
305	WayName(ways,way));
306
307	result=FindResult(OSMResults,result->quickest.Prev->id);
308	}
309	else
310	{
311	fprintf(file,"%9.5f %9.5f %9d\n",result->Node->latitude,result->Node->longitude,result->node);
312
313	result=NULL;
314	}
315	}
316	while(result);
317
318	/* Print all the distance results. */
319
320	// file=fopen("distance.txt","w");
321	//
322	// for(i=0;i<NBINS;i++)
323	// for(j=0;j<OSMResults->number[i];j++)
324	// {
325	// result=OSMResults->results[i][j];
326	//
327	// fprintf(file,"%9.5f %9.5f 0 %5.3f\n",result->Node->latitude,result->Node->longitude,
328	// distance_to_km(result->shortest.distance));
329	// }
330	//
331	// fclose(file);
332
333	/* Print all the duration results. */
334
335	// file=fopen("duration.txt","w");
336	//
337	// for(i=0;i<NBINS;i++)
338	// for(j=0;j<OSMResults->number[i];j++)
339	// {
340	// result=OSMResults->results[i][j];
341	//
342	// fprintf(file,"%9.5f %9.5f 0 %5.3f\n",result->Node->latitude,result->Node->longitude,
343	// duration_to_minutes(result->quickest.duration));
344	// }
345	//
346	// fclose(file);
347	}
348
349
350	/*++++++++++++++++++++++++++++++++++++++
351	Insert an item into the queue in the right order.
352
353	Result *result The result to insert into the queue.
354	++++++++++++++++++++++++++++++++++++++*/
355
356	static void insert_in_queue(Result *result)
357	{
358	int start;
359	int end;
360	int mid;
361	int insert=-1;
362
363	/* Check that the whole thing is allocated. */
364
365	if(!OSMQueue)
366	{
367	OSMQueue=(Queue)malloc(sizeof(Queue)-sizeof(OSMQueue->queue)+INCREMENTsizeof(Result*));
368
369	OSMQueue->alloced=INCREMENT;
370	OSMQueue->number=0;
371	}
372
373	/* Check that the arrays have enough space. */
374
375	if(OSMQueue->number==OSMQueue->alloced)
376	{
377	OSMQueue->alloced+=INCREMENT;
378	OSMQueue=(Queue)realloc((void)OSMQueue,sizeof(Queue)-sizeof(OSMQueue->queue)+OSMQueue->allocedsizeof(Result));
379	}
380
381	/* Binary search - search key may not match, new insertion point required
382	*
383	* # <- start \| Check mid and move start or end if it doesn't match
384	* # \|
385	* # \| Since there may not be an exact match we must set end=mid
386	* # <- mid \| or start=mid because we know that mid doesn't match.
387	* # \|
388	* # \| Eventually end=start+1 and the insertion point is before
389	* # <- end \| end (since it cannot be before the initial start or end).
390	*/
391
392	start=0;
393	end=OSMQueue->number-1;
394
395	if(OSMQueue->number==0) /* There is nothing in the queue */
396	insert=start;
397	else if(result->shortest.distance>OSMQueue->queue[start]->shortest.distance) /* Check key is not before start */
398	insert=start;
399	else if(result->shortest.distance<OSMQueue->queue[end]->shortest.distance) /* Check key is not after end */
400	insert=end+1;
401	else
402	{
403	do
404	{
405	mid=(start+end)/2; /* Choose mid point */
406
407	if(OSMQueue->queue[mid]->shortest.distance>result->shortest.distance) /* Mid point is too low */
408	start=mid;
409	else if(OSMQueue->queue[mid]->shortest.distance<result->shortest.distance) /* Mid point is too high */
410	end=mid;
411	else /* Mid point is correct */
412	{
413	if(OSMQueue->queue[mid]==result)
414	return;
415
416	insert=mid;
417	break;
418	}
419	}
420	while((end-start)>1);
421
422	if(insert==-1)
423	insert=end;
424	}
425
426	/* Shuffle the array up */
427
428	if(insert!=OSMQueue->number)
429	memmove(&OSMQueue->queue[insert+1],&OSMQueue->queue[insert],(OSMQueue->number-insert)sizeof(Result));
430
431	/* Insert the new entry */
432
433	OSMQueue->queue[insert]=result;
434
435	OSMQueue->number++;
436	}
437
438