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 303 - (show annotations) (download) (as text)
Sat Nov 14 19:39:20 2009 UTC (15 years, 4 months ago) by amb
File MIME type: text/x-csrc
File size: 14829 byte(s) 
If a selected waypoint is not very close to an existing node then insert a fake
node in the segment that comes closest and use that instead.
1 /***************************************
2 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.34 2009-11-14 19:39:19 amb Exp $
3
4 Node data type functions.
5
6 Part of the Routino routing software.
7 ******************/ /******************
8 This file Copyright 2008,2009 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 if(!data)
54 return(NULL);
55
56 /* Copy the Nodes structure from the loaded data */
57
58 *nodes=*((Nodes*)data);
59
60 /* Adjust the pointers in the Nodes structure. */
61
62 nodes->data=data;
63 nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
64 nodes->nodes=(Node*)(data+(off_t)nodes->nodes);
65
66 return(nodes);
67 }
68
69
70 /*++++++++++++++++++++++++++++++++++++++
71 Find the closest node given its latitude, longitude and optionally profile.
72
73 index_t FindClosestNode Returns the closest node.
74
75 Nodes* nodes The set of nodes to search.
76
77 Segments *segments The set of segments to use.
78
79 Ways *ways The set of ways to use.
80
81 double latitude The latitude to look for.
82
83 double longitude The longitude to look for.
84
85 distance_t distance The maximum distance to look.
86
87 Profile *profile The profile of the mode of transport (or NULL).
88
89 distance_t *bestdist Returns the distance to the best node.
90 ++++++++++++++++++++++++++++++++++++++*/
91
92 index_t FindClosestNode(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
93 distance_t distance,Profile *profile,distance_t *bestdist)
94 {
95 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
96 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
97 int delta=0,count;
98 index_t i,bestn=NO_NODE;
99 distance_t bestd=INF_DISTANCE;
100
101 /* Start with the bin containing the location, then spiral outwards. */
102
103 do
104 {
105 int latb,lonb,llbin;
106
107 count=0;
108
109 for(latb=latbin-delta;latb<=latbin+delta;latb++)
110 {
111 if(latb<0 || latb>=nodes->latbins)
112 continue;
113
114 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
115 {
116 if(lonb<0 || lonb>=nodes->lonbins)
117 continue;
118
119 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
120 continue;
121
122 llbin=lonb*nodes->latbins+latb;
123
124 /* Check if this grid square has any hope of being close enough */
125
126 if(delta>0)
127 {
128 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
129 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
130 double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
131 double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
132
133 if(latb==latbin)
134 {
135 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
136 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
137
138 if(dist1>distance && dist2>distance)
139 continue;
140 }
141 else if(lonb==lonbin)
142 {
143 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
144 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
145
146 if(dist1>distance && dist2>distance)
147 continue;
148 }
149 else
150 {
151 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
152 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
153 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
154 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
155
156 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
157 continue;
158 }
159 }
160
161 /* Check every node in this grid square. */
162
163 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
164 {
165 double lat=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
166 double lon=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
167
168 distance_t dist=Distance(lat,lon,latitude,longitude);
169
170 if(dist<distance)
171 {
172 if(profile)
173 {
174 Segment *segment;
175
176 /* Decide if this is node is valid for the profile */
177
178 segment=FirstSegment(segments,nodes,i);
179
180 do
181 {
182 Way *way=LookupWay(ways,segment->way);
183
184 if(way->allow&profile->allow)
185 break;
186
187 segment=NextSegment(segments,segment,i);
188 }
189 while(segment);
190
191 if(!segment)
192 continue;
193 }
194
195 bestn=i; bestd=distance=dist;
196 }
197 }
198
199 count++;
200 }
201 }
202
203 delta++;
204 }
205 while(count);
206
207 *bestdist=bestd;
208
209 return(bestn);
210 }
211
212
213 /*++++++++++++++++++++++++++++++++++++++
214 Find the closest segment to a latitude, longitude and optionally profile.
215
216 Segment *FindClosestSegment Returns the closest segment.
217
218 Nodes* nodes The set of nodes to search.
219
220 Segments *segments The set of segments to use.
221
222 Ways *ways The set of ways to use.
223
224 double latitude The latitude to look for.
225
226 double longitude The longitude to look for.
227
228 distance_t distance The maximum distance to look.
229
230 Profile *profile The profile of the mode of transport (or NULL).
231
232 distance_t *bestdist Returns the distance to the best segment.
233
234 index_t *bestnode1 Returns the best node at one end.
235
236 index_t *bestnode2 Returns the best node at the other end.
237
238 distance_t *bestdist1 Returns the distance to the best node at one end.
239
240 distance_t *bestdist2 Returns the distance to the best node at the other end.
241 ++++++++++++++++++++++++++++++++++++++*/
242
243 Segment *FindClosestSegment(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
244 distance_t distance,Profile *profile, distance_t *bestdist,
245 index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
246 {
247 ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
248 ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
249 int delta=0,count;
250 index_t i,bestn1=NO_NODE,bestn2=NO_NODE;
251 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
252 Segment *bests=NULL;
253
254 /* Start with the bin containing the location, then spiral outwards. */
255
256 do
257 {
258 int latb,lonb,llbin;
259
260 count=0;
261
262 for(latb=latbin-delta;latb<=latbin+delta;latb++)
263 {
264 if(latb<0 || latb>=nodes->latbins)
265 continue;
266
267 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
268 {
269 if(lonb<0 || lonb>=nodes->lonbins)
270 continue;
271
272 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
273 continue;
274
275 llbin=lonb*nodes->latbins+latb;
276
277 /* Check if this grid square has any hope of being close enough */
278
279 if(delta>0)
280 {
281 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
282 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
283 double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
284 double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
285
286 if(latb==latbin)
287 {
288 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
289 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
290
291 if(dist1>distance && dist2>distance)
292 continue;
293 }
294 else if(lonb==lonbin)
295 {
296 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
297 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
298
299 if(dist1>distance && dist2>distance)
300 continue;
301 }
302 else
303 {
304 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
305 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
306 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
307 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
308
309 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
310 continue;
311 }
312 }
313
314 /* Check every node in this grid square. */
315
316 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
317 {
318 double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
319 double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
320 Segment *segment;
321 double dist1,dist2,dist3,dist3a,dist3b,distp;
322
323 dist1=Distance(lat1,lon1,latitude,longitude);
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 double lat2,lon2;
341
342 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
343
344 dist2=Distance(lat2,lon2,latitude,longitude);
345 dist3=Distance(lat1,lon1,lat2,lon2);
346
347 /* Use law of cosines (assume flat Earth) */
348
349 dist3a=(dist1*dist1-dist2*dist2+dist3*dist3)/(2*dist3);
350 dist3b=dist3-dist3a;
351
352 if(dist3a>=0 && dist3b>=0)
353 {
354 distp=sqrt(dist1*dist1-dist3a*dist3a);
355
356 if((distance_t)distp<bestd)
357 {
358 bests=segment;
359 bestn1=i;
360 bestn2=OtherNode(segment,i);
361 bestd1=(distance_t)dist3a;
362 bestd2=(distance_t)dist3b;
363 bestd=(distance_t)distp;
364 }
365 }
366 }
367 }
368
369 segment=NextSegment(segments,segment,i);
370 }
371 while(segment);
372 }
373
374 count++;
375 }
376 }
377
378 delta++;
379 }
380 while(count);
381
382 *bestdist=bestd;
383
384 *bestnode1=bestn1;
385 *bestnode2=bestn2;
386 *bestdist1=bestd1;
387 *bestdist2=bestd2;
388
389 return(bests);
390 }
391
392
393 /*++++++++++++++++++++++++++++++++++++++
394 Get the latitude and longitude associated with a node.
395
396 Nodes *nodes The set of nodes.
397
398 index_t index The node index.
399
400 double *latitude Returns the latitude.
401
402 double *longitude Returns the logitude.
403 ++++++++++++++++++++++++++++++++++++++*/
404
405 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
406 {
407 Node *node=&nodes->nodes[index];
408 int latbin=-1,lonbin=-1;
409 int start,end,mid;
410
411 /* Binary search - search key closest below is required.
412 *
413 * # <- start | Check mid and move start or end if it doesn't match
414 * # |
415 * # | Since an inexact match is wanted we must set end=mid-1
416 * # <- mid | or start=mid because we know that mid doesn't match.
417 * # |
418 * # | Eventually either end=start or end=start+1 and one of
419 * # <- end | start or end is the wanted one.
420 */
421
422 /* Search for longitude */
423
424 start=0;
425 end=nodes->lonbins-1;
426
427 do
428 {
429 mid=(start+end)/2; /* Choose mid point */
430
431 if(nodes->offsets[nodes->latbins*mid]<index) /* Mid point is too low */
432 start=mid;
433 else if(nodes->offsets[nodes->latbins*mid]>index) /* Mid point is too high */
434 end=mid-1;
435 else /* Mid point is correct */
436 {lonbin=mid;break;}
437 }
438 while((end-start)>1);
439
440 if(lonbin==-1)
441 {
442 if(nodes->offsets[nodes->latbins*end]>index)
443 lonbin=start;
444 else
445 lonbin=end;
446 }
447
448 while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
449 lonbin++;
450
451 /* Search for latitude */
452
453 start=0;
454 end=nodes->latbins-1;
455
456 do
457 {
458 mid=(start+end)/2; /* Choose mid point */
459
460 if(nodes->offsets[lonbin*nodes->latbins+mid]<index) /* Mid point is too low */
461 start=mid;
462 else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
463 end=mid-1;
464 else /* Mid point is correct */
465 {latbin=mid;break;}
466 }
467 while((end-start)>1);
468
469 if(latbin==-1)
470 {
471 if(nodes->offsets[lonbin*nodes->latbins+end]>index)
472 latbin=start;
473 else
474 latbin=end;
475 }
476
477 while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
478 latbin++;
479
480 /* Return the values */
481
482 *latitude =latlong_to_radians(bin_to_latlong(nodes->latzero+latbin)+off_to_latlong(node->latoffset));
483 *longitude=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonbin)+off_to_latlong(node->lonoffset));
484 }

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