trunk/src/nodes.c

/***************************************
 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.29 2009-05-14 18:02:29 amb Exp $

 Node data type functions.

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008,2009 Andrew M. Bishop

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU Affero General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Affero General Public License for more details.

 You should have received a copy of the GNU Affero General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ***************************************/


#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.

  index_t FindNode Returns the node index.

  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.
  ++++++++++++++++++++++++++++++++++++++*/

index_t 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=NO_NODE;

 /* 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->latbins*nodes->lonbins))
             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+latb)*LAT_LONG_BIN+nodes->nodes[i].latoffset)/LAT_LONG_SCALE;
             float lon=(float)((nodes->lonzero+lonb)*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);

 return(best);
}


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

  Nodes *nodes The set of nodes.

  index_t index The node index.

  float *latitude Returns the latitude.

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

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