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 443 - (show annotations) (download) (as text)
Thu Jul 8 17:54:54 2010 UTC (14 years, 8 months ago) by amb
File MIME type: text/x-csrc
File size: 15524 byte(s) 
Fix error with finding closest segment to the specified point.
1 /***************************************
2 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.39 2010-07-08 17:54:54 amb Exp $
3
4 Node data type functions.
5
6 Part of the Routino routing software.
7 ******************/ /******************
8 This file Copyright 2008-2010 Andrew M. Bishop
9
10 This program is free software: you can redistribute it and/or modify
11 it under the terms of the GNU Affero General Public License as published by
12 the Free Software Foundation, either version 3 of the License, or
13 (at your option) any later version.
14
15 This program is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU Affero General Public License for more details.
19
20 You should have received a copy of the GNU Affero General Public License
21 along with this program. If not, see <http://www.gnu.org/licenses/>.
22 ***************************************/
23
24
25 #include <sys/types.h>
26 #include <stdlib.h>
27 #include <math.h>
28
29 #include "profiles.h"
30 #include "nodes.h"
31 #include "segments.h"
32 #include "ways.h"
33 #include "functions.h"
34
35
36 /*++++++++++++++++++++++++++++++++++++++
37 Load in a node list from a file.
38
39 Nodes* LoadNodeList Returns the node list.
40
41 const char *filename The name of the file to load.
42 ++++++++++++++++++++++++++++++++++++++*/
43
44 Nodes *LoadNodeList(const char *filename)
45 {
46 void *data;
47 Nodes *nodes;
48
49 nodes=(Nodes*)malloc(sizeof(Nodes));
50
51 data=MapFile(filename);
52
53 /* Copy the Nodes structure from the loaded data */
54
55 *nodes=*((Nodes*)data);
56
57 /* Adjust the pointers in the Nodes structure. */
58
59 nodes->data=data;
60 nodes->offsets=(index_t*)(data+sizeof(Nodes));
61 nodes->nodes=(Node*)(data+(sizeof(Nodes)+(nodes->latbins*nodes->lonbins+1)*sizeof(index_t)));
62
63 return(nodes);
64 }
65
66
67 /*++++++++++++++++++++++++++++++++++++++
68 Find the closest node given its latitude, longitude and optionally profile.
69
70 index_t FindClosestNode Returns the closest node.
71
72 Nodes* nodes The set of nodes to search.
73
74 Segments *segments The set of segments to use.
75
76 Ways *ways The set of ways to use.
77
78 double latitude The latitude to look for.
79
80 double longitude The longitude to look for.
81
82 distance_t distance The maximum distance to look.
83
84 Profile *profile The profile of the mode of transport (or NULL).
85
86 distance_t *bestdist Returns the distance to the best node.
87 ++++++++++++++++++++++++++++++++++++++*/
88
89 index_t FindClosestNode(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
90 distance_t distance,Profile *profile,distance_t *bestdist)
91 {
92 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
93 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
94 int delta=0,count;
95 index_t i,bestn=NO_NODE;
96 distance_t bestd=INF_DISTANCE;
97
98 /* Start with the bin containing the location, then spiral outwards. */
99
100 do
101 {
102 int latb,lonb,llbin;
103
104 count=0;
105
106 for(latb=latbin-delta;latb<=latbin+delta;latb++)
107 {
108 if(latb<0 || latb>=nodes->latbins)
109 continue;
110
111 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
112 {
113 if(lonb<0 || lonb>=nodes->lonbins)
114 continue;
115
116 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
117 continue;
118
119 llbin=lonb*nodes->latbins+latb;
120
121 /* Check if this grid square has any hope of being close enough */
122
123 if(delta>0)
124 {
125 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
126 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
127 double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
128 double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
129
130 if(latb==latbin)
131 {
132 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
133 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
134
135 if(dist1>distance && dist2>distance)
136 continue;
137 }
138 else if(lonb==lonbin)
139 {
140 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
141 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
142
143 if(dist1>distance && dist2>distance)
144 continue;
145 }
146 else
147 {
148 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
149 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
150 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
151 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
152
153 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
154 continue;
155 }
156 }
157
158 /* Check every node in this grid square. */
159
160 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
161 {
162 double lat=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
163 double lon=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
164
165 distance_t dist=Distance(lat,lon,latitude,longitude);
166
167 if(dist<distance)
168 {
169 if(profile)
170 {
171 Segment *segment;
172
173 /* Decide if this is node is valid for the profile */
174
175 segment=FirstSegment(segments,nodes,i);
176
177 do
178 {
179 Way *way=LookupWay(ways,segment->way);
180
181 if(way->allow&profile->allow)
182 break;
183
184 segment=NextSegment(segments,segment,i);
185 }
186 while(segment);
187
188 if(!segment)
189 continue;
190 }
191
192 bestn=i; bestd=distance=dist;
193 }
194 }
195
196 count++;
197 }
198 }
199
200 delta++;
201 }
202 while(count);
203
204 *bestdist=bestd;
205
206 return(bestn);
207 }
208
209
210 /*++++++++++++++++++++++++++++++++++++++
211 Find the closest segment to a latitude, longitude and optionally profile.
212
213 Segment *FindClosestSegment Returns the closest segment.
214
215 Nodes* nodes The set of nodes to search.
216
217 Segments *segments The set of segments to use.
218
219 Ways *ways The set of ways to use.
220
221 double latitude The latitude to look for.
222
223 double longitude The longitude to look for.
224
225 distance_t distance The maximum distance to look.
226
227 Profile *profile The profile of the mode of transport (or NULL).
228
229 distance_t *bestdist Returns the distance to the best segment.
230
231 index_t *bestnode1 Returns the best node at one end.
232
233 index_t *bestnode2 Returns the best node at the other end.
234
235 distance_t *bestdist1 Returns the distance to the best node at one end.
236
237 distance_t *bestdist2 Returns the distance to the best node at the other end.
238 ++++++++++++++++++++++++++++++++++++++*/
239
240 Segment *FindClosestSegment(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
241 distance_t distance,Profile *profile, distance_t *bestdist,
242 index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
243 {
244 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
245 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
246 int delta=0,count;
247 index_t i,bestn1=NO_NODE,bestn2=NO_NODE;
248 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
249 Segment *bests=NULL;
250
251 /* Start with the bin containing the location, then spiral outwards. */
252
253 do
254 {
255 int latb,lonb,llbin;
256
257 count=0;
258
259 for(latb=latbin-delta;latb<=latbin+delta;latb++)
260 {
261 if(latb<0 || latb>=nodes->latbins)
262 continue;
263
264 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
265 {
266 if(lonb<0 || lonb>=nodes->lonbins)
267 continue;
268
269 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
270 continue;
271
272 llbin=lonb*nodes->latbins+latb;
273
274 /* Check if this grid square has any hope of being close enough */
275
276 if(delta>0)
277 {
278 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
279 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
280 double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
281 double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
282
283 if(latb==latbin)
284 {
285 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
286 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
287
288 if(dist1>distance && dist2>distance)
289 continue;
290 }
291 else if(lonb==lonbin)
292 {
293 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
294 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
295
296 if(dist1>distance && dist2>distance)
297 continue;
298 }
299 else
300 {
301 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
302 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
303 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
304 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
305
306 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
307 continue;
308 }
309 }
310
311 /* Check every node in this grid square. */
312
313 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
314 {
315 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
316 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
317 distance_t dist1;
318
319 dist1=Distance(lat1,lon1,latitude,longitude);
320
321 if(dist1<distance)
322 {
323 Segment *segment;
324
325 /* Check each segment for closeness and if valid for the profile */
326
327 segment=FirstSegment(segments,nodes,i);
328
329 do
330 {
331 if(IsNormalSegment(segment))
332 {
333 Way *way=NULL;
334
335 if(profile)
336 way=LookupWay(ways,segment->way);
337
338 if(!profile || way->allow&profile->allow)
339 {
340 distance_t dist2,dist3;
341 double lat2,lon2,dist3a,dist3b,distp;
342
343 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
344
345 dist2=Distance(lat2,lon2,latitude,longitude);
346
347 dist3=Distance(lat1,lon1,lat2,lon2);
348
349 /* Use law of cosines (assume flat Earth) */
350
351 dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
352 dist3b=(double)dist3-dist3a;
353
354 if(dist3a>=0 && dist3b>=0)
355 distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
356 else if(dist3a>0)
357 {
358 distp=dist2;
359 dist3a=dist3;
360 dist3b=0;
361 }
362 else /* if(dist3b>0) */
363 {
364 distp=dist1;
365 dist3a=0;
366 dist3b=dist3;
367 }
368
369 if(distp<(double)bestd)
370 {
371 bests=segment;
372 bestn1=i;
373 bestn2=OtherNode(segment,i);
374 bestd1=(distance_t)dist3a;
375 bestd2=(distance_t)dist3b;
376 bestd=(distance_t)distp;
377 }
378 }
379 }
380
381 segment=NextSegment(segments,segment,i);
382 }
383 while(segment);
384 }
385
386 } /* dist1 < distance */
387
388 count++;
389 }
390 }
391
392 delta++;
393 }
394 while(count);
395
396 *bestdist=bestd;
397
398 *bestnode1=bestn1;
399 *bestnode2=bestn2;
400 *bestdist1=bestd1;
401 *bestdist2=bestd2;
402
403 return(bests);
404 }
405
406
407 /*++++++++++++++++++++++++++++++++++++++
408 Get the latitude and longitude associated with a node.
409
410 Nodes *nodes The set of nodes.
411
412 index_t index The node index.
413
414 double *latitude Returns the latitude.
415
416 double *longitude Returns the logitude.
417 ++++++++++++++++++++++++++++++++++++++*/
418
419 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
420 {
421 Node *node=&nodes->nodes[index];
422 int latbin=-1,lonbin=-1;
423 int start,end,mid;
424
425 /* Binary search - search key closest below is required.
426 *
427 * # <- start | Check mid and move start or end if it doesn't match
428 * # |
429 * # | Since an inexact match is wanted we must set end=mid-1
430 * # <- mid | or start=mid because we know that mid doesn't match.
431 * # |
432 * # | Eventually either end=start or end=start+1 and one of
433 * # <- end | start or end is the wanted one.
434 */
435
436 /* Search for longitude */
437
438 start=0;
439 end=nodes->lonbins-1;
440
441 do
442 {
443 mid=(start+end)/2; /* Choose mid point */
444
445 if(nodes->offsets[nodes->latbins*mid]<index) /* Mid point is too low */
446 start=mid;
447 else if(nodes->offsets[nodes->latbins*mid]>index) /* Mid point is too high */
448 end=mid-1;
449 else /* Mid point is correct */
450 {lonbin=mid;break;}
451 }
452 while((end-start)>1);
453
454 if(lonbin==-1)
455 {
456 if(nodes->offsets[nodes->latbins*end]>index)
457 lonbin=start;
458 else
459 lonbin=end;
460 }
461
462 while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
463 lonbin++;
464
465 /* Search for latitude */
466
467 start=0;
468 end=nodes->latbins-1;
469
470 do
471 {
472 mid=(start+end)/2; /* Choose mid point */
473
474 if(nodes->offsets[lonbin*nodes->latbins+mid]<index) /* Mid point is too low */
475 start=mid;
476 else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
477 end=mid-1;
478 else /* Mid point is correct */
479 {latbin=mid;break;}
480 }
481 while((end-start)>1);
482
483 if(latbin==-1)
484 {
485 if(nodes->offsets[lonbin*nodes->latbins+end]>index)
486 latbin=start;
487 else
488 latbin=end;
489 }
490
491 while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
492 latbin++;
493
494 /* Return the values */
495
496 *latitude =latlong_to_radians(bin_to_latlong(nodes->latzero+latbin)+off_to_latlong(node->latoffset));
497 *longitude=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonbin)+off_to_latlong(node->lonoffset));
498 }

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