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

Annotation of /trunk/src/nodes.c

Parent Directory Parent Directory | Revision Log Revision Log

Revision 321 - (hide annotations) (download) (as text)
Sat Mar 6 22:07:47 2010 UTC (15 years ago) by amb
File MIME type: text/x-csrc
File size: 15285 byte(s) 
Speed up start/via/stop point search algorithm.
1 amb 2 /***************************************
2 amb 321 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.35 2010-03-06 22:07:41 amb Exp $
3 amb 2
4     Node data type functions.
5 amb 151
6     Part of the Routino routing software.
7 amb 2 ******************/ /******************
8 amb 321 This file Copyright 2008,2009,2010 Andrew M. Bishop
9 amb 2
10 amb 151 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 amb 2 ***************************************/
23    
24    
25 amb 214 #include <sys/types.h>
26 amb 2 #include <stdlib.h>
27 amb 118 #include <math.h>
28 amb 2
29 amb 179 #include "profiles.h"
30 amb 109 #include "nodes.h"
31 amb 114 #include "segments.h"
32 amb 179 #include "ways.h"
33 amb 2 #include "functions.h"
34    
35    
36 amb 17 /*++++++++++++++++++++++++++++++++++++++
37 amb 2 Load in a node list from a file.
38    
39 amb 19 Nodes* LoadNodeList Returns the node list.
40 amb 17
41 amb 2 const char *filename The name of the file to load.
42     ++++++++++++++++++++++++++++++++++++++*/
43    
44 amb 19 Nodes *LoadNodeList(const char *filename)
45 amb 2 {
46 amb 88 void *data;
47     Nodes *nodes;
48    
49     nodes=(Nodes*)malloc(sizeof(Nodes));
50    
51     data=MapFile(filename);
52    
53 amb 100 if(!data)
54     return(NULL);
55    
56 amb 88 /* Copy the Nodes structure from the loaded data */
57    
58     *nodes=*((Nodes*)data);
59    
60     /* Adjust the pointers in the Nodes structure. */
61    
62 amb 99 nodes->data=data;
63     nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
64 amb 88 nodes->nodes=(Node*)(data+(off_t)nodes->nodes);
65    
66     return(nodes);
67 amb 2 }
68    
69    
70     /*++++++++++++++++++++++++++++++++++++++
71 amb 303 Find the closest node given its latitude, longitude and optionally profile.
72 amb 99
73 amb 303 index_t FindClosestNode Returns the closest node.
74 amb 99
75     Nodes* nodes The set of nodes to search.
76    
77 amb 179 Segments *segments The set of segments to use.
78    
79     Ways *ways The set of ways to use.
80    
81 amb 219 double latitude The latitude to look for.
82 amb 99
83 amb 219 double longitude The longitude to look for.
84 amb 124
85 amb 303 distance_t distance The maximum distance to look.
86 amb 179
87 amb 303 Profile *profile The profile of the mode of transport (or NULL).
88    
89     distance_t *bestdist Returns the distance to the best node.
90 amb 99 ++++++++++++++++++++++++++++++++++++++*/
91    
92 amb 303 index_t FindClosestNode(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
93     distance_t distance,Profile *profile,distance_t *bestdist)
94 amb 99 {
95 amb 303 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 amb 99
101 amb 124 /* Start with the bin containing the location, then spiral outwards. */
102 amb 118
103 amb 124 do
104 amb 99 {
105 amb 124 int latb,lonb,llbin;
106 amb 99
107 amb 124 count=0;
108    
109     for(latb=latbin-delta;latb<=latbin+delta;latb++)
110 amb 303 {
111     if(latb<0 || latb>=nodes->latbins)
112     continue;
113    
114 amb 124 for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
115     {
116 amb 303 if(lonb<0 || lonb>=nodes->lonbins)
117     continue;
118    
119 amb 124 if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
120     continue;
121 amb 99
122 amb 124 llbin=lonb*nodes->latbins+latb;
123    
124 amb 303 /* Check if this grid square has any hope of being close enough */
125 amb 124
126     if(delta>0)
127     {
128 amb 223 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 amb 124
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 amb 303 if(dist1>distance && dist2>distance)
139 amb 124 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 amb 303 if(dist1>distance && dist2>distance)
147 amb 124 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 amb 303 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
157 amb 124 continue;
158     }
159     }
160    
161 amb 303 /* Check every node in this grid square. */
162    
163 amb 124 for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
164     {
165 amb 223 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 amb 124
168     distance_t dist=Distance(lat,lon,latitude,longitude);
169    
170 amb 303 if(dist<distance)
171 amb 179 {
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 amb 303 bestn=i; bestd=distance=dist;
196 amb 179 }
197 amb 124 }
198    
199     count++;
200     }
201 amb 303 }
202 amb 124
203     delta++;
204 amb 99 }
205 amb 124 while(count);
206 amb 99
207 amb 303 *bestdist=bestd;
208    
209     return(bestn);
210 amb 99 }
211    
212    
213     /*++++++++++++++++++++++++++++++++++++++
214 amb 303 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 amb 321 double dist1;
321 amb 303
322     dist1=Distance(lat1,lon1,latitude,longitude);
323    
324 amb 321 if(dist1<distance)
325     {
326     Segment *segment;
327 amb 303
328 amb 321 /* Check each segment for closeness and if valid for the profile */
329 amb 303
330 amb 321 segment=FirstSegment(segments,nodes,i);
331    
332     do
333 amb 303 {
334 amb 321 if(IsNormalSegment(segment))
335     {
336     Way *way=NULL;
337 amb 303
338 amb 321 if(profile)
339     way=LookupWay(ways,segment->way);
340 amb 303
341 amb 321 if(!profile || way->allow&profile->allow)
342     {
343     double lat2,lon2;
344     double dist2;
345 amb 303
346 amb 321 GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
347 amb 303
348 amb 321 dist2=Distance(lat2,lon2,latitude,longitude);
349 amb 303
350 amb 321 if(dist2<distance)
351     {
352     double dist3,dist3a,dist3b,distp;
353 amb 303
354 amb 321 dist3=Distance(lat1,lon1,lat2,lon2);
355 amb 303
356 amb 321 /* Use law of cosines (assume flat Earth) */
357 amb 303
358 amb 321 dist3a=(dist1*dist1-dist2*dist2+dist3*dist3)/(2*dist3);
359     dist3b=dist3-dist3a;
360    
361     if(dist3a>=0 && dist3b>=0)
362     {
363     distp=sqrt(dist1*dist1-dist3a*dist3a);
364    
365     if((distance_t)distp<bestd)
366     {
367     bests=segment;
368     bestn1=i;
369     bestn2=OtherNode(segment,i);
370     bestd1=(distance_t)dist3a;
371     bestd2=(distance_t)dist3b;
372     bestd=(distance_t)distp;
373     }
374     }
375    
376     } /* dist2 < distance */
377    
378 amb 303 }
379     }
380 amb 321
381     segment=NextSegment(segments,segment,i);
382 amb 303 }
383 amb 321 while(segment);
384 amb 303 }
385    
386 amb 321 } /* dist1 < distance */
387    
388 amb 303 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 amb 99 Get the latitude and longitude associated with a node.
409    
410     Nodes *nodes The set of nodes.
411    
412 amb 174 index_t index The node index.
413 amb 99
414 amb 219 double *latitude Returns the latitude.
415 amb 99
416 amb 219 double *longitude Returns the logitude.
417 amb 99 ++++++++++++++++++++++++++++++++++++++*/
418    
419 amb 219 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
420 amb 99 {
421 amb 174 Node *node=&nodes->nodes[index];
422 amb 99 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 amb 223 *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 amb 99 }

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