Routino SVN Repository Browser

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

/[routino]/branches/destination-access/src/nodes.c
ViewVC logotype

Contents of /branches/destination-access/src/nodes.c

Parent Directory Parent Directory | Revision Log Revision Log

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

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.2 (destination-access branch) Version 3.4.2 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