Check out the latest version of Routino: svn co http://routino.org/svn/trunk routino
Contents of /trunk/src/nodes.c
Parent Directory
|
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)
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. |