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

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