trunk/src/nodes.c

/***************************************
 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.25 2009-02-15 16:19:28 amb Exp $

 Node data type functions.
 ******************/ /******************
 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 <stdlib.h>
#include <math.h>

#include "nodes.h"
#include "segments.h"
#include "functions.h"


/*++++++++++++++++++++++++++++++++++++++
  Load in a node list from a file.

  Nodes* LoadNodeList Returns the node list.

  const char *filename The name of the file to load.
  ++++++++++++++++++++++++++++++++++++++*/

Nodes *LoadNodeList(const char *filename)
{
 void *data;
 Nodes *nodes;

 nodes=(Nodes*)malloc(sizeof(Nodes));

 data=MapFile(filename);

 if(!data)
    return(NULL);

 /* Copy the Nodes structure from the loaded data */

 *nodes=*((Nodes*)data);

 /* Adjust the pointers in the Nodes structure. */

 nodes->data=data;
 nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
 nodes->nodes=(Node*)(data+(off_t)nodes->nodes);

 return(nodes);
}


/*++++++++++++++++++++++++++++++++++++++
  Find a node given its latitude and longitude.

  Node *FindNode Returns the node.

  Nodes* nodes The set of nodes to search.

  float latitude The latitude to look for.

  float longitude The longitude to look for.

  distance_t distance The maximum distance to look, returns the final distance.
  ++++++++++++++++++++++++++++++++++++++*/

