Routino SVN Repository Browser

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

/[routino]/trunk/src/nodes.c
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.
Index: This Site Routino Software Download Source Code Documentation XML Files Links
Index: View SVN Development Recent changes Development (destination-access branch) Version 3.4.1 (destination-access branch) Version 3.4.1 Version 3.4 (destination-access branch) Version 3.4 Version 3.3.3 (destination-access branch) Version 3.3.3 Version 3.3.2 (destination-access branch) Version 3.3.2 Version 3.3.1 (destination-access branch) Version 3.3.1 Version 3.3 (destination-access branch) Version 3.3 Version 3.2 Version 3.1.1 Version 3.1 Version 3.0 Version 2.7.3 Version 2.7.2 Version 2.7.1 Version 2.7 Version 2.6 Version 2.5.1 Version 2.5 Version 2.4.1 Version 2.4 Version 2.3.2 Version 2.3.1 Version 2.3 Version 2.2 Version 2.1.2 Version 2.1.1 Version 2.1 Version 2.0.3 Version 2.0.2 Version 2.0.1 Version 2.0 Version 1.5.1 Version 1.5 Version 1.4.1 Version 1.4 Version 1.3 Version 1.2 Version 1.1 Version 1.0
Powered by ViewVC 1.3.0-dev ViewVC Help
© Andrew M. Bishop = < amb @ ... >
Root