Routino SVN Repository Browser

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

/[routino]/branches/libroutino/src/nodes.c
ViewVC logotype

Contents of /branches/libroutino/src/nodes.c

Parent Directory Parent Directory | Revision Log Revision Log

Revision 1742 - (show annotations) (download) (as text)
Sat Jul 11 15:42:22 2015 UTC (9 years, 8 months ago) by amb
File MIME type: text/x-csrc
File size: 18447 byte(s) 
Merge change from MS-Windows branch (size_t and offset_t).
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);
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
155 index_t FindClosestNode(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
156 distance_t distance,Profile *profile,distance_t *bestdist)
157 {
158 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
159 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
160 int delta=0,count;
161 index_t i,index1,index2;
162 index_t bestn=NO_NODE;
163 distance_t bestd=INF_DISTANCE;
164
165 /* Find the maximum distance to search */
166
167 double dlat=DeltaLat(longitude,distance);
168 double dlon=DeltaLon(latitude ,distance);
169
170 double minlat=latitude -dlat;
171 double maxlat=latitude +dlat;
172 double minlon=longitude-dlon;
173 double maxlon=longitude+dlon;
174
175 ll_bin_t minlatbin=latlong_to_bin(radians_to_latlong(minlat))-nodes->file.latzero;
176 ll_bin_t maxlatbin=latlong_to_bin(radians_to_latlong(maxlat))-nodes->file.latzero;
177 ll_bin_t minlonbin=latlong_to_bin(radians_to_latlong(minlon))-nodes->file.lonzero;
178 ll_bin_t maxlonbin=latlong_to_bin(radians_to_latlong(maxlon))-nodes->file.lonzero;
179
180 ll_off_t minlatoff=latlong_to_off(radians_to_latlong(minlat));
181 ll_off_t maxlatoff=latlong_to_off(radians_to_latlong(maxlat));
182 ll_off_t minlonoff=latlong_to_off(radians_to_latlong(minlon));
183 ll_off_t maxlonoff=latlong_to_off(radians_to_latlong(maxlon));
184
185 /* Start with the bin containing the location, then spiral outwards. */
186
187 do
188 {
189 ll_bin_t latb,lonb;
190 ll_bin2_t llbin;
191
192 count=0;
193
194 for(latb=latbin-delta;latb<=latbin+delta;latb++)
195 {
196 if(latb<0 || latb>=nodes->file.latbins || latb<minlatbin || latb>maxlatbin)
197 continue;
198
199 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
200 {
201 if(lonb<0 || lonb>=nodes->file.lonbins || lonb<minlonbin || lonb>maxlonbin)
202 continue;
203
204 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
205 continue;
206
207 /* Check every node in this grid square. */
208
209 llbin=lonb*nodes->file.latbins+latb;
210
211 index1=LookupNodeOffset(nodes,llbin);
212 index2=LookupNodeOffset(nodes,llbin+1);
213
214 for(i=index1;i<index2;i++)
215 {
216 Node *nodep=LookupNode(nodes,i,3);
217 double lat,lon;
218 distance_t dist;
219
220 if(latb==minlatbin && nodep->latoffset<minlatoff)
221 continue;
222
223 if(latb==maxlatbin && nodep->latoffset>maxlatoff)
224 continue;
225
226 if(lonb==minlonbin && nodep->lonoffset<minlonoff)
227 continue;
228
229 if(lonb==maxlonbin && nodep->lonoffset>maxlonoff)
230 continue;
231
232 lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(nodep->latoffset));
233 lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(nodep->lonoffset));
234
235 dist=Distance(lat,lon,latitude,longitude);
236
237 if(dist<bestd)
238 {
239 Segment *segmentp;
240
241 /* Check that at least one segment is valid for the profile */
242
243 segmentp=FirstSegment(segments,nodep,1);
244
245 do
246 {
247 if(IsNormalSegment(segmentp) && valid_segment_for_profile(ways,segmentp,profile))
248 {
249 bestn=i;
250 bestd=dist;
251
252 break;
253 }
254
255 segmentp=NextSegment(segments,segmentp,i);
256 }
257 while(segmentp);
258 }
259 }
260
261 count++;
262 }
263 }
264
265 delta++;
266 }
267 while(count);
268
269 *bestdist=bestd;
270
271 return(bestn);
272 }
273
274
275 /*++++++++++++++++++++++++++++++++++++++
276 Find the closest point on the closest segment given its latitude, longitude
277 and the profile of the mode of transport that must be able to move along this
278 segment.
279
280 index_t FindClosestSegment Returns the closest segment index.
281
282 Nodes *nodes The set of nodes to use.
283
284 Segments *segments The set of segments to search.
285
286 Ways *ways The set of ways to use.
287
288 double latitude The latitude to look for.
289
290 double longitude The longitude to look for.
291
292 distance_t distance The maximum distance to look from the specified coordinates.
293
294 Profile *profile The profile of the mode of transport.
295
296 distance_t *bestdist Returns the distance to the closest point on the best segment.
297
298 index_t *bestnode1 Returns the index of the node at one end of the closest segment.
299
300 index_t *bestnode2 Returns the index of the node at the other end of the closest segment.
301
302 distance_t *bestdist1 Returns the distance along the segment to the node at one end.
303
304 distance_t *bestdist2 Returns the distance along the segment to the node at the other end.
305 ++++++++++++++++++++++++++++++++++++++*/
306
307 index_t FindClosestSegment(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
308 distance_t distance,Profile *profile, distance_t *bestdist,
309 index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
310 {
311 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
312 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
313 int delta=0,count;
314 index_t i,index1,index2;
315 index_t bestn1=NO_NODE,bestn2=NO_NODE;
316 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
317 index_t bests=NO_SEGMENT;
318
319 /* Find the maximum distance to search */
320
321 double dlat=DeltaLat(longitude,distance);
322 double dlon=DeltaLon(latitude ,distance);
323
324 double minlat=latitude -dlat;
325 double maxlat=latitude +dlat;
326 double minlon=longitude-dlon;
327 double maxlon=longitude+dlon;
328
329 ll_bin_t minlatbin=latlong_to_bin(radians_to_latlong(minlat))-nodes->file.latzero;
330 ll_bin_t maxlatbin=latlong_to_bin(radians_to_latlong(maxlat))-nodes->file.latzero;
331 ll_bin_t minlonbin=latlong_to_bin(radians_to_latlong(minlon))-nodes->file.lonzero;
332 ll_bin_t maxlonbin=latlong_to_bin(radians_to_latlong(maxlon))-nodes->file.lonzero;
333
334 ll_off_t minlatoff=latlong_to_off(radians_to_latlong(minlat));
335 ll_off_t maxlatoff=latlong_to_off(radians_to_latlong(maxlat));
336 ll_off_t minlonoff=latlong_to_off(radians_to_latlong(minlon));
337 ll_off_t maxlonoff=latlong_to_off(radians_to_latlong(maxlon));
338
339 /* Start with the bin containing the location, then spiral outwards. */
340
341 do
342 {
343 ll_bin_t latb,lonb;
344 ll_bin2_t llbin;
345
346 count=0;
347
348 for(latb=latbin-delta;latb<=latbin+delta;latb++)
349 {
350 if(latb<0 || latb>=nodes->file.latbins || latb<minlatbin || latb>maxlatbin)
351 continue;
352
353 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
354 {
355 if(lonb<0 || lonb>=nodes->file.lonbins || lonb<minlonbin || lonb>maxlonbin)
356 continue;
357
358 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
359 continue;
360
361 /* Check every node in this grid square. */
362
363 llbin=lonb*nodes->file.latbins+latb;
364
365 index1=LookupNodeOffset(nodes,llbin);
366 index2=LookupNodeOffset(nodes,llbin+1);
367
368 for(i=index1;i<index2;i++)
369 {
370 Node *nodep=LookupNode(nodes,i,3);
371 double lat1,lon1;
372 distance_t dist1;
373
374 if(latb==minlatbin && nodep->latoffset<minlatoff)
375 continue;
376
377 if(latb==maxlatbin && nodep->latoffset>maxlatoff)
378 continue;
379
380 if(lonb==minlonbin && nodep->lonoffset<minlonoff)
381 continue;
382
383 if(lonb==maxlonbin && nodep->lonoffset>maxlonoff)
384 continue;
385
386 lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(nodep->latoffset));
387 lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(nodep->lonoffset));
388
389 dist1=Distance(lat1,lon1,latitude,longitude);
390
391 if(dist1<distance)
392 {
393 Segment *segmentp;
394
395 /* Check each segment for closeness and if valid for the profile */
396
397 segmentp=FirstSegment(segments,nodep,1);
398
399 do
400 {
401 if(IsNormalSegment(segmentp) && valid_segment_for_profile(ways,segmentp,profile))
402 {
403 distance_t dist2,dist3;
404 double lat2,lon2,dist3a,dist3b,distp;
405
406 GetLatLong(nodes,OtherNode(segmentp,i),NULL,&lat2,&lon2);
407
408 dist2=Distance(lat2,lon2,latitude,longitude);
409
410 dist3=Distance(lat1,lon1,lat2,lon2);
411
412 /* Use law of cosines (assume flat Earth) */
413
414 if(dist3==0)
415 {
416 distp=dist1; /* == dist2 */
417 dist3a=dist1; /* == dist2 */
418 dist3b=dist2; /* == dist1 */
419 }
420 else if((dist1+dist2)<dist3)
421 {
422 distp=0;
423 dist3a=dist1;
424 dist3b=dist2;
425 }
426 else
427 {
428 dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
429 dist3b=(double)dist3-dist3a;
430
431 if(dist3a>=0 && dist3b>=0)
432 distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
433 else if(dist3a>0)
434 {
435 distp=dist2;
436 dist3a=dist3;
437 dist3b=0;
438 }
439 else /* if(dist3b>0) */
440 {
441 distp=dist1;
442 dist3a=0;
443 dist3b=dist3;
444 }
445 }
446
447 if(distp<(double)bestd)
448 {
449 bests=IndexSegment(segments,segmentp);
450
451 if(segmentp->node1==i)
452 {
453 bestn1=i;
454 bestn2=OtherNode(segmentp,i);
455 bestd1=(distance_t)dist3a;
456 bestd2=(distance_t)dist3b;
457 }
458 else
459 {
460 bestn1=OtherNode(segmentp,i);
461 bestn2=i;
462 bestd1=(distance_t)dist3b;
463 bestd2=(distance_t)dist3a;
464 }
465
466 bestd=(distance_t)distp;
467 }
468 }
469
470 segmentp=NextSegment(segments,segmentp,i);
471 }
472 while(segmentp);
473
474 } /* dist1 < distance */
475 }
476
477 count++;
478 }
479 }
480
481 delta++;
482 }
483 while(count);
484
485 *bestdist=bestd;
486
487 *bestnode1=bestn1;
488 *bestnode2=bestn2;
489 *bestdist1=bestd1;
490 *bestdist2=bestd2;
491
492 return(bests);
493 }
494
495
496 /*++++++++++++++++++++++++++++++++++++++
497 Check if the transport defined by the profile is allowed on the segment.
498
499 int valid_segment_for_profile Return 1 if it is or 0 if not.
500
501 Ways *ways The set of ways to use.
502
503 Segment *segmentp The segment to check.
504
505 Profile *profile The profile to check.
506 ++++++++++++++++++++++++++++++++++++++*/
507
508 static int valid_segment_for_profile(Ways *ways,Segment *segmentp,Profile *profile)
509 {
510 Way *wayp=LookupWay(ways,segmentp->way,1);
511 score_t segment_pref;
512 int i;
513
514 /* mode of transport must be allowed on the highway */
515 if(!(wayp->allow&profile->allow))
516 return(0);
517
518 /* must obey weight restriction (if exists) */
519 if(wayp->weight && wayp->weight<profile->weight)
520 return(0);
521
522 /* must obey height/width/length restriction (if exists) */
523 if((wayp->height && wayp->height<profile->height) ||
524 (wayp->width && wayp->width <profile->width ) ||
525 (wayp->length && wayp->length<profile->length))
526 return(0);
527
528 segment_pref=profile->highway[HIGHWAY(wayp->type)];
529
530 for(i=1;i<Property_Count;i++)
531 if(ways->file.props & PROPERTIES(i))
532 {
533 if(wayp->props & PROPERTIES(i))
534 segment_pref*=profile->props_yes[i];
535 else
536 segment_pref*=profile->props_no[i];
537 }
538
539 /* profile preferences must allow this highway */
540 if(segment_pref==0)
541 return(0);
542
543 /* Must be OK */
544 return(1);
545 }
546
547
548 /*++++++++++++++++++++++++++++++++++++++
549 Get the latitude and longitude associated with a node.
550
551 Nodes *nodes The set of nodes to use.
552
553 index_t index The node index.
554
555 Node *nodep A pointer to the node if already available.
556
557 double *latitude Returns the latitude.
558
559 double *longitude Returns the logitude.
560 ++++++++++++++++++++++++++++++++++++++*/
561
562 void GetLatLong(Nodes *nodes,index_t index,Node *nodep,double *latitude,double *longitude)
563 {
564 ll_bin_t latbin,lonbin;
565 ll_bin2_t bin=-1;
566 ll_bin2_t start,end,mid;
567 index_t offset;
568
569 /* Binary search - search key nearest match below is required.
570 *
571 * # <- start | Check mid and move start or end if it doesn't match
572 * # |
573 * # | A lower bound match is wanted we can set end=mid-1 or
574 * # <- mid | start=mid because we know that mid doesn't match.
575 * # |
576 * # | Eventually either end=start or end=start+1 and one of
577 * # <- end | start or end is the wanted one.
578 */
579
580 /* Search for offset */
581
582 start=0;
583 end=nodes->file.lonbins*nodes->file.latbins;
584
585 do
586 {
587 mid=(start+end)/2; /* Choose mid point */
588
589 offset=LookupNodeOffset(nodes,mid);
590
591 if(offset<index) /* Mid point is too low for an exact match but could be lower bound */
592 start=mid;
593 else if(offset>index) /* Mid point is too high */
594 end=mid?(mid-1):mid;
595 else /* Mid point is correct */
596 {bin=mid;break;}
597 }
598 while((end-start)>1);
599
600 if(bin==-1)
601 {
602 offset=LookupNodeOffset(nodes,end);
603
604 if(offset>index)
605 bin=start;
606 else
607 bin=end;
608 }
609
610 while(bin<=(nodes->file.lonbins*nodes->file.latbins) &&
611 LookupNodeOffset(nodes,bin)==LookupNodeOffset(nodes,bin+1))
612 bin++;
613
614 latbin=bin%nodes->file.latbins;
615 lonbin=bin/nodes->file.latbins;
616
617 /* Return the values */
618
619 if(nodep==NULL)
620 nodep=LookupNode(nodes,index,4);
621
622 *latitude =latlong_to_radians(bin_to_latlong(nodes->file.latzero+latbin)+off_to_latlong(nodep->latoffset));
623 *longitude=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonbin)+off_to_latlong(nodep->lonoffset));
624 }

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