MS-Windows/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
   {
    ll_bin_t latb,lonb;
    ll_bin2_t 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,1);

                   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
   {
    ll_bin_t latb,lonb;
    ll_bin2_t 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,1);

                do
                  {
                   if(IsNormalSegment(segment))
                     {
                      distance_t dist2,dist3;
                      double lat2,lon2,dist3a,dist3b,distp;

                      if(profile)
                        {
                         Way *way=LookupWay(ways,segment->way,1);
                         score_t segment_pref;
                         int pi;

                         /* mode of transport must be allowed on the highway */
                         if(!(way->allow&profile->allow))
                            goto endloop;

                         /* must obey weight restriction (if exists) */
                         if(way->weight && way->weight<profile->weight)
                            goto endloop;

                         /* must obey height/width/length restriction (if exists) */
                         if((way->height && way->height<profile->height) ||
                            (way->width  && way->width <profile->width ) ||
                            (way->length && way->length<profile->length))
                            goto endloop;

                         segment_pref=profile->highway[HIGHWAY(way->type)];

                         for(pi=1;pi<Property_Count;pi++)
                            if(ways->file.props & PROPERTIES(pi))
                              {
                               if(way->props & PROPERTIES(pi))
                                  segment_pref*=profile->props_yes[pi];
                               else
                                  segment_pref*=profile->props_no[pi];
                              }

                         /* profile preferences must allow this highway */
                         if(segment_pref==0)
                            goto endloop;
                        }

                      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;
                        }
                     }
                  endloop:

                   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);
 ll_bin_t latbin=-1,lonbin=-1;
 ll_bin_t 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	780	ll_bin_t latb,lonb;
122			ll_bin2_t llbin;
123	amb	99
124	amb	124	count=0;
125	amb	453
126	amb	124	for(latb=latbin-delta;latb<=latbin+delta;latb++)
127	amb	303	{
128	amb	453	if(latb<0 \|\| latb>=nodes->file.latbins)
129	amb	303	continue;
130
131	amb	124	for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
132			{
133	amb	453	if(lonb<0 \|\| lonb>=nodes->file.lonbins)
134	amb	303	continue;
135
136	amb	124	if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
137			continue;
138	amb	99
139	amb	453	llbin=lonb*nodes->file.latbins+latb;
140	amb	124
141	amb	303	/* Check if this grid square has any hope of being close enough */
142	amb	124
143			if(delta>0)
144			{
145	amb	453	double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb));
146			double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb));
147			double lat2=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb+1));
148			double lon2=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb+1));
149	amb	124
150			if(latb==latbin)
151			{
152			distance_t dist1=Distance(latitude,lon1,latitude,longitude);
153			distance_t dist2=Distance(latitude,lon2,latitude,longitude);
154
155	amb	303	if(dist1>distance && dist2>distance)
156	amb	124	continue;
157			}
158			else if(lonb==lonbin)
159			{
160			distance_t dist1=Distance(lat1,longitude,latitude,longitude);
161			distance_t dist2=Distance(lat2,longitude,latitude,longitude);
162
163	amb	303	if(dist1>distance && dist2>distance)
164	amb	124	continue;
165			}
166			else
167			{
168			distance_t dist1=Distance(lat1,lon1,latitude,longitude);
169			distance_t dist2=Distance(lat2,lon1,latitude,longitude);
170			distance_t dist3=Distance(lat2,lon2,latitude,longitude);
171			distance_t dist4=Distance(lat1,lon2,latitude,longitude);
172
173	amb	303	if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
174	amb	124	continue;
175			}
176			}
177
178	amb	303	/* Check every node in this grid square. */
179
180	amb	462	index1=LookupNodeOffset(nodes,llbin);
181			index2=LookupNodeOffset(nodes,llbin+1);
182
183			for(i=index1;i<index2;i++)
184	amb	124	{
185	amb	453	Node *node=LookupNode(nodes,i,1);
186			double lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(node->latoffset));
187			double lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(node->lonoffset));
188	amb	124
189			distance_t dist=Distance(lat,lon,latitude,longitude);
190
191	amb	303	if(dist<distance)
192	amb	179	{
193			if(profile)
194			{
195			Segment *segment;
196
197			/* Decide if this is node is valid for the profile */
198
199	amb	707	segment=FirstSegment(segments,nodes,i,1);
200	amb	179
201			do
202			{
203	amb	460	Way *way=LookupWay(ways,segment->way,1);
204	amb	179
205			if(way->allow&profile->allow)
206			break;
207
208			segment=NextSegment(segments,segment,i);
209			}
210			while(segment);
211
212			if(!segment)
213			continue;
214			}
215
216	amb	303	bestn=i; bestd=distance=dist;
217	amb	179	}
218	amb	124	}
219
220			count++;
221			}
222	amb	303	}
223	amb	124
224			delta++;
225	amb	99	}
226	amb	124	while(count);
227	amb	99
228	amb	303	*bestdist=bestd;
229
230			return(bestn);
231	amb	99	}
232
233
234			/*++++++++++++++++++++++++++++++++++++++
235	amb	680	Find the closest point on the closest segment given its latitude, longitude
236			and optionally the profile of the mode of transport that must be able to move
237			along this segment.
238	amb	303
239	amb	459	index_t FindClosestSegment Returns the closest segment index.
240	amb	303
241	amb	681	Nodes *nodes The set of nodes to use.
242	amb	303
243	amb	680	Segments *segments The set of segments to search.
244	amb	303
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	amb	680	distance_t distance The maximum distance to look from the specified coordinates.
252	amb	303
253			Profile *profile The profile of the mode of transport (or NULL).
254
255	amb	680	distance_t *bestdist Returns the distance to the closest point on the best segment.
256	amb	303
257	amb	680	index_t *bestnode1 Returns the index of the node at one end of the closest segment.
258	amb	303
259	amb	680	index_t *bestnode2 Returns the index of the node at the other end of the closest segment.
260	amb	303
261	amb	680	distance_t *bestdist1 Returns the distance along the segment to the node at one end.
262	amb	303
263	amb	680	distance_t *bestdist2 Returns the distance along the segment to the node at the other end.
264	amb	303	++++++++++++++++++++++++++++++++++++++*/
265
266	amb	681	index_t FindClosestSegment(Nodes nodes,Segments segments,Ways *ways,double latitude,double longitude,
267	amb	459	distance_t distance,Profile profile, distance_t bestdist,
268			index_t bestnode1,index_t bestnode2,distance_t bestdist1,distance_t bestdist2)
269	amb	303	{
270	amb	453	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	amb	303	int delta=0,count;
273	amb	462	index_t i,index1,index2;
274			index_t bestn1=NO_NODE,bestn2=NO_NODE;
275	amb	303	distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
276	amb	459	index_t bests=NO_SEGMENT;
277	amb	303
278			/* Start with the bin containing the location, then spiral outwards. */
279
280			do
281			{
282	amb	780	ll_bin_t latb,lonb;
283			ll_bin2_t llbin;
284	amb	303
285			count=0;
286	amb	453
287	amb	303	for(latb=latbin-delta;latb<=latbin+delta;latb++)
288			{
289	amb	453	if(latb<0 \|\| latb>=nodes->file.latbins)
290	amb	303	continue;
291
292			for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
293			{
294	amb	453	if(lonb<0 \|\| lonb>=nodes->file.lonbins)
295	amb	303	continue;
296
297			if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
298			continue;
299
300	amb	453	llbin=lonb*nodes->file.latbins+latb;
301	amb	303
302			/* Check if this grid square has any hope of being close enough */
303
304			if(delta>0)
305			{
306	amb	453	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	amb	303
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	amb	462	index1=LookupNodeOffset(nodes,llbin);
342			index2=LookupNodeOffset(nodes,llbin+1);
343
344			for(i=index1;i<index2;i++)
345	amb	303	{
346	amb	453	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	amb	408	distance_t dist1;
350	amb	303
351			dist1=Distance(lat1,lon1,latitude,longitude);
352
353	amb	321	if(dist1<distance)
354			{
355			Segment *segment;
356	amb	303
357	amb	321	/* Check each segment for closeness and if valid for the profile */
358	amb	303
359	amb	707	segment=FirstSegment(segments,nodes,i,1);
360	amb	321
361			do
362	amb	303	{
363	amb	321	if(IsNormalSegment(segment))
364			{
365	amb	828	distance_t dist2,dist3;
366			double lat2,lon2,dist3a,dist3b,distp;
367	amb	303
368	amb	321	if(profile)
369			{
370	amb	828	Way *way=LookupWay(ways,segment->way,1);
371			score_t segment_pref;
372			int pi;
373	amb	303
374	amb	828	/* mode of transport must be allowed on the highway */
375			if(!(way->allow&profile->allow))
376			goto endloop;
377	amb	303
378	amb	828	/* must obey weight restriction (if exists) */
379			if(way->weight && way->weight<profile->weight)
380			goto endloop;
381	amb	303
382	amb	828	/* must obey height/width/length restriction (if exists) */
383			if((way->height && way->height<profile->height) \|\|
384			(way->width && way->width <profile->width ) \|\|
385			(way->length && way->length<profile->length))
386			goto endloop;
387	amb	303
388	amb	828	segment_pref=profile->highway[HIGHWAY(way->type)];
389	amb	303
390	amb	828	for(pi=1;pi<Property_Count;pi++)
391			if(ways->file.props & PROPERTIES(pi))
392			{
393			if(way->props & PROPERTIES(pi))
394			segment_pref*=profile->props_yes[pi];
395			else
396			segment_pref*=profile->props_no[pi];
397			}
398	amb	303
399	amb	828	/* profile preferences must allow this highway */
400			if(segment_pref==0)
401			goto endloop;
402			}
403
404			GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
405
406			dist2=Distance(lat2,lon2,latitude,longitude);
407
408			dist3=Distance(lat1,lon1,lat2,lon2);
409
410			/* Use law of cosines (assume flat Earth) */
411
412			dist3a=((double)dist1(double)dist1-(double)dist2(double)dist2+(double)dist3(double)dist3)/(2.0(double)dist3);
413			dist3b=(double)dist3-dist3a;
414
415			if((dist1+dist2)<dist3)
416			{
417			distp=0;
418			}
419			else if(dist3a>=0 && dist3b>=0)
420			distp=sqrt((double)dist1(double)dist1-dist3adist3a);
421			else if(dist3a>0)
422			{
423			distp=dist2;
424			dist3a=dist3;
425			dist3b=0;
426			}
427			else /* if(dist3b>0) */
428			{
429			distp=dist1;
430			dist3a=0;
431			dist3b=dist3;
432			}
433
434			if(distp<(double)bestd)
435			{
436			bests=IndexSegment(segments,segment);
437
438			if(segment->node1==i)
439	amb	607	{
440	amb	828	bestn1=i;
441			bestn2=OtherNode(segment,i);
442			bestd1=(distance_t)dist3a;
443			bestd2=(distance_t)dist3b;
444	amb	607	}
445	amb	828	else
446	amb	408	{
447	amb	828	bestn1=OtherNode(segment,i);
448			bestn2=i;
449			bestd1=(distance_t)dist3b;
450			bestd2=(distance_t)dist3a;
451	amb	443	}
452	amb	321
453	amb	828	bestd=(distance_t)distp;
454	amb	303	}
455			}
456	amb	828	endloop:
457	amb	321
458			segment=NextSegment(segments,segment,i);
459	amb	303	}
460	amb	321	while(segment);
461	amb	303	}
462
463	amb	321	} /* dist1 < distance */
464
465	amb	303	count++;
466			}
467			}
468
469			delta++;
470			}
471			while(count);
472
473			*bestdist=bestd;
474
475			*bestnode1=bestn1;
476			*bestnode2=bestn2;
477			*bestdist1=bestd1;
478			*bestdist2=bestd2;
479
480			return(bests);
481			}
482
483
484			/*++++++++++++++++++++++++++++++++++++++
485	amb	99	Get the latitude and longitude associated with a node.
486
487	amb	680	Nodes *nodes The set of nodes to use.
488	amb	99
489	amb	174	index_t index The node index.
490	amb	99
491	amb	219	double *latitude Returns the latitude.
492	amb	99
493	amb	219	double *longitude Returns the logitude.
494	amb	99	++++++++++++++++++++++++++++++++++++++*/
495
496	amb	219	void GetLatLong(Nodes nodes,index_t index,double latitude,double *longitude)
497	amb	99	{
498	amb	453	Node *node=LookupNode(nodes,index,2);
499	amb	780	ll_bin_t latbin=-1,lonbin=-1;
500			ll_bin_t start,end,mid;
501	amb	462	index_t offset;
502	amb	99
503	amb	680	/* Binary search - search key exact match only is required.
504	amb	99	*
505			* # <- start \| Check mid and move start or end if it doesn't match
506			* # \|
507	amb	680	* # \| Since an exact match is wanted we can set end=mid-1
508			* # <- mid \| or start=mid+1 because we know that mid doesn't match.
509	amb	99	* # \|
510			* # \| Eventually either end=start or end=start+1 and one of
511			* # <- end \| start or end is the wanted one.
512			*/
513
514			/* Search for longitude */
515
516			start=0;
517	amb	453	end=nodes->file.lonbins-1;
518	amb	99
519			do
520			{
521	amb	462	mid=(start+end)/2; /* Choose mid point */
522	amb	99
523	amb	462	offset=LookupNodeOffset(nodes,nodes->file.latbins*mid);
524
525			if(offset<index) /* Mid point is too low */
526	amb	99	start=mid;
527	amb	462	else if(offset>index) /* Mid point is too high */
528	amb	99	end=mid-1;
529	amb	462	else /* Mid point is correct */
530	amb	99	{lonbin=mid;break;}
531			}
532			while((end-start)>1);
533
534			if(lonbin==-1)
535			{
536	amb	462	offset=LookupNodeOffset(nodes,nodes->file.latbins*end);
537
538			if(offset>index)
539	amb	99	lonbin=start;
540			else
541			lonbin=end;
542			}
543
544	amb	462	while(lonbin<nodes->file.lonbins &&
545			LookupNodeOffset(nodes,lonbinnodes->file.latbins)==LookupNodeOffset(nodes,(lonbin+1)nodes->file.latbins))
546	amb	99	lonbin++;
547
548			/* Search for latitude */
549
550			start=0;
551	amb	453	end=nodes->file.latbins-1;
552	amb	99
553			do
554			{
555	amb	462	mid=(start+end)/2; /* Choose mid point */
556	amb	99
557	amb	462	offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+mid);
558
559			if(offset<index) /* Mid point is too low */
560	amb	99	start=mid;
561	amb	462	else if(offset>index) /* Mid point is too high */
562	amb	99	end=mid-1;
563	amb	462	else /* Mid point is correct */
564	amb	99	{latbin=mid;break;}
565			}
566			while((end-start)>1);
567
568			if(latbin==-1)
569			{
570	amb	462	offset=LookupNodeOffset(nodes,lonbin*nodes->file.latbins+end);
571
572			if(offset>index)
573	amb	99	latbin=start;
574			else
575			latbin=end;
576			}
577
578	amb	462	while(latbin<nodes->file.latbins &&
579			LookupNodeOffset(nodes,lonbinnodes->file.latbins+latbin)==LookupNodeOffset(nodes,lonbinnodes->file.latbins+latbin+1))
580	amb	99	latbin++;
581
582			/* Return the values */
583
584	amb	453	*latitude =latlong_to_radians(bin_to_latlong(nodes->file.latzero+latbin)+off_to_latlong(node->latoffset));
585			*longitude=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonbin)+off_to_latlong(node->lonoffset));
586	amb	99	}