1 /***************************************
2 $Header: /home/amb/routino/src/RCS/nodes.c,v 1.39 2010/07/08 17:54:54 amb Exp $
4 Node data type functions.
6 Part of the Routino routing software.
7 ******************/ /******************
8 This file Copyright 2008-2010 Andrew M. Bishop
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.
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.
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 ***************************************/
25 #include <sys/types.h>
33 #include "functions.h"
36 /*++++++++++++++++++++++++++++++++++++++
37 Load in a node list from a file.
39 Nodes* LoadNodeList Returns the node list.
41 const char *filename The name of the file to load.
42 ++++++++++++++++++++++++++++++++++++++*/
44 Nodes *LoadNodeList(const char *filename)
49 nodes=(Nodes*)malloc(sizeof(Nodes));
51 data=MapFile(filename);
53 /* Copy the Nodes structure from the loaded data */
55 *nodes=*((Nodes*)data);
57 /* Adjust the pointers in the Nodes structure. */
60 nodes->offsets=(index_t*)(data+sizeof(Nodes));
61 nodes->nodes=(Node*)(data+(sizeof(Nodes)+(nodes->latbins*nodes->lonbins+1)*sizeof(index_t)));
67 /*++++++++++++++++++++++++++++++++++++++
68 Find the closest node given its latitude, longitude and optionally profile.
70 index_t FindClosestNode Returns the closest node.
72 Nodes* nodes The set of nodes to search.
74 Segments *segments The set of segments to use.
76 Ways *ways The set of ways to use.
78 double latitude The latitude to look for.
80 double longitude The longitude to look for.
82 distance_t distance The maximum distance to look.
84 Profile *profile The profile of the mode of transport (or NULL).
86 distance_t *bestdist Returns the distance to the best node.
87 ++++++++++++++++++++++++++++++++++++++*/
89 index_t FindClosestNode(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
90 distance_t distance,Profile *profile,distance_t *bestdist)
92 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
93 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
95 index_t i,bestn=NO_NODE;
96 distance_t bestd=INF_DISTANCE;
98 /* Start with the bin containing the location, then spiral outwards. */
106 for(latb=latbin-delta;latb<=latbin+delta;latb++)
108 if(latb<0 || latb>=nodes->latbins)
111 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
113 if(lonb<0 || lonb>=nodes->lonbins)
116 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
119 llbin=lonb*nodes->latbins+latb;
121 /* Check if this grid square has any hope of being close enough */
125 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
126 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
127 double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
128 double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
132 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
133 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
135 if(dist1>distance && dist2>distance)
138 else if(lonb==lonbin)
140 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
141 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
143 if(dist1>distance && dist2>distance)
148 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
149 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
150 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
151 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
153 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
158 /* Check every node in this grid square. */
160 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
162 double lat=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
163 double lon=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
165 distance_t dist=Distance(lat,lon,latitude,longitude);
173 /* Decide if this is node is valid for the profile */
175 segment=FirstSegment(segments,nodes,i);
179 Way *way=LookupWay(ways,segment->way);
181 if(way->allow&profile->allow)
184 segment=NextSegment(segments,segment,i);
192 bestn=i; bestd=distance=dist;
210 /*++++++++++++++++++++++++++++++++++++++
211 Find the closest segment to a latitude, longitude and optionally profile.
213 Segment *FindClosestSegment Returns the closest segment.
215 Nodes* nodes The set of nodes to search.
217 Segments *segments The set of segments to use.
219 Ways *ways The set of ways to use.
221 double latitude The latitude to look for.
223 double longitude The longitude to look for.
225 distance_t distance The maximum distance to look.
227 Profile *profile The profile of the mode of transport (or NULL).
229 distance_t *bestdist Returns the distance to the best segment.
231 index_t *bestnode1 Returns the best node at one end.
233 index_t *bestnode2 Returns the best node at the other end.
235 distance_t *bestdist1 Returns the distance to the best node at one end.
237 distance_t *bestdist2 Returns the distance to the best node at the other end.
238 ++++++++++++++++++++++++++++++++++++++*/
240 Segment *FindClosestSegment(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
241 distance_t distance,Profile *profile, distance_t *bestdist,
242 index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
244 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
245 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
247 index_t i,bestn1=NO_NODE,bestn2=NO_NODE;
248 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
251 /* Start with the bin containing the location, then spiral outwards. */
259 for(latb=latbin-delta;latb<=latbin+delta;latb++)
261 if(latb<0 || latb>=nodes->latbins)
264 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
266 if(lonb<0 || lonb>=nodes->lonbins)
269 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
272 llbin=lonb*nodes->latbins+latb;
274 /* Check if this grid square has any hope of being close enough */
278 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
279 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
280 double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
281 double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
285 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
286 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
288 if(dist1>distance && dist2>distance)
291 else if(lonb==lonbin)
293 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
294 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
296 if(dist1>distance && dist2>distance)
301 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
302 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
303 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
304 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
306 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
311 /* Check every node in this grid square. */
313 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
315 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
316 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
319 dist1=Distance(lat1,lon1,latitude,longitude);
325 /* Check each segment for closeness and if valid for the profile */
327 segment=FirstSegment(segments,nodes,i);
331 if(IsNormalSegment(segment))
336 way=LookupWay(ways,segment->way);
338 if(!profile || way->allow&profile->allow)
340 distance_t dist2,dist3;
341 double lat2,lon2,dist3a,dist3b,distp;
343 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
345 dist2=Distance(lat2,lon2,latitude,longitude);
347 dist3=Distance(lat1,lon1,lat2,lon2);
349 /* Use law of cosines (assume flat Earth) */
351 dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
352 dist3b=(double)dist3-dist3a;
354 if(dist3a>=0 && dist3b>=0)
355 distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
362 else /* if(dist3b>0) */
369 if(distp<(double)bestd)
373 bestn2=OtherNode(segment,i);
374 bestd1=(distance_t)dist3a;
375 bestd2=(distance_t)dist3b;
376 bestd=(distance_t)distp;
381 segment=NextSegment(segments,segment,i);
386 } /* dist1 < distance */
407 /*++++++++++++++++++++++++++++++++++++++
408 Get the latitude and longitude associated with a node.
410 Nodes *nodes The set of nodes.
412 index_t index The node index.
414 double *latitude Returns the latitude.
416 double *longitude Returns the logitude.
417 ++++++++++++++++++++++++++++++++++++++*/
419 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
421 Node *node=&nodes->nodes[index];
422 int latbin=-1,lonbin=-1;
425 /* Binary search - search key closest below is required.
427 * # <- start | Check mid and move start or end if it doesn't match
429 * # | Since an inexact match is wanted we must set end=mid-1
430 * # <- mid | or start=mid because we know that mid doesn't match.
432 * # | Eventually either end=start or end=start+1 and one of
433 * # <- end | start or end is the wanted one.
436 /* Search for longitude */
439 end=nodes->lonbins-1;
443 mid=(start+end)/2; /* Choose mid point */
445 if(nodes->offsets[nodes->latbins*mid]<index) /* Mid point is too low */
447 else if(nodes->offsets[nodes->latbins*mid]>index) /* Mid point is too high */
449 else /* Mid point is correct */
452 while((end-start)>1);
456 if(nodes->offsets[nodes->latbins*end]>index)
462 while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
465 /* Search for latitude */
468 end=nodes->latbins-1;
472 mid=(start+end)/2; /* Choose mid point */
474 if(nodes->offsets[lonbin*nodes->latbins+mid]<index) /* Mid point is too low */
476 else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
478 else /* Mid point is correct */
481 while((end-start)>1);
485 if(nodes->offsets[lonbin*nodes->latbins+end]>index)
491 while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
494 /* Return the values */
496 *latitude =latlong_to_radians(bin_to_latlong(nodes->latzero+latbin)+off_to_latlong(node->latoffset));
497 *longitude=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonbin)+off_to_latlong(node->lonoffset));