Routino SVN Repository Browser

Check out the latest version of Routino: svn co http://routino.org/svn/trunk routino

ViewVC logotype

Contents of /trunk/src/nodes.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 385 - (show annotations) (download) (as text)
Wed Apr 28 17:27:02 2010 UTC (14 years, 10 months ago) by amb
File MIME type: text/x-csrc
File size: 15288 byte(s)
Change file format to allow 64-bit off_t type with 32 bit void* type.

1 /***************************************
2 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.37 2010-04-28 17:27:02 amb Exp $
3
4 Node data type functions.
5
6 Part of the Routino routing software.
7 ******************/ /******************
8 This file Copyright 2008-2010 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 /* 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+sizeof(Nodes));
61 nodes->nodes=(Node*)(data+(sizeof(Nodes)+(nodes->latbins*nodes->lonbins+1)*sizeof(index_t)));
62
63 return(nodes);
64 }
65
66
67 /*++++++++++++++++++++++++++++++++++++++
68 Find the closest node given its latitude, longitude and optionally profile.
69
70 index_t FindClosestNode Returns the closest node.
71
72 Nodes* nodes The set of nodes to search.
73
74 Segments *segments The set of segments to use.
75
76 Ways *ways The set of ways to use.
77
78 double latitude The latitude to look for.
79
80 double longitude The longitude to look for.
81
82 distance_t distance The maximum distance to look.
83
84 Profile *profile The profile of the mode of transport (or NULL).
85
86 distance_t *bestdist Returns the distance to the best node.
87 ++++++++++++++++++++++++++++++++++++++*/
88
89 index_t FindClosestNode(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
90 distance_t distance,Profile *profile,distance_t *bestdist)
91 {
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;
94 int delta=0,count;
95 index_t i,bestn=NO_NODE;
96 distance_t bestd=INF_DISTANCE;
97
98 /* Start with the bin containing the location, then spiral outwards. */
99
100 do
101 {
102 int latb,lonb,llbin;
103
104 count=0;
105
106 for(latb=latbin-delta;latb<=latbin+delta;latb++)
107 {
108 if(latb<0 || latb>=nodes->latbins)
109 continue;
110
111 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
112 {
113 if(lonb<0 || lonb>=nodes->lonbins)
114 continue;
115
116 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
117 continue;
118
119 llbin=lonb*nodes->latbins+latb;
120
121 /* Check if this grid square has any hope of being close enough */
122
123 if(delta>0)
124 {
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));
129
130 if(latb==latbin)
131 {
132 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
133 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
134
135 if(dist1>distance && dist2>distance)
136 continue;
137 }
138 else if(lonb==lonbin)
139 {
140 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
141 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
142
143 if(dist1>distance && dist2>distance)
144 continue;
145 }
146 else
147 {
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);
152
153 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
154 continue;
155 }
156 }
157
158 /* Check every node in this grid square. */
159
160 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
161 {
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));
164
165 distance_t dist=Distance(lat,lon,latitude,longitude);
166
167 if(dist<distance)
168 {
169 if(profile)
170 {
171 Segment *segment;
172
173 /* Decide if this is node is valid for the profile */
174
175 segment=FirstSegment(segments,nodes,i);
176
177 do
178 {
179 Way *way=LookupWay(ways,segment->way);
180
181 if(way->allow&profile->allow)
182 break;
183
184 segment=NextSegment(segments,segment,i);
185 }
186 while(segment);
187
188 if(!segment)
189 continue;
190 }
191
192 bestn=i; bestd=distance=dist;
193 }
194 }
195
196 count++;
197 }
198 }
199
200 delta++;
201 }
202 while(count);
203
204 *bestdist=bestd;
205
206 return(bestn);
207 }
208
209
210 /*++++++++++++++++++++++++++++++++++++++
211 Find the closest segment to a latitude, longitude and optionally profile.
212
213 Segment *FindClosestSegment Returns the closest segment.
214
215 Nodes* nodes The set of nodes to search.
216
217 Segments *segments The set of segments to use.
218
219 Ways *ways The set of ways to use.
220
221 double latitude The latitude to look for.
222
223 double longitude The longitude to look for.
224
225 distance_t distance The maximum distance to look.
226
227 Profile *profile The profile of the mode of transport (or NULL).
228
229 distance_t *bestdist Returns the distance to the best segment.
230
231 index_t *bestnode1 Returns the best node at one end.
232
233 index_t *bestnode2 Returns the best node at the other end.
234
235 distance_t *bestdist1 Returns the distance to the best node at one end.
236
237 distance_t *bestdist2 Returns the distance to the best node at the other end.
238 ++++++++++++++++++++++++++++++++++++++*/
239
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)
243 {
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;
246 int delta=0,count;
247 index_t i,bestn1=NO_NODE,bestn2=NO_NODE;
248 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
249 Segment *bests=NULL;
250
251 /* Start with the bin containing the location, then spiral outwards. */
252
253 do
254 {
255 int latb,lonb,llbin;
256
257 count=0;
258
259 for(latb=latbin-delta;latb<=latbin+delta;latb++)
260 {
261 if(latb<0 || latb>=nodes->latbins)
262 continue;
263
264 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
265 {
266 if(lonb<0 || lonb>=nodes->lonbins)
267 continue;
268
269 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
270 continue;
271
272 llbin=lonb*nodes->latbins+latb;
273
274 /* Check if this grid square has any hope of being close enough */
275
276 if(delta>0)
277 {
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));
282
283 if(latb==latbin)
284 {
285 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
286 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
287
288 if(dist1>distance && dist2>distance)
289 continue;
290 }
291 else if(lonb==lonbin)
292 {
293 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
294 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
295
296 if(dist1>distance && dist2>distance)
297 continue;
298 }
299 else
300 {
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);
305
306 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
307 continue;
308 }
309 }
310
311 /* Check every node in this grid square. */
312
313 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
314 {
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));
317 double dist1;
318
319 dist1=Distance(lat1,lon1,latitude,longitude);
320
321 if(dist1<distance)
322 {
323 Segment *segment;
324
325 /* Check each segment for closeness and if valid for the profile */
326
327 segment=FirstSegment(segments,nodes,i);
328
329 do
330 {
331 if(IsNormalSegment(segment))
332 {
333 Way *way=NULL;
334
335 if(profile)
336 way=LookupWay(ways,segment->way);
337
338 if(!profile || way->allow&profile->allow)
339 {
340 double lat2,lon2;
341 double dist2;
342
343 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
344
345 dist2=Distance(lat2,lon2,latitude,longitude);
346
347 if(dist2<distance)
348 {
349 double dist3,dist3a,dist3b,distp;
350
351 dist3=Distance(lat1,lon1,lat2,lon2);
352
353 /* Use law of cosines (assume flat Earth) */
354
355 dist3a=(dist1*dist1-dist2*dist2+dist3*dist3)/(2*dist3);
356 dist3b=dist3-dist3a;
357
358 if(dist3a>=0 && dist3b>=0)
359 {
360 distp=sqrt(dist1*dist1-dist3a*dist3a);
361
362 if((distance_t)distp<bestd)
363 {
364 bests=segment;
365 bestn1=i;
366 bestn2=OtherNode(segment,i);
367 bestd1=(distance_t)dist3a;
368 bestd2=(distance_t)dist3b;
369 bestd=(distance_t)distp;
370 }
371 }
372
373 } /* dist2 < distance */
374
375 }
376 }
377
378 segment=NextSegment(segments,segment,i);
379 }
380 while(segment);
381 }
382
383 } /* dist1 < distance */
384
385 count++;
386 }
387 }
388
389 delta++;
390 }
391 while(count);
392
393 *bestdist=bestd;
394
395 *bestnode1=bestn1;
396 *bestnode2=bestn2;
397 *bestdist1=bestd1;
398 *bestdist2=bestd2;
399
400 return(bests);
401 }
402
403
404 /*++++++++++++++++++++++++++++++++++++++
405 Get the latitude and longitude associated with a node.
406
407 Nodes *nodes The set of nodes.
408
409 index_t index The node index.
410
411 double *latitude Returns the latitude.
412
413 double *longitude Returns the logitude.
414 ++++++++++++++++++++++++++++++++++++++*/
415
416 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
417 {
418 Node *node=&nodes->nodes[index];
419 int latbin=-1,lonbin=-1;
420 int start,end,mid;
421
422 /* Binary search - search key closest below is required.
423 *
424 * # <- start | Check mid and move start or end if it doesn't match
425 * # |
426 * # | Since an inexact match is wanted we must set end=mid-1
427 * # <- mid | or start=mid because we know that mid doesn't match.
428 * # |
429 * # | Eventually either end=start or end=start+1 and one of
430 * # <- end | start or end is the wanted one.
431 */
432
433 /* Search for longitude */
434
435 start=0;
436 end=nodes->lonbins-1;
437
438 do
439 {
440 mid=(start+end)/2; /* Choose mid point */
441
442 if(nodes->offsets[nodes->latbins*mid]<index) /* Mid point is too low */
443 start=mid;
444 else if(nodes->offsets[nodes->latbins*mid]>index) /* Mid point is too high */
445 end=mid-1;
446 else /* Mid point is correct */
447 {lonbin=mid;break;}
448 }
449 while((end-start)>1);
450
451 if(lonbin==-1)
452 {
453 if(nodes->offsets[nodes->latbins*end]>index)
454 lonbin=start;
455 else
456 lonbin=end;
457 }
458
459 while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
460 lonbin++;
461
462 /* Search for latitude */
463
464 start=0;
465 end=nodes->latbins-1;
466
467 do
468 {
469 mid=(start+end)/2; /* Choose mid point */
470
471 if(nodes->offsets[lonbin*nodes->latbins+mid]<index) /* Mid point is too low */
472 start=mid;
473 else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
474 end=mid-1;
475 else /* Mid point is correct */
476 {latbin=mid;break;}
477 }
478 while((end-start)>1);
479
480 if(latbin==-1)
481 {
482 if(nodes->offsets[lonbin*nodes->latbins+end]>index)
483 latbin=start;
484 else
485 latbin=end;
486 }
487
488 while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
489 latbin++;
490
491 /* Return the values */
492
493 *latitude =latlong_to_radians(bin_to_latlong(nodes->latzero+latbin)+off_to_latlong(node->latoffset));
494 *longitude=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonbin)+off_to_latlong(node->lonoffset));
495 }

Properties

Name Value
cvs:description Node data type.