Node *FindNode(Nodes* nodes,float latitude,float longitude,distance_t *distance)
{
 int32_t latbin=lat_long_to_bin(latitude )-nodes->latzero;
 int32_t lonbin=lat_long_to_bin(longitude)-nodes->lonzero;
 int     delta=0,count;
 index_t i,best=~0;

 /* Start with the bin containing the location, then spiral outwards. */

 do
   {
    int latb,lonb,llbin;

    count=0;
   
    for(latb=latbin-delta;latb<=latbin+delta;latb++)
       for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
         {
          if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
             continue;

          llbin=lonb*nodes->latbins+latb;

          if(llbin<0 || llbin>nodes->number)
             continue;

          if(delta>0)
            {
             float lat1=(float)((nodes->latzero+latb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
             float lon1=(float)((nodes->lonzero+lonb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
             float lat2=(float)((nodes->latzero+latb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;
             float lon2=(float)((nodes->lonzero+lonb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;

             if(latb==latbin)
               {
                distance_t dist1=Distance(latitude,lon1,latitude,longitude);
                distance_t dist2=Distance(latitude,lon2,latitude,longitude);

                if(dist1>*distance && dist2>*distance)
                   continue;
               }
             else if(lonb==lonbin)
               {
                distance_t dist1=Distance(lat1,longitude,latitude,longitude);
                distance_t dist2=Distance(lat2,longitude,latitude,longitude);

                if(dist1>*distance && dist2>*distance)
                   continue;
               }
             else
               {
                distance_t dist1=Distance(lat1,lon1,latitude,longitude);
                distance_t dist2=Distance(lat2,lon1,latitude,longitude);
                distance_t dist3=Distance(lat2,lon2,latitude,longitude);
                distance_t dist4=Distance(lat1,lon2,latitude,longitude);

                if(dist1>*distance && dist2>*distance && dist3>*distance && dist4>*distance)
                   continue;
               }
            }

          for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
            {
             float lat=(float)((nodes->latzero+latbin)*LAT_LONG_BIN+nodes->nodes[i].latoffset)/LAT_LONG_SCALE;
             float lon=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+nodes->nodes[i].lonoffset)/LAT_LONG_SCALE;

             distance_t dist=Distance(lat,lon,latitude,longitude);

             if(dist<*distance)
               {best=i; *distance=dist;}
            }

          count++;
         }

    delta++;
   }
 while(count);

 if(best==~0)
    return(NULL);
 else
    return(&nodes->nodes[best]);
}


/*++++++++++++++++++++++++++++++++++++++
  Get the latitude and longitude associated with a node.

  Nodes *nodes The set of nodes.

  Node *node The node.

  float *latitude Returns the latitude.

  float *longitude Returns the logitude.
  ++++++++++++++++++++++++++++++++++++++*/

void GetLatLong(Nodes *nodes,Node *node,float *latitude,float *longitude)
{
 index_t index=IndexNode(nodes,node);
 int latbin=-1,lonbin=-1;
 int start,end,mid;

 /* Binary search - search key closest below is required.
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  Since an inexact match is wanted we must set end=mid-1
  *  # <- mid    |  or start=mid 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.
  */

 /* Search for longitude */

 start=0;
 end=nodes->lonbins-1;

 do
   {
    mid=(start+end)/2;                                 /* Choose mid point */

    if(nodes->offsets[nodes->latbins*mid]<index)       /* Mid point is too low */
       start=mid;
    else if(nodes->offsets[nodes->latbins*mid]>index)  /* Mid point is too high */
       end=mid-1;
    else                                               /* Mid point is correct */
      {lonbin=mid;break;}
   }
 while((end-start)>1);

 if(lonbin==-1)
   {
    if(nodes->offsets[nodes->latbins*end]>index)
       lonbin=start;
    else
       lonbin=end;
   }

 while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
    lonbin++;

 /* Search for latitude */

 start=0;
 end=nodes->latbins-1;

 do
   {
    mid=(start+end)/2;                                       /* Choose mid point */

    if(nodes->offsets[lonbin*nodes->latbins+mid]<index)      /* Mid point is too low */
       start=mid;
    else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
       end=mid-1;
    else                                                     /* Mid point is correct */
      {latbin=mid;break;}
   }
 while((end-start)>1);

 if(latbin==-1)
   {
    if(nodes->offsets[lonbin*nodes->latbins+end]>index)
       latbin=start;
    else
       latbin=end;
   }

 while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
    latbin++;

 /* Return the values */

 *latitude =(float)((nodes->latzero+latbin)*LAT_LONG_BIN+node->latoffset)/LAT_LONG_SCALE;
 *longitude=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+node->lonoffset)/LAT_LONG_SCALE;
}
1	amb	2	/***************************************
2	amb	124	$Header: /home/amb/CVS/routino/src/nodes.c,v 1.25 2009-02-15 16:19:28 amb Exp $
3	amb	2
4			Node data type functions.
5			****************/ /****************
6			Written by Andrew M. Bishop
7
8	amb	4	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 <stdlib.h>
16	amb	118	#include <math.h>
17	amb	2
18	amb	109	#include "nodes.h"
19	amb	114	#include "segments.h"
20	amb	2	#include "functions.h"
21
22
23	amb	17	/*++++++++++++++++++++++++++++++++++++++
24	amb	2	Load in a node list from a file.
25
26	amb	19	Nodes* LoadNodeList Returns the node list.
27	amb	17
28	amb	2	const char *filename The name of the file to load.
29			++++++++++++++++++++++++++++++++++++++*/
30
31	amb	19	Nodes LoadNodeList(const char filename)
32	amb	2	{
33	amb	88	void *data;
34			Nodes *nodes;
35
36			nodes=(Nodes*)malloc(sizeof(Nodes));
37
38			data=MapFile(filename);
39
40	amb	100	if(!data)
41			return(NULL);
42
43	amb	88	/* Copy the Nodes structure from the loaded data */
44
45			nodes=((Nodes*)data);
46
47			/* Adjust the pointers in the Nodes structure. */
48
49	amb	99	nodes->data=data;
50			nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
51	amb	88	nodes->nodes=(Node*)(data+(off_t)nodes->nodes);
52
53			return(nodes);
54	amb	2	}
55
56
57			/*++++++++++++++++++++++++++++++++++++++
58	amb	99	Find a node given its latitude and longitude.
59
60			Node *FindNode Returns the node.
61
62			Nodes* nodes The set of nodes to search.
63
64			float latitude The latitude to look for.
65
66			float longitude The longitude to look for.
67	amb	124
68			distance_t distance The maximum distance to look, returns the final distance.
69	amb	99	++++++++++++++++++++++++++++++++++++++*/
70
71	amb	124	Node FindNode(Nodes nodes,float latitude,float longitude,distance_t *distance)
72	amb	99	{
73	amb	118	int32_t latbin=lat_long_to_bin(latitude )-nodes->latzero;
74			int32_t lonbin=lat_long_to_bin(longitude)-nodes->lonzero;
75	amb	124	int delta=0,count;
76			index_t i,best=~0;
77	amb	99
78	amb	124	/* Start with the bin containing the location, then spiral outwards. */
79	amb	118
80	amb	124	do
81	amb	99	{
82	amb	124	int latb,lonb,llbin;
83	amb	99
84	amb	124	count=0;
85
86			for(latb=latbin-delta;latb<=latbin+delta;latb++)
87			for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
88			{
89			if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
90			continue;
91	amb	99
92	amb	124	llbin=lonb*nodes->latbins+latb;
93
94			if(llbin<0 \|\| llbin>nodes->number)
95			continue;
96
97			if(delta>0)
98			{
99			float lat1=(float)((nodes->latzero+latb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
100			float lon1=(float)((nodes->lonzero+lonb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
101			float lat2=(float)((nodes->latzero+latb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;
102			float lon2=(float)((nodes->lonzero+lonb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;
103
104			if(latb==latbin)
105			{
106			distance_t dist1=Distance(latitude,lon1,latitude,longitude);
107			distance_t dist2=Distance(latitude,lon2,latitude,longitude);
108
109			if(dist1>distance && dist2>distance)
110			continue;
111			}
112			else if(lonb==lonbin)
113			{
114			distance_t dist1=Distance(lat1,longitude,latitude,longitude);
115			distance_t dist2=Distance(lat2,longitude,latitude,longitude);
116
117			if(dist1>distance && dist2>distance)
118			continue;
119			}
120			else
121			{
122			distance_t dist1=Distance(lat1,lon1,latitude,longitude);
123			distance_t dist2=Distance(lat2,lon1,latitude,longitude);
124			distance_t dist3=Distance(lat2,lon2,latitude,longitude);
125			distance_t dist4=Distance(lat1,lon2,latitude,longitude);
126
127			if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
128			continue;
129			}
130			}
131
132			for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
133			{
134			float lat=(float)((nodes->latzero+latbin)*LAT_LONG_BIN+nodes->nodes[i].latoffset)/LAT_LONG_SCALE;
135			float lon=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+nodes->nodes[i].lonoffset)/LAT_LONG_SCALE;
136
137			distance_t dist=Distance(lat,lon,latitude,longitude);
138
139			if(dist<*distance)
140			{best=i; *distance=dist;}
141			}
142
143			count++;
144			}
145
146			delta++;
147	amb	99	}
148	amb	124	while(count);
149	amb	99
150	amb	108	if(best==~0)
151			return(NULL);
152			else
153			return(&nodes->nodes[best]);
154	amb	99	}
155
156
157			/*++++++++++++++++++++++++++++++++++++++
158			Get the latitude and longitude associated with a node.
159
160			Nodes *nodes The set of nodes.
161
162			Node *node The node.
163
164			float *latitude Returns the latitude.
165
166			float *longitude Returns the logitude.
167			++++++++++++++++++++++++++++++++++++++*/
168
169			void GetLatLong(Nodes nodes,Node node,float latitude,float longitude)
170			{
171			index_t index=IndexNode(nodes,node);
172			int latbin=-1,lonbin=-1;
173			int start,end,mid;
174
175			/* Binary search - search key closest below is required.
176			*
177			* # <- start \| Check mid and move start or end if it doesn't match
178			* # \|
179			* # \| Since an inexact match is wanted we must set end=mid-1
180			* # <- mid \| or start=mid because we know that mid doesn't match.
181			* # \|
182			* # \| Eventually either end=start or end=start+1 and one of
183			* # <- end \| start or end is the wanted one.
184			*/
185
186			/* Search for longitude */
187
188			start=0;
189			end=nodes->lonbins-1;
190
191			do
192			{
193			mid=(start+end)/2; /* Choose mid point */
194
195			if(nodes->offsets[nodes->latbinsmid]<index) / Mid point is too low */
196			start=mid;
197			else if(nodes->offsets[nodes->latbinsmid]>index) / Mid point is too high */
198			end=mid-1;
199			else /* Mid point is correct */
200			{lonbin=mid;break;}
201			}
202			while((end-start)>1);
203
204			if(lonbin==-1)
205			{
206			if(nodes->offsets[nodes->latbins*end]>index)
207			lonbin=start;
208			else
209			lonbin=end;
210			}
211
212			while(lonbin<nodes->lonbins && nodes->offsets[lonbinnodes->latbins]==nodes->offsets[(lonbin+1)nodes->latbins])
213			lonbin++;
214
215			/* Search for latitude */
216
217			start=0;
218			end=nodes->latbins-1;
219
220			do
221			{
222			mid=(start+end)/2; /* Choose mid point */
223
224			if(nodes->offsets[lonbinnodes->latbins+mid]<index) / Mid point is too low */
225			start=mid;
226			else if(nodes->offsets[lonbinnodes->latbins+mid]>index) / Mid point is too high */
227			end=mid-1;
228			else /* Mid point is correct */
229			{latbin=mid;break;}
230			}
231			while((end-start)>1);
232
233			if(latbin==-1)
234			{
235			if(nodes->offsets[lonbin*nodes->latbins+end]>index)
236			latbin=start;
237			else
238			latbin=end;
239			}
240
241			while(latbin<nodes->latbins && nodes->offsets[lonbinnodes->latbins+latbin]==nodes->offsets[lonbinnodes->latbins+latbin+1])
242			latbin++;
243
244			/* Return the values */
245
246	amb	118	latitude =(float)((nodes->latzero+latbin)LAT_LONG_BIN+node->latoffset)/LAT_LONG_SCALE;
247			longitude=(float)((nodes->lonzero+lonbin)LAT_LONG_BIN+node->lonoffset)/LAT_LONG_SCALE;
248	amb	99	}