trunk/src/nodes.c

/***************************************
 Node data type functions.

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2011 Andrew M. Bishop

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU Affero General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Affero General Public License for more details.

 You should have received a copy of the GNU Affero General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ***************************************/


#include <sys/types.h>
#include <stdlib.h>
#include <math.h>

#include "nodes.h"
#include "segments.h"
#include "ways.h"

#include "files.h"
#include "profiles.h"


/*++++++++++++++++++++++++++++++++++++++
  Load in a node list from a file.

  Nodes *LoadNodeList Returns the node list.

  const char *filename The name of the file to load.
  ++++++++++++++++++++++++++++++++++++++*/

Nodes *LoadNodeList(const char *filename)
{
 Nodes *nodes;
#if SLIM
 int i;
#endif

 nodes=(Nodes*)malloc(sizeof(Nodes));

#if !SLIM

 nodes->data=MapFile(filename);

 /* Copy the NodesFile header structure from the loaded data */

 nodes->file=*((NodesFile*)nodes->data);

 /* Set the pointers in the Nodes structure. */

 nodes->offsets=(index_t*)(nodes->data+sizeof(NodesFile));
 nodes->nodes  =(Node*   )(nodes->data+sizeof(NodesFile)+(nodes->file.latbins*nodes->file.lonbins+1)*sizeof(index_t));

#else

 nodes->fd=ReOpenFile(filename);

 /* Copy the NodesFile header structure from the loaded data */

 ReadFile(nodes->fd,&nodes->file,sizeof(NodesFile));

 nodes->nodesoffset=sizeof(NodesFile)+(nodes->file.latbins*nodes->file.lonbins+1)*sizeof(index_t);

 for(i=0;i<sizeof(nodes->cached)/sizeof(nodes->cached[0]);i++)
    nodes->incache[i]=NO_NODE;

#endif

 return(nodes);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the closest node given its latitude, longitude and optionally the profile
  of the mode of transport that must be able to move to/from this node.

  index_t FindClosestNode Returns the closest node.

  Nodes *nodes The set of nodes to search.

  Segments *segments The set of segments to use.

  Ways *ways The set of ways to use.

  double latitude The latitude to look for.

  double longitude The longitude to look for.

  distance_t distance The maximum distance to look from the specified coordinates.

  Profile *profile The profile of the mode of transport (or NULL).

  distance_t *bestdist Returns the distance to the best node.
  ++++++++++++++++++++++++++++++++++++++*/

index_t FindClosestNode(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
                        distance_t distance,Profile *profile,distance_t *bestdist)
{
 ll_bin_t   latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
 ll_bin_t   lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
 int        delta=0,count;
 index_t    i,index1,index2;
 index_t    bestn=NO_NODE;
 distance_t bestd=INF_DISTANCE;

 /* Start with the bin containing the location, then spiral outwards. */

 do
   {
    int latb,lonb,llbin;

    count=0;

    for(latb=latbin-delta;latb<=latbin+delta;latb++)
      {
       if(latb<0 || latb>=nodes->file.latbins)
          continue;

       for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
         {
          if(lonb<0 || lonb>=nodes->file.lonbins)
             continue;

          if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
             continue;

          llbin=lonb*nodes->file.latbins+latb;

          /* Check if this grid square has any hope of being close enough */

          if(delta>0)
            {
             double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
             double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
             double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
             double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));

             if(latb==latbin)
               {
                distance_t dist1=Distance(latitude,lon1,latitude,longitude);
                distance_t dist2=Distance(latitude,lon2,latitude,longitude);

                if(dist1>distance && dist2>distance)
                   continue;
               }
             else if(lonb==lonbin)
               {
                distance_t dist1=Distance(lat1,longitude,latitude,longitude);
                distance_t dist2=Distance(lat2,longitude,latitude,longitude);

                if(dist1>distance && dist2>distance)
                   continue;
               }
             else
               {
                distance_t dist1=Distance(lat1,lon1,latitude,longitude);
                distance_t dist2=Distance(lat2,lon1,latitude,longitude);
                distance_t dist3=Distance(lat2,lon2,latitude,longitude);
                distance_t dist4=Distance(lat1,lon2,latitude,longitude);

                if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
                   continue;
               }
            }

          /* Check every node in this grid square. */

          index1=LookupNodeOffset(nodes,llbin);
          index2=LookupNodeOffset(nodes,llbin+1);

          for(i=index1;i<index2;i++)
            {
             Node *node=LookupNode(nodes,i,1);
             double lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
             double lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));

             distance_t dist=Distance(lat,lon,latitude,longitude);

             if(dist<distance)
               {
                if(profile)
                  {
                   Segment *segment;

                   /* Decide if this is node is valid for the profile */

                   segment=FirstSegment(segments,nodes,i);

                   do
                     {
                      Way *way=LookupWay(ways,segment->way,1);

                      if(way->allow&profile->allow)
                         break;

                      segment=NextSegment(segments,segment,i);
                     }
                   while(segment);

                   if(!segment)
                      continue;
                  }

                bestn=i; bestd=distance=dist;
               }
            }

          count++;
         }
      }

    delta++;
   }
 while(count);

 *bestdist=bestd;

 return(bestn);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the closest point on the closest segment given its latitude, longitude
  and optionally the profile of the mode of transport that must be able to move
  along this segment.

  index_t FindClosestSegment Returns the closest segment index.

  Nodes *nodes The set of nodes to use.

  Segments *segments The set of segments to search.

  Ways *ways The set of ways to use.

  double latitude The latitude to look for.

  double longitude The longitude to look for.

  distance_t distance The maximum distance to look from the specified coordinates.

  Profile *profile The profile of the mode of transport (or NULL).

  distance_t *bestdist Returns the distance to the closest point on the best segment.

  index_t *bestnode1 Returns the index of the node at one end of the closest segment.

  index_t *bestnode2 Returns the index of the node at the other end of the closest segment.

  distance_t *bestdist1 Returns the distance along the segment to the node at one end.

  distance_t *bestdist2 Returns the distance along the segment to the node at the other end.
  ++++++++++++++++++++++++++++++++++++++*/

