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 681 - (show annotations) (download) (as text)
Sun Apr 24 18:01:24 2011 UTC (13 years, 10 months ago) by amb
File MIME type: text/x-csrc
File size: 17338 byte(s) 
Make the comments more consistent.
1 /***************************************
2 Node data type functions.
3
4 Part of the Routino routing software.
5 ******************/ /******************
6 This file Copyright 2008-2011 Andrew M. Bishop
7
8 This program is free software: you can redistribute it and/or modify
9 it under the terms of the GNU Affero General Public License as published by
10 the Free Software Foundation, either version 3 of the License, or
11 (at your option) any later version.
12
13 This program is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU Affero General Public License for more details.
17
18 You should have received a copy of the GNU Affero General Public License
19 along with this program. If not, see <http://www.gnu.org/licenses/>.
20 ***************************************/
21
22
23 #include <sys/types.h>
24 #include <stdlib.h>
25 #include <math.h>
26
27 #include "nodes.h"
28 #include "segments.h"
29 #include "ways.h"
30
31 #include "files.h"
32 #include "profiles.h"
33
34
35 /*++++++++++++++++++++++++++++++++++++++
36 Load in a node list from a file.
37
38 Nodes *LoadNodeList Returns the node list.
39
40 const char *filename The name of the file to load.
41 ++++++++++++++++++++++++++++++++++++++*/
42
43 Nodes *LoadNodeList(const char *filename)
44 {
45 Nodes *nodes;
46
47 nodes=(Nodes*)malloc(sizeof(Nodes));
48
49 #if !SLIM
50
51 nodes->data=MapFile(filename);
52
53 /* Copy the NodesFile header structure from the loaded data */
54
55 nodes->file=*((NodesFile*)nodes->data);
56
57 /* Set the pointers in the Nodes structure. */
58
59 nodes->offsets=(index_t*)(nodes->data+sizeof(NodesFile));
60 nodes->nodes =(Node* )(nodes->data+sizeof(NodesFile)+(nodes->file.latbins*nodes->file.lonbins+1)*sizeof(index_t));
61
62 #else
63
64 nodes->fd=ReOpenFile(filename);
65
66 /* Copy the NodesFile header structure from the loaded data */
67
68 ReadFile(nodes->fd,&nodes->file,sizeof(NodesFile));
69
70 nodes->nodesoffset=sizeof(NodesFile)+(nodes->file.latbins*nodes->file.lonbins+1)*sizeof(index_t);
71
72 nodes->incache[0]=NO_NODE;
73 nodes->incache[1]=NO_NODE;
74 nodes->incache[2]=NO_NODE;
75
76 #endif
77
78 return(nodes);
79 }
80
81
82 /*++++++++++++++++++++++++++++++++++++++
83 Find the closest node given its latitude, longitude and optionally the profile
84 of the mode of transport that must be able to move to/from this node.
85
86 index_t FindClosestNode Returns the closest node.
87
88 Nodes *nodes The set of nodes to search.
89
90 Segments *segments The set of segments to use.
91
92 Ways *ways The set of ways to use.
93
94 double latitude The latitude to look for.
95
96 double longitude The longitude to look for.
97
98 distance_t distance The maximum distance to look from the specified coordinates.
99
100 Profile *profile The profile of the mode of transport (or NULL).
101
102 distance_t *bestdist Returns the distance to the best node.
103 ++++++++++++++++++++++++++++++++++++++*/
104
105 index_t FindClosestNode(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
106 distance_t distance,Profile *profile,distance_t *bestdist)
107 {
108 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
109 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
110 int delta=0,count;
111 index_t i,index1,index2;
112 index_t bestn=NO_NODE;
113 distance_t bestd=INF_DISTANCE;
114
115 /* Start with the bin containing the location, then spiral outwards. */
116
117 do
118 {
119 int latb,lonb,llbin;
120
121 count=0;
122
123 for(latb=latbin-delta;latb<=latbin+delta;latb++)
124 {
125 if(latb<0 || latb>=nodes->file.latbins)
126 continue;
127
128 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
129 {
130 if(lonb<0 || lonb>=nodes->file.lonbins)
131 continue;
132
133 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
134 continue;
135
136 llbin=lonb*nodes->file.latbins+latb;
137
138 /* Check if this grid square has any hope of being close enough */
139
140 if(delta>0)
141 {
142 double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
143 double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
144 double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
145 double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));
146
147 if(latb==latbin)
148 {
149 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
150 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
151
152 if(dist1>distance && dist2>distance)
153 continue;
154 }
155 else if(lonb==lonbin)
156 {
157 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
158 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
159
160 if(dist1>distance && dist2>distance)
161 continue;
162 }
163 else
164 {
165 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
166 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
167 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
168 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
169
170 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
171 continue;
172 }
173 }
174
175 /* Check every node in this grid square. */
176
177 index1=LookupNodeOffset(nodes,llbin);
178 index2=LookupNodeOffset(nodes,llbin+1);
179
180 for(i=index1;i<index2;i++)
181 {
182 Node *node=LookupNode(nodes,i,1);
183 double lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
184 double lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
185
186 distance_t dist=Distance(lat,lon,latitude,longitude);
187
188 if(dist<distance)
189 {
190 if(profile)
191 {
192 Segment *segment;
193
194 /* Decide if this is node is valid for the profile */
195
196 segment=FirstSegment(segments,nodes,i);
197
198 do
199 {
200 Way *way=LookupWay(ways,segment->way,1);
201
202 if(way->allow&profile->allow)
203 break;
204
205 segment=NextSegment(segments,segment,i);
206 }
207 while(segment);
208
209 if(!segment)
210 continue;
211 }
212
213 bestn=i; bestd=distance=dist;
214 }
215 }
216
217 count++;
218 }
219 }
220
221 delta++;
222 }
223 while(count);
224
225 *bestdist=bestd;
226
227 return(bestn);
228 }
229
230
231 /*++++++++++++++++++++++++++++++++++++++
232 Find the closest point on the closest segment given its latitude, longitude
233 and optionally the profile of the mode of transport that must be able to move
234 along this segment.
235
236 index_t FindClosestSegment Returns the closest segment index.
237
238 Nodes *nodes The set of nodes to use.
239
240 Segments *segments The set of segments to search.
241
242 Ways *ways The set of ways to use.
243
244 double latitude The latitude to look for.
245
246 double longitude The longitude to look for.
247
248 distance_t distance The maximum distance to look from the specified coordinates.
249
250 Profile *profile The profile of the mode of transport (or NULL).
251
252 distance_t *bestdist Returns the distance to the closest point on the best segment.
253
254 index_t *bestnode1 Returns the index of the node at one end of the closest segment.
255
256 index_t *bestnode2 Returns the index of the node at the other end of the closest segment.
257
258 distance_t *bestdist1 Returns the distance along the segment to the node at one end.
259
260 distance_t *bestdist2 Returns the distance along the segment to the node at the other end.
261 ++++++++++++++++++++++++++++++++++++++*/
262
263 index_t FindClosestSegment(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
264 distance_t distance,Profile *profile, distance_t *bestdist,
265 index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
266 {
267 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
268 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
269 int delta=0,count;
270 index_t i,index1,index2;
271 index_t bestn1=NO_NODE,bestn2=NO_NODE;
272 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
273 index_t bests=NO_SEGMENT;
274
275 /* Start with the bin containing the location, then spiral outwards. */
276
277 do
278 {
279 int latb,lonb,llbin;
280
281 count=0;
282
283 for(latb=latbin-delta;latb<=latbin+delta;latb++)
284 {
285 if(latb<0 || latb>=nodes->file.latbins)
286 continue;
287
288 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
289 {
290 if(lonb<0 || lonb>=nodes->file.lonbins)
291 continue;
292
293 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
294 continue;
295
296 llbin=lonb*nodes->file.latbins+latb;
297
298 /* Check if this grid square has any hope of being close enough */
299
300 if(delta>0)
301 {
302 double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
303 double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
304 double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
305 double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));
306
307 if(latb==latbin)
308 {
309 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
310 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
311
312 if(dist1>distance && dist2>distance)
313 continue;
314 }
315 else if(lonb==lonbin)
316 {
317 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
318 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
319
320 if(dist1>distance && dist2>distance)
321 continue;
322 }
323 else
324 {
325 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
326 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
327 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
328 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
329
330 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
331 continue;
332 }
333 }
334
335 /* Check every node in this grid square. */
336
337 index1=LookupNodeOffset(nodes,llbin);
338 index2=LookupNodeOffset(nodes,llbin+1);
339
340 for(i=index1;i<index2;i++)
341 {
342 Node *node=LookupNode(nodes,i,1);
343 double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
344 double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
345 distance_t dist1;
346
347 dist1=Distance(lat1,lon1,latitude,longitude);
348
349 if(dist1<distance)
350 {
351 Segment *segment;
352
353 /* Check each segment for closeness and if valid for the profile */
354
355 segment=FirstSegment(segments,nodes,i);
356
357 do
358 {
359 if(IsNormalSegment(segment))
360 {
361 Way *way=NULL;
362
363 if(profile)
364 way=LookupWay(ways,segment->way,1);
365
366 if(!profile || way->allow&profile->allow)
367 {
368 distance_t dist2,dist3;
369 double lat2,lon2,dist3a,dist3b,distp;
370
371 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
372
373 dist2=Distance(lat2,lon2,latitude,longitude);
374
375 dist3=Distance(lat1,lon1,lat2,lon2);
376
377 /* Use law of cosines (assume flat Earth) */
378
379 dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
380 dist3b=(double)dist3-dist3a;
381
382 if((dist1+dist2)<dist3)
383 {
384 distp=0;
385 }
386 else if(dist3a>=0 && dist3b>=0)
387 distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
388 else if(dist3a>0)
389 {
390 distp=dist2;
391 dist3a=dist3;
392 dist3b=0;
393 }
394 else /* if(dist3b>0) */
395 {
396 distp=dist1;
397 dist3a=0;
398 dist3b=dist3;
399 }
400
401 if(distp<(double)bestd)
402 {
403 bests=IndexSegment(segments,segment);
404
405 if(segment->node1==i)
406 {
407 bestn1=i;
408 bestn2=OtherNode(segment,i);
409 bestd1=(distance_t)dist3a;
410 bestd2=(distance_t)dist3b;
411 }
412 else
413 {
414 bestn1=OtherNode(segment,i);
415 bestn2=i;
416 bestd1=(distance_t)dist3b;
417 bestd2=(distance_t)dist3a;
418 }
419
420 bestd=(distance_t)distp;
421 }
422 }
423 }
424
425 segment=NextSegment(segments,segment,i);
426 }
427 while(segment);
428 }
429
430 } /* dist1 < distance */
431
432 count++;
433 }
434 }
435
436 delta++;
437 }
438 while(count);
439
440 *bestdist=bestd;
441
442 *bestnode1=bestn1;
443 *bestnode2=bestn2;
444 *bestdist1=bestd1;
445 *bestdist2=bestd2;
446
447 return(bests);
448 }
449
450
451 /*++++++++++++++++++++++++++++++++++++++
452 Get the latitude and longitude associated with a node.
453
454 Nodes *nodes The set of nodes to use.
455
456 index_t index The node index.
457
458 double *latitude Returns the latitude.
459
460 double *longitude Returns the logitude.
461 ++++++++++++++++++++++++++++++++++++++*/
462
463 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
464 {
465 Node *node=LookupNode(nodes,index,2);
466 int latbin=-1,lonbin=-1;
467 int start,end,mid;
468 index_t offset;
469
470 /* Binary search - search key exact match only is required.
471 *
472 * # <- start | Check mid and move start or end if it doesn't match
473 * # |
474 * # | Since an exact match is wanted we can set end=mid-1
475 * # <- mid | or start=mid+1 because we know that mid doesn't match.
476 * # |
477 * # | Eventually either end=start or end=start+1 and one of
478 * # <- end | start or end is the wanted one.
479 */
480
481 /* Search for longitude */
482
483 start=0;
484 end=nodes->file.lonbins-1;
485
486 do
487 {
488 mid=(start+end)/2; /* Choose mid point */
489
490 offset=LookupNodeOffset(nodes,nodes->file.latbins*mid);
491
492 if(offset<index) /* Mid point is too low */
493 start=mid;
494 else if(offset>index) /* Mid point is too high */
495 end=mid-1;
496 else /* Mid point is correct */
497 {lonbin=mid;break;}
498 }
499 while((end-start)>1);
500
501 if(lonbin==-1)
502 {
503 offset=LookupNodeOffset(nodes,nodes->file.latbins*end);
504
505 if(offset>index)
506 lonbin=start;
507 else
508 lonbin=end;
509 }
510
511 while(lonbin<nodes->file.lonbins &&
512 LookupNodeOffset(nodes,lonbin*nodes->file.latbins)==LookupNodeOffset(nodes,(lonbin+1)*nodes->file.latbins))
513 lonbin++;
514
515 /* Search for latitude */
516
517 start=0;
518 end=nodes->file.latbins-1;
519
520 do
521 {
522 mid=(start+end)/2; /* Choose mid point */
523
524 offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+mid);
525
526 if(offset<index) /* Mid point is too low */
527 start=mid;
528 else if(offset>index) /* Mid point is too high */
529 end=mid-1;
530 else /* Mid point is correct */
531 {latbin=mid;break;}
532 }
533 while((end-start)>1);
534
535 if(latbin==-1)
536 {
537 offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+end);
538
539 if(offset>index)
540 latbin=start;
541 else
542 latbin=end;
543 }
544
545 while(latbin<nodes->file.latbins &&
546 LookupNodeOffset(nodes,lonbin*nodes->file.latbins+latbin)==LookupNodeOffset(nodes,lonbin*nodes->file.latbins+latbin+1))
547 latbin++;
548
549 /* Return the values */
550
551 *latitude =latlong_to_radians(bin_to_latlong(nodes->file.latzero+latbin)+off_to_latlong(node->latoffset));
552 *longitude=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonbin)+off_to_latlong(node->lonoffset));
553 }

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