trunk/src/nodes.c

/***************************************
 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.31 2009-07-02 17:49:16 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 <sys/types.h>
#include <stdlib.h>
#include <math.h>

#include "profiles.h"
#include "nodes.h"
#include "segments.h"
#include "ways.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 the closest node given its latitude and longitude and optionally profile.

  index_t FindNode Returns the node index.

  Nodes* nodes The set of nodes to search.

  Segments *segments The set of segments to use.

  Ways *ways The set of ways to use.

  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.

  Profile *profile The profile of the mode of transport.
  ++++++++++++++++++++++++++++++++++++++*/

index_t FindNode(Nodes* nodes,Segments *segments,Ways *ways,float latitude,float longitude,distance_t *distance,Profile *profile)
{
 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)
               {
                if(profile)
                  {
                   Segment *segment;

                   /* Decide if this is node is valid for the profile */

                   segment=FirstSegment(segments,nodes,i);

                   do
                     {
                      Way *way=LookupWay(ways,segment->way);

                      if(way->allow&profile->allow)
                         break;

                      segment=NextSegment(segments,segment,i);
                     }
                   while(segment);

                   if(!segment)
                      continue;
                  }

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