index_t FindClosestSegment(Nodes *nodes,Segments *segments,Ways *ways,double latitude,double longitude,
                           distance_t distance,Profile *profile, distance_t *bestdist,
                           index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
{
 ll_bin_t   latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
 ll_bin_t   lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
 int        delta=0,count;
 index_t    i,index1,index2;
 index_t    bestn1=NO_NODE,bestn2=NO_NODE;
 distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
 index_t    bests=NO_SEGMENT;

 /* Start with the bin containing the location, then spiral outwards. */

 do
   {
    int latb,lonb,llbin;

    count=0;

    for(latb=latbin-delta;latb<=latbin+delta;latb++)
      {
       if(latb<0 || latb>=nodes->file.latbins)
          continue;

       for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
         {
          if(lonb<0 || lonb>=nodes->file.lonbins)
             continue;

          if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
             continue;

          llbin=lonb*nodes->file.latbins+latb;

          /* Check if this grid square has any hope of being close enough */

          if(delta>0)
            {
             double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
             double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
             double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
             double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));

             if(latb==latbin)
               {
                distance_t dist1=Distance(latitude,lon1,latitude,longitude);
                distance_t dist2=Distance(latitude,lon2,latitude,longitude);

                if(dist1>distance && dist2>distance)
                   continue;
               }
             else if(lonb==lonbin)
               {
                distance_t dist1=Distance(lat1,longitude,latitude,longitude);
                distance_t dist2=Distance(lat2,longitude,latitude,longitude);

                if(dist1>distance && dist2>distance)
                   continue;
               }
             else
               {
                distance_t dist1=Distance(lat1,lon1,latitude,longitude);
                distance_t dist2=Distance(lat2,lon1,latitude,longitude);
                distance_t dist3=Distance(lat2,lon2,latitude,longitude);
                distance_t dist4=Distance(lat1,lon2,latitude,longitude);

                if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
                   continue;
               }
            }

          /* Check every node in this grid square. */

          index1=LookupNodeOffset(nodes,llbin);
          index2=LookupNodeOffset(nodes,llbin+1);

          for(i=index1;i<index2;i++)
            {
             Node *node=LookupNode(nodes,i,1);
             double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
             double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
             distance_t dist1;

             dist1=Distance(lat1,lon1,latitude,longitude);

             if(dist1<distance)
               {
                Segment *segment;

                /* Check each segment for closeness and if valid for the profile */

                segment=FirstSegment(segments,nodes,i);

                do
                  {
                   if(IsNormalSegment(segment))
                     {
                      Way *way=NULL;

                      if(profile)
                         way=LookupWay(ways,segment->way,1);

                      if(!profile || way->allow&profile->allow)
                        {
                         distance_t dist2,dist3;
                         double lat2,lon2,dist3a,dist3b,distp;

                         GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);

                         dist2=Distance(lat2,lon2,latitude,longitude);

                         dist3=Distance(lat1,lon1,lat2,lon2);

                         /* Use law of cosines (assume flat Earth) */

                         dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
                         dist3b=(double)dist3-dist3a;

                         if((dist1+dist2)<dist3)
                           {
                            distp=0;
                           }
                         else if(dist3a>=0 && dist3b>=0)
                            distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
                         else if(dist3a>0)
                           {
                            distp=dist2;
                            dist3a=dist3;
                            dist3b=0;
                           }
                         else /* if(dist3b>0) */
                           {
                            distp=dist1;
                            dist3a=0;
                            dist3b=dist3;
                           }

                         if(distp<(double)bestd)
                           {
                            bests=IndexSegment(segments,segment);

                            if(segment->node1==i)
                              {
                               bestn1=i;
                               bestn2=OtherNode(segment,i);
                               bestd1=(distance_t)dist3a;
                               bestd2=(distance_t)dist3b;
                              }
                            else
                              {
                               bestn1=OtherNode(segment,i);
                               bestn2=i;
                               bestd1=(distance_t)dist3b;
                               bestd2=(distance_t)dist3a;
                              }

                            bestd=(distance_t)distp;
                           }
                        }
                     }

                   segment=NextSegment(segments,segment,i);
                  }
                while(segment);
               }

            } /* dist1 < distance */

          count++;
         }
      }

    delta++;
   }
 while(count);

 *bestdist=bestd;

 *bestnode1=bestn1;
 *bestnode2=bestn2;
 *bestdist1=bestd1;
 *bestdist2=bestd2;

 return(bests);
}


/*++++++++++++++++++++++++++++++++++++++
  Get the latitude and longitude associated with a node.

  Nodes *nodes The set of nodes to use.

  index_t index The node index.

  double *latitude Returns the latitude.

  double *longitude Returns the logitude.
  ++++++++++++++++++++++++++++++++++++++*/

void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
{
 Node *node=LookupNode(nodes,index,2);
 int latbin=-1,lonbin=-1;
 int start,end,mid;
 index_t offset;

 /* Binary search - search key exact match only is required.
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  Since an exact match is wanted we can set end=mid-1
  *  # <- mid    |  or start=mid+1 because we know that mid doesn't match.
  *  #           |
  *  #           |  Eventually either end=start or end=start+1 and one of
  *  # <- end    |  start or end is the wanted one.
  */

 /* Search for longitude */

 start=0;
 end=nodes->file.lonbins-1;

 do
   {
    mid=(start+end)/2;                  /* Choose mid point */

    offset=LookupNodeOffset(nodes,nodes->file.latbins*mid);

    if(offset<index)                    /* Mid point is too low */
       start=mid;
    else if(offset>index)               /* Mid point is too high */
       end=mid-1;
    else                                /* Mid point is correct */
      {lonbin=mid;break;}
   }
 while((end-start)>1);

 if(lonbin==-1)
   {
    offset=LookupNodeOffset(nodes,nodes->file.latbins*end);

    if(offset>index)
       lonbin=start;
    else
       lonbin=end;
   }

 while(lonbin<nodes->file.lonbins && 
       LookupNodeOffset(nodes,lonbin*nodes->file.latbins)==LookupNodeOffset(nodes,(lonbin+1)*nodes->file.latbins))
    lonbin++;

 /* Search for latitude */

 start=0;
 end=nodes->file.latbins-1;

 do
   {
    mid=(start+end)/2;                  /* Choose mid point */

    offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+mid);

    if(offset<index)                    /* Mid point is too low */
       start=mid;
    else if(offset>index)               /* Mid point is too high */
       end=mid-1;
    else                                /* Mid point is correct */
      {latbin=mid;break;}
   }
 while((end-start)>1);

 if(latbin==-1)
   {
    offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+end);

    if(offset>index)
       latbin=start;
    else
       latbin=end;
   }

 while(latbin<nodes->file.latbins &&
       LookupNodeOffset(nodes,lonbin*nodes->file.latbins+latbin)==LookupNodeOffset(nodes,lonbin*nodes->file.latbins+latbin+1))
    latbin++;

 /* Return the values */

 *latitude =latlong_to_radians(bin_to_latlong(nodes->file.latzero+latbin)+off_to_latlong(node->latoffset));
 *longitude=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonbin)+off_to_latlong(node->lonoffset));
}
1	amb	2	/***************************************
2			Node data type functions.
3	amb	151
4			Part of the Routino routing software.
5	amb	2	****************/ /****************
6	amb	607	This file Copyright 2008-2011 Andrew M. Bishop
7	amb	2
8	amb	151	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	amb	2	***************************************/
21
22
23	amb	214	#include <sys/types.h>
24	amb	2	#include <stdlib.h>
25	amb	118	#include <math.h>
26	amb	2
27	amb	109	#include "nodes.h"
28	amb	114	#include "segments.h"
29	amb	179	#include "ways.h"
30	amb	2
31	amb	449	#include "files.h"
32			#include "profiles.h"
33	amb	2
34	amb	449
35	amb	17	/*++++++++++++++++++++++++++++++++++++++
36	amb	2	Load in a node list from a file.
37
38	amb	681	Nodes *LoadNodeList Returns the node list.
39	amb	17
40	amb	2	const char *filename The name of the file to load.
41			++++++++++++++++++++++++++++++++++++++*/
42
43	amb	19	Nodes LoadNodeList(const char filename)
44	amb	2	{
45	amb	88	Nodes *nodes;
46	amb	706	#if SLIM
47			int i;
48			#endif
49	amb	88
50			nodes=(Nodes*)malloc(sizeof(Nodes));
51
52	amb	453	#if !SLIM
53	amb	88
54	amb	453	nodes->data=MapFile(filename);
55	amb	88
56	amb	453	/* Copy the NodesFile header structure from the loaded data */
57	amb	88
58	amb	453	nodes->file=((NodesFile)nodes->data);
59	amb	88
60	amb	453	/* Set the pointers in the Nodes structure. */
61	amb	88
62	amb	453	nodes->offsets=(index_t*)(nodes->data+sizeof(NodesFile));
63	amb	460	nodes->nodes =(Node* )(nodes->data+sizeof(NodesFile)+(nodes->file.latbinsnodes->file.lonbins+1)sizeof(index_t));
64	amb	453
65			#else
66
67			nodes->fd=ReOpenFile(filename);
68
69			/* Copy the NodesFile header structure from the loaded data */
70
71			ReadFile(nodes->fd,&nodes->file,sizeof(NodesFile));
72
73			nodes->nodesoffset=sizeof(NodesFile)+(nodes->file.latbinsnodes->file.lonbins+1)sizeof(index_t);
74
75	amb	706	for(i=0;i<sizeof(nodes->cached)/sizeof(nodes->cached[0]);i++)
76			nodes->incache[i]=NO_NODE;
77	amb	453
78			#endif
79
80	amb	88	return(nodes);
81	amb	2	}
82
83
84			/*++++++++++++++++++++++++++++++++++++++
85	amb	680	Find the closest node given its latitude, longitude and optionally the profile
86			of the mode of transport that must be able to move to/from this node.
87	amb	99
88	amb	303	index_t FindClosestNode Returns the closest node.
89	amb	99
90	amb	681	Nodes *nodes The set of nodes to search.
91	amb	99
92	amb	179	Segments *segments The set of segments to use.
93
94			Ways *ways The set of ways to use.
95
96	amb	219	double latitude The latitude to look for.
97	amb	99
98	amb	219	double longitude The longitude to look for.
99	amb	124
100	amb	680	distance_t distance The maximum distance to look from the specified coordinates.
101	amb	179
102	amb	303	Profile *profile The profile of the mode of transport (or NULL).
103
104			distance_t *bestdist Returns the distance to the best node.
105	amb	99	++++++++++++++++++++++++++++++++++++++*/
106
107	amb	681	index_t FindClosestNode(Nodes nodes,Segments segments,Ways *ways,double latitude,double longitude,
108	amb	303	distance_t distance,Profile profile,distance_t bestdist)
109	amb	99	{
110	amb	453	ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
111			ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
112	amb	303	int delta=0,count;
113	amb	462	index_t i,index1,index2;
114			index_t bestn=NO_NODE;
115	amb	303	distance_t bestd=INF_DISTANCE;
116	amb	99
117	amb	124	/* Start with the bin containing the location, then spiral outwards. */
118	amb	118
119	amb	124	do
120	amb	99	{
121	amb	124	int latb,lonb,llbin;
122	amb	99
123	amb	124	count=0;
124	amb	453
125	amb	124	for(latb=latbin-delta;latb<=latbin+delta;latb++)
126	amb	303	{
127	amb	453	if(latb<0 \|\| latb>=nodes->file.latbins)
128	amb	303	continue;
129
130	amb	124	for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
131			{
132	amb	453	if(lonb<0 \|\| lonb>=nodes->file.lonbins)
133	amb	303	continue;
134
135	amb	124	if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
136			continue;
137	amb	99
138	amb	453	llbin=lonb*nodes->file.latbins+latb;
139	amb	124
140	amb	303	/* Check if this grid square has any hope of being close enough */
141	amb	124
142			if(delta>0)
143			{
144	amb	453	double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
145			double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
146			double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
147			double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));
148	amb	124
149			if(latb==latbin)
150			{
151			distance_t dist1=Distance(latitude,lon1,latitude,longitude);
152			distance_t dist2=Distance(latitude,lon2,latitude,longitude);
153
154	amb	303	if(dist1>distance && dist2>distance)
155	amb	124	continue;
156			}
157			else if(lonb==lonbin)
158			{
159			distance_t dist1=Distance(lat1,longitude,latitude,longitude);
160			distance_t dist2=Distance(lat2,longitude,latitude,longitude);
161
162	amb	303	if(dist1>distance && dist2>distance)
163	amb	124	continue;
164			}
165			else
166			{
167			distance_t dist1=Distance(lat1,lon1,latitude,longitude);
168			distance_t dist2=Distance(lat2,lon1,latitude,longitude);
169			distance_t dist3=Distance(lat2,lon2,latitude,longitude);
170			distance_t dist4=Distance(lat1,lon2,latitude,longitude);
171
172	amb	303	if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
173	amb	124	continue;
174			}
175			}
176
177	amb	303	/* Check every node in this grid square. */
178
179	amb	462	index1=LookupNodeOffset(nodes,llbin);
180			index2=LookupNodeOffset(nodes,llbin+1);
181
182			for(i=index1;i<index2;i++)
183	amb	124	{
184	amb	453	Node *node=LookupNode(nodes,i,1);
185			double lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
186			double lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
187	amb	124
188			distance_t dist=Distance(lat,lon,latitude,longitude);
189
190	amb	303	if(dist<distance)
191	amb	179	{
192			if(profile)
193			{
194			Segment *segment;
195
196			/* Decide if this is node is valid for the profile */
197
198			segment=FirstSegment(segments,nodes,i);
199
200			do
201			{
202	amb	460	Way *way=LookupWay(ways,segment->way,1);
203	amb	179
204			if(way->allow&profile->allow)
205			break;
206
207			segment=NextSegment(segments,segment,i);
208			}
209			while(segment);
210
211			if(!segment)
212			continue;
213			}
214
215	amb	303	bestn=i; bestd=distance=dist;
216	amb	179	}
217	amb	124	}
218
219			count++;
220			}
221	amb	303	}
222	amb	124
223			delta++;
224	amb	99	}
225	amb	124	while(count);
226	amb	99
227	amb	303	*bestdist=bestd;
228
229			return(bestn);
230	amb	99	}
231
232
233			/*++++++++++++++++++++++++++++++++++++++
234	amb	680	Find the closest point on the closest segment given its latitude, longitude
235			and optionally the profile of the mode of transport that must be able to move
236			along this segment.
237	amb	303
238	amb	459	index_t FindClosestSegment Returns the closest segment index.
239	amb	303
240	amb	681	Nodes *nodes The set of nodes to use.
241	amb	303
242	amb	680	Segments *segments The set of segments to search.
243	amb	303
244			Ways *ways The set of ways to use.
245
246			double latitude The latitude to look for.
247
248			double longitude The longitude to look for.
249
250	amb	680	distance_t distance The maximum distance to look from the specified coordinates.
251	amb	303
252			Profile *profile The profile of the mode of transport (or NULL).
253
254	amb	680	distance_t *bestdist Returns the distance to the closest point on the best segment.
255	amb	303
256	amb	680	index_t *bestnode1 Returns the index of the node at one end of the closest segment.
257	amb	303
258	amb	680	index_t *bestnode2 Returns the index of the node at the other end of the closest segment.
259	amb	303
260	amb	680	distance_t *bestdist1 Returns the distance along the segment to the node at one end.
261	amb	303
262	amb	680	distance_t *bestdist2 Returns the distance along the segment to the node at the other end.
263	amb	303	++++++++++++++++++++++++++++++++++++++*/
264
265	amb	681	index_t FindClosestSegment(Nodes nodes,Segments segments,Ways *ways,double latitude,double longitude,
266	amb	459	distance_t distance,Profile profile, distance_t bestdist,
267			index_t bestnode1,index_t bestnode2,distance_t bestdist1,distance_t bestdist2)
268	amb	303	{
269	amb	453	ll_bin_t latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->file.latzero;
270			ll_bin_t lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->file.lonzero;
271	amb	303	int delta=0,count;
272	amb	462	index_t i,index1,index2;
273			index_t bestn1=NO_NODE,bestn2=NO_NODE;
274	amb	303	distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
275	amb	459	index_t bests=NO_SEGMENT;
276	amb	303
277			/* Start with the bin containing the location, then spiral outwards. */
278
279			do
280			{
281			int latb,lonb,llbin;
282
283			count=0;
284	amb	453
285	amb	303	for(latb=latbin-delta;latb<=latbin+delta;latb++)
286			{
287	amb	453	if(latb<0 \|\| latb>=nodes->file.latbins)
288	amb	303	continue;
289
290			for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
291			{
292	amb	453	if(lonb<0 \|\| lonb>=nodes->file.lonbins)
293	amb	303	continue;
294
295			if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
296			continue;
297
298	amb	453	llbin=lonb*nodes->file.latbins+latb;
299	amb	303
300			/* Check if this grid square has any hope of being close enough */
301
302			if(delta>0)
303			{
304	amb	453	double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
305			double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
306			double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
307			double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));
308	amb	303
309			if(latb==latbin)
310			{
311			distance_t dist1=Distance(latitude,lon1,latitude,longitude);
312			distance_t dist2=Distance(latitude,lon2,latitude,longitude);
313
314			if(dist1>distance && dist2>distance)
315			continue;
316			}
317			else if(lonb==lonbin)
318			{
319			distance_t dist1=Distance(lat1,longitude,latitude,longitude);
320			distance_t dist2=Distance(lat2,longitude,latitude,longitude);
321
322			if(dist1>distance && dist2>distance)
323			continue;
324			}
325			else
326			{
327			distance_t dist1=Distance(lat1,lon1,latitude,longitude);
328			distance_t dist2=Distance(lat2,lon1,latitude,longitude);
329			distance_t dist3=Distance(lat2,lon2,latitude,longitude);
330			distance_t dist4=Distance(lat1,lon2,latitude,longitude);
331
332			if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
333			continue;
334			}
335			}
336
337			/* Check every node in this grid square. */
338
339	amb	462	index1=LookupNodeOffset(nodes,llbin);
340			index2=LookupNodeOffset(nodes,llbin+1);
341
342			for(i=index1;i<index2;i++)
343	amb	303	{
344	amb	453	Node *node=LookupNode(nodes,i,1);
345			double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
346			double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
347	amb	408	distance_t dist1;
348	amb	303
349			dist1=Distance(lat1,lon1,latitude,longitude);
350
351	amb	321	if(dist1<distance)
352			{
353			Segment *segment;
354	amb	303
355	amb	321	/* Check each segment for closeness and if valid for the profile */
356	amb	303
357	amb	321	segment=FirstSegment(segments,nodes,i);
358
359			do
360	amb	303	{
361	amb	321	if(IsNormalSegment(segment))
362			{
363			Way *way=NULL;
364	amb	303
365	amb	321	if(profile)
366	amb	460	way=LookupWay(ways,segment->way,1);
367	amb	303
368	amb	321	if(!profile \|\| way->allow&profile->allow)
369			{
370	amb	408	distance_t dist2,dist3;
371			double lat2,lon2,dist3a,dist3b,distp;
372	amb	303
373	amb	321	GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
374	amb	303
375	amb	321	dist2=Distance(lat2,lon2,latitude,longitude);
376	amb	303
377	amb	408	dist3=Distance(lat1,lon1,lat2,lon2);
378	amb	303
379	amb	408	/* Use law of cosines (assume flat Earth) */
380	amb	303
381	amb	408	dist3a=((double)dist1(double)dist1-(double)dist2(double)dist2+(double)dist3(double)dist3)/(2.0(double)dist3);
382			dist3b=(double)dist3-dist3a;
383	amb	303
384	amb	607	if((dist1+dist2)<dist3)
385			{
386			distp=0;
387			}
388			else if(dist3a>=0 && dist3b>=0)
389	amb	443	distp=sqrt((double)dist1(double)dist1-dist3adist3a);
390			else if(dist3a>0)
391	amb	408	{
392	amb	443	distp=dist2;
393			dist3a=dist3;
394			dist3b=0;
395			}
396			else /* if(dist3b>0) */
397			{
398			distp=dist1;
399			dist3a=0;
400			dist3b=dist3;
401			}
402	amb	321
403	amb	443	if(distp<(double)bestd)
404			{
405	amb	459	bests=IndexSegment(segments,segment);
406	amb	534
407			if(segment->node1==i)
408			{
409			bestn1=i;
410			bestn2=OtherNode(segment,i);
411			bestd1=(distance_t)dist3a;
412			bestd2=(distance_t)dist3b;
413			}
414			else
415			{
416			bestn1=OtherNode(segment,i);
417			bestn2=i;
418			bestd1=(distance_t)dist3b;
419			bestd2=(distance_t)dist3a;
420			}
421
422	amb	443	bestd=(distance_t)distp;
423	amb	408	}
424	amb	303	}
425			}
426	amb	321
427			segment=NextSegment(segments,segment,i);
428	amb	303	}
429	amb	321	while(segment);
430	amb	303	}
431
432	amb	321	} /* dist1 < distance */
433
434	amb	303	count++;
435			}
436			}
437
438			delta++;
439			}
440			while(count);
441
442			*bestdist=bestd;
443
444			*bestnode1=bestn1;
445			*bestnode2=bestn2;
446			*bestdist1=bestd1;
447			*bestdist2=bestd2;
448
449			return(bests);
450			}
451
452
453			/*++++++++++++++++++++++++++++++++++++++
454	amb	99	Get the latitude and longitude associated with a node.
455
456	amb	680	Nodes *nodes The set of nodes to use.
457	amb	99
458	amb	174	index_t index The node index.
459	amb	99
460	amb	219	double *latitude Returns the latitude.
461	amb	99
462	amb	219	double *longitude Returns the logitude.
463	amb	99	++++++++++++++++++++++++++++++++++++++*/
464
465	amb	219	void GetLatLong(Nodes nodes,index_t index,double latitude,double *longitude)
466	amb	99	{
467	amb	453	Node *node=LookupNode(nodes,index,2);
468	amb	99	int latbin=-1,lonbin=-1;
469			int start,end,mid;
470	amb	462	index_t offset;
471	amb	99
472	amb	680	/* Binary search - search key exact match only is required.
473	amb	99	*
474			* # <- start \| Check mid and move start or end if it doesn't match
475			* # \|
476	amb	680	* # \| Since an exact match is wanted we can set end=mid-1
477			* # <- mid \| or start=mid+1 because we know that mid doesn't match.
478	amb	99	* # \|
479			* # \| Eventually either end=start or end=start+1 and one of
480			* # <- end \| start or end is the wanted one.
481			*/
482
483			/* Search for longitude */
484
485			start=0;
486	amb	453	end=nodes->file.lonbins-1;
487	amb	99
488			do
489			{
490	amb	462	mid=(start+end)/2; /* Choose mid point */
491	amb	99
492	amb	462	offset=LookupNodeOffset(nodes,nodes->file.latbins*mid);
493
494			if(offset<index) /* Mid point is too low */
495	amb	99	start=mid;
496	amb	462	else if(offset>index) /* Mid point is too high */
497	amb	99	end=mid-1;
498	amb	462	else /* Mid point is correct */
499	amb	99	{lonbin=mid;break;}
500			}
501			while((end-start)>1);
502
503			if(lonbin==-1)
504			{
505	amb	462	offset=LookupNodeOffset(nodes,nodes->file.latbins*end);
506
507			if(offset>index)
508	amb	99	lonbin=start;
509			else
510			lonbin=end;
511			}
512
513	amb	462	while(lonbin<nodes->file.lonbins &&
514			LookupNodeOffset(nodes,lonbinnodes->file.latbins)==LookupNodeOffset(nodes,(lonbin+1)nodes->file.latbins))
515	amb	99	lonbin++;
516
517			/* Search for latitude */
518
519			start=0;
520	amb	453	end=nodes->file.latbins-1;
521	amb	99
522			do
523			{
524	amb	462	mid=(start+end)/2; /* Choose mid point */
525	amb	99
526	amb	462	offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+mid);
527
528			if(offset<index) /* Mid point is too low */
529	amb	99	start=mid;
530	amb	462	else if(offset>index) /* Mid point is too high */
531	amb	99	end=mid-1;
532	amb	462	else /* Mid point is correct */
533	amb	99	{latbin=mid;break;}
534			}
535			while((end-start)>1);
536
537			if(latbin==-1)
538			{
539	amb	462	offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+end);
540
541			if(offset>index)
542	amb	99	latbin=start;
543			else
544			latbin=end;
545			}
546
547	amb	462	while(latbin<nodes->file.latbins &&
548			LookupNodeOffset(nodes,lonbinnodes->file.latbins+latbin)==LookupNodeOffset(nodes,lonbinnodes->file.latbins+latbin+1))
549	amb	99	latbin++;
550
551			/* Return the values */
552
553	amb	453	*latitude =latlong_to_radians(bin_to_latlong(nodes->file.latzero+latbin)+off_to_latlong(node->latoffset));
554			*longitude=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonbin)+off_to_latlong(node->lonoffset));
555	amb	99	}