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

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