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 845 - (show annotations) (download) (as text)
Wed Sep 7 12:44:13 2011 UTC (13 years, 6 months ago) by amb
File MIME type: text/x-csrc
File size: 18532 byte(s) 
Make stricter checks for closest nodes just like in v2.0.3 for segments.
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 /* Local functions */
36
37 static int valid_segment_for_profile(Ways *ways,Segment *segment,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 int i;
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 nodes->nodesoffset=sizeof(NodesFile)+(nodes->file.latbins*nodes->file.lonbins+1)*sizeof(index_t);
79
80 for(i=0;i<sizeof(nodes->cached)/sizeof(nodes->cached[0]);i++)
81 nodes->incache[i]=NO_NODE;
82
83 #endif
84
85 return(nodes);
86 }
87
88
89 /*++++++++++++++++++++++++++++++++++++++
90 Find the closest node given its latitude, longitude and the profile of the
91 mode of transport that must be able to move to/from this node.
92
93 index_t FindClosestNode Returns the closest node.
94
95 Nodes *nodes The set of nodes to search.
96
97 Segments *segments The set of segments to use.
98
99 Ways *ways The set of ways to use.
100
101 double latitude The latitude to look for.
102
103 double longitude The longitude to look for.
104
105 distance_t distance The maximum distance to look from the specified coordinates.
106
107 Profile *profile The profile of the mode of transport.
108
109 distance_t *bestdist Returns the distance to the best node.
110 ++++++++++++++++++++++++++++++++++++++*/
111
112 index_t FindClosestNode(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
113 distance_t distance,Profile *profile,distance_t *bestdist)
114 {
115 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
116 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
117 int delta=0,count;
118 index_t i,index1,index2;
119 index_t bestn=NO_NODE;
120 distance_t bestd=INF_DISTANCE;
121
122 /* Start with the bin containing the location, then spiral outwards. */
123
124 do
125 {
126 ll_bin_t latb,lonb;
127 ll_bin2_t llbin;
128
129 count=0;
130
131 for(latb=latbin-delta;latb<=latbin+delta;latb++)
132 {
133 if(latb<0 || latb>=nodes->file.latbins)
134 continue;
135
136 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
137 {
138 if(lonb<0 || lonb>=nodes->file.lonbins)
139 continue;
140
141 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
142 continue;
143
144 llbin=lonb*nodes->file.latbins+latb;
145
146 /* Check if this grid square has any hope of being close enough */
147
148 if(delta>0)
149 {
150 double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
151 double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
152 double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
153 double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));
154
155 if(latb==latbin)
156 {
157 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
158 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
159
160 if(dist1>distance && dist2>distance)
161 continue;
162 }
163 else if(lonb==lonbin)
164 {
165 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
166 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
167
168 if(dist1>distance && dist2>distance)
169 continue;
170 }
171 else
172 {
173 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
174 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
175 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
176 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
177
178 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
179 continue;
180 }
181 }
182
183 /* Check every node in this grid square. */
184
185 index1=LookupNodeOffset(nodes,llbin);
186 index2=LookupNodeOffset(nodes,llbin+1);
187
188 for(i=index1;i<index2;i++)
189 {
190 Node *node=LookupNode(nodes,i,1);
191 double lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
192 double lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
193
194 distance_t dist=Distance(lat,lon,latitude,longitude);
195
196 if(dist<distance)
197 {
198 Segment *segment;
199
200 /* Check that at least one segment is valid for the profile */
201
202 segment=FirstSegment(segments,nodes,i,1);
203
204 do
205 {
206 if(IsNormalSegment(segment) && valid_segment_for_profile(ways,segment,profile))
207 {
208 bestn=i;
209 bestd=distance=dist;
210
211 break;
212 }
213
214 segment=NextSegment(segments,segment,i);
215 }
216 while(segment);
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 the profile of the mode of transport that must be able to move along this
237 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.
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) && valid_segment_for_profile(ways,segment,profile))
364 {
365 distance_t dist2,dist3;
366 double lat2,lon2,dist3a,dist3b,distp;
367
368 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
369
370 dist2=Distance(lat2,lon2,latitude,longitude);
371
372 dist3=Distance(lat1,lon1,lat2,lon2);
373
374 /* Use law of cosines (assume flat Earth) */
375
376 dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
377 dist3b=(double)dist3-dist3a;
378
379 if((dist1+dist2)<dist3)
380 {
381 distp=0;
382 }
383 else if(dist3a>=0 && dist3b>=0)
384 distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
385 else if(dist3a>0)
386 {
387 distp=dist2;
388 dist3a=dist3;
389 dist3b=0;
390 }
391 else /* if(dist3b>0) */
392 {
393 distp=dist1;
394 dist3a=0;
395 dist3b=dist3;
396 }
397
398 if(distp<(double)bestd)
399 {
400 bests=IndexSegment(segments,segment);
401
402 if(segment->node1==i)
403 {
404 bestn1=i;
405 bestn2=OtherNode(segment,i);
406 bestd1=(distance_t)dist3a;
407 bestd2=(distance_t)dist3b;
408 }
409 else
410 {
411 bestn1=OtherNode(segment,i);
412 bestn2=i;
413 bestd1=(distance_t)dist3b;
414 bestd2=(distance_t)dist3a;
415 }
416
417 bestd=(distance_t)distp;
418 }
419 }
420
421 segment=NextSegment(segments,segment,i);
422 }
423 while(segment);
424 }
425
426 } /* dist1 < distance */
427
428 count++;
429 }
430 }
431
432 delta++;
433 }
434 while(count);
435
436 *bestdist=bestd;
437
438 *bestnode1=bestn1;
439 *bestnode2=bestn2;
440 *bestdist1=bestd1;
441 *bestdist2=bestd2;
442
443 return(bests);
444 }
445
446
447 /*++++++++++++++++++++++++++++++++++++++
448 Check if the transport defined by the profile is allowed on the segment.
449
450 int valid_segment_for_profile Return 1 if it is or 0 if not.
451
452 Ways *ways The set of ways to use.
453
454 Segment *segment The segment to check.
455
456 Profile *profile The profile to check.
457 ++++++++++++++++++++++++++++++++++++++*/
458
459 static int valid_segment_for_profile(Ways *ways,Segment *segment,Profile *profile)
460 {
461 Way *way=LookupWay(ways,segment->way,1);
462 score_t segment_pref;
463 int i;
464
465 /* mode of transport must be allowed on the highway */
466 if(!(way->allow&profile->allow))
467 return(0);
468
469 /* must obey weight restriction (if exists) */
470 if(way->weight && way->weight<profile->weight)
471 return(0);
472
473 /* must obey height/width/length restriction (if exists) */
474 if((way->height && way->height<profile->height) ||
475 (way->width && way->width <profile->width ) ||
476 (way->length && way->length<profile->length))
477 return(0);
478
479 segment_pref=profile->highway[HIGHWAY(way->type)];
480
481 for(i=1;i<Property_Count;i++)
482 if(ways->file.props & PROPERTIES(i))
483 {
484 if(way->props & PROPERTIES(i))
485 segment_pref*=profile->props_yes[i];
486 else
487 segment_pref*=profile->props_no[i];
488 }
489
490 /* profile preferences must allow this highway */
491 if(segment_pref==0)
492 return(0);
493
494 /* Must be OK */
495 return(1);
496 }
497
498
499 /*++++++++++++++++++++++++++++++++++++++
500 Get the latitude and longitude associated with a node.
501
502 Nodes *nodes The set of nodes to use.
503
504 index_t index The node index.
505
506 double *latitude Returns the latitude.
507
508 double *longitude Returns the logitude.
509 ++++++++++++++++++++++++++++++++++++++*/
510
511 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
512 {
513 Node *node=LookupNode(nodes,index,2);
514 ll_bin_t latbin=-1,lonbin=-1;
515 ll_bin_t start,end,mid;
516 index_t offset;
517
518 /* Binary search - search key nearest below for lon, exact match for lat.
519 *
520 * # <- start | Check mid and move start or end if it doesn't match
521 * # |
522 * # | If an exact match is wanted we can set end=mid-1 or
523 * # <- mid | start=mid+1 because we know that mid doesn't match.
524 * # |
525 * # | Eventually either end=start or end=start+1 and one of
526 * # <- end | start or end is the wanted one.
527 */
528
529 /* Search for longitude - search for row containing node */
530
531 start=0;
532 end=nodes->file.lonbins-1;
533
534 do
535 {
536 mid=(start+end)/2; /* Choose mid point */
537
538 offset=LookupNodeOffset(nodes,nodes->file.latbins*mid);
539
540 if(offset<index) /* Mid point is too low or just right */
541 start=mid;
542 else if(offset>index) /* Mid point is too high */
543 end=mid?(mid-1):mid;
544 else /* Mid point is correct */
545 {lonbin=mid;break;}
546 }
547 while((end-start)>1);
548
549 if(lonbin==-1)
550 {
551 offset=LookupNodeOffset(nodes,nodes->file.latbins*end);
552
553 if(offset>index)
554 lonbin=start;
555 else
556 lonbin=end;
557 }
558
559 while(lonbin<nodes->file.lonbins &&
560 LookupNodeOffset(nodes,lonbin*nodes->file.latbins)==LookupNodeOffset(nodes,(lonbin+1)*nodes->file.latbins))
561 lonbin++;
562
563 /* Search for latitude - search for exact column */
564
565 start=0;
566 end=nodes->file.latbins-1;
567
568 do
569 {
570 mid=(start+end)/2; /* Choose mid point */
571
572 offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+mid);
573
574 if(offset<index) /* Mid point is too low */
575 start=mid+1;
576 else if(offset>index) /* Mid point is too high */
577 end=mid?(mid-1):mid;
578 else /* Mid point is correct */
579 {latbin=mid;break;}
580 }
581 while((end-start)>1);
582
583 if(latbin==-1)
584 {
585 offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+end);
586
587 if(offset>index)
588 latbin=start;
589 else
590 latbin=end;
591 }
592
593 while(latbin<nodes->file.latbins &&
594 LookupNodeOffset(nodes,lonbin*nodes->file.latbins+latbin)==LookupNodeOffset(nodes,lonbin*nodes->file.latbins+latbin+1))
595 latbin++;
596
597 /* Return the values */
598
599 *latitude =latlong_to_radians(bin_to_latlong(nodes->file.latzero+latbin)+off_to_latlong(node->latoffset));
600 *longitude=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonbin)+off_to_latlong(node->lonoffset));
601 }

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