MS-Windows/src/nodes.c

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

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2015 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 <stdlib.h>
#include <math.h>

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

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


/* Local functions */

static int valid_segment_for_profile(Ways *ways,Segment *segmentp,Profile *profile);


/*++++++++++++++++++++++++++++++++++++++
  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
 size_t sizeoffsets;
#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=SlimMapFile(filename);

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

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

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

 nodes->offsets=(index_t*)malloc(sizeoffsets);
 log_malloc(nodes->offsets,sizeoffsets);

 SlimFetch(nodes->fd,nodes->offsets,sizeoffsets,sizeof(NodesFile));

 nodes->nodesoffset=(off_t)(sizeof(NodesFile)+sizeoffsets);

 nodes->cache=NewNodeCache();
 log_malloc(nodes->cache,sizeof(*nodes->cache));

#endif

 return(nodes);
}


/*++++++++++++++++++++++++++++++++++++++
  Destroy the node list.

  Nodes *nodes The node list to destroy.
  ++++++++++++++++++++++++++++++++++++++*/

void DestroyNodeList(Nodes *nodes)
{
#if !SLIM

 nodes->data=UnmapFile(nodes->data);

#else

 nodes->fd=SlimUnmapFile(nodes->fd);

 log_free(nodes->offsets);
 free(nodes->offsets);

 log_free(nodes->cache);
 DeleteNodeCache(nodes->cache);

#endif

 free(nodes);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the closest node given its latitude, longitude and 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.

  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;

 /* Find the maximum distance to search */

 double dlat=DeltaLat(longitude,distance);
 double dlon=DeltaLon(latitude ,distance);

 double minlat=latitude -dlat;
 double maxlat=latitude +dlat;
 double minlon=longitude-dlon;
 double maxlon=longitude+dlon;

 ll_bin_t minlatbin=latlong_to_bin(radians_to_latlong(minlat))-nodes->file.latzero;
 ll_bin_t maxlatbin=latlong_to_bin(radians_to_latlong(maxlat))-nodes->file.latzero;
 ll_bin_t minlonbin=latlong_to_bin(radians_to_latlong(minlon))-nodes->file.lonzero;
 ll_bin_t maxlonbin=latlong_to_bin(radians_to_latlong(maxlon))-nodes->file.lonzero;

 ll_off_t minlatoff=latlong_to_off(radians_to_latlong(minlat));
 ll_off_t maxlatoff=latlong_to_off(radians_to_latlong(maxlat));
 ll_off_t minlonoff=latlong_to_off(radians_to_latlong(minlon));
 ll_off_t maxlonoff=latlong_to_off(radians_to_latlong(maxlon));

 /* 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 || latb<minlatbin || latb>maxlatbin)
          continue;

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

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

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

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

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

          for(i=index1;i<index2;i++)
            {
             Node *nodep=LookupNode(nodes,i,3);
             double lat,lon;
             distance_t dist;

             if(latb==minlatbin && nodep->latoffset<minlatoff)
                continue;

             if(latb==maxlatbin && nodep->latoffset>maxlatoff)
                continue;

             if(lonb==minlonbin && nodep->lonoffset<minlonoff)
                continue;

             if(lonb==maxlonbin && nodep->lonoffset>maxlonoff)
                continue;

             lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(nodep->latoffset));
             lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(nodep->lonoffset));

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

             if(dist<bestd)
               {
                Segment *segmentp;

                /* Check that at least one segment is valid for the profile */

                segmentp=FirstSegment(segments,nodep,1);

                do
                  {
                   if(IsNormalSegment(segmentp) && valid_segment_for_profile(ways,segmentp,profile))
                     {
                      bestn=i;
                      bestd=dist;

                      break;
                     }

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

          count++;
         }
      }

    delta++;
   }
 while(count);

 *bestdist=bestd;

 return(bestn);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the closest point on the closest segment given its latitude, longitude
  and 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.

  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;

 /* Find the maximum distance to search */

 double dlat=DeltaLat(longitude,distance);
 double dlon=DeltaLon(latitude ,distance);

 double minlat=latitude -dlat;
 double maxlat=latitude +dlat;
 double minlon=longitude-dlon;
 double maxlon=longitude+dlon;

 ll_bin_t minlatbin=latlong_to_bin(radians_to_latlong(minlat))-nodes->file.latzero;
 ll_bin_t maxlatbin=latlong_to_bin(radians_to_latlong(maxlat))-nodes->file.latzero;
 ll_bin_t minlonbin=latlong_to_bin(radians_to_latlong(minlon))-nodes->file.lonzero;
 ll_bin_t maxlonbin=latlong_to_bin(radians_to_latlong(maxlon))-nodes->file.lonzero;

 ll_off_t minlatoff=latlong_to_off(radians_to_latlong(minlat));
 ll_off_t maxlatoff=latlong_to_off(radians_to_latlong(maxlat));
 ll_off_t minlonoff=latlong_to_off(radians_to_latlong(minlon));
 ll_off_t maxlonoff=latlong_to_off(radians_to_latlong(maxlon));

 /* 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 || latb<minlatbin || latb>maxlatbin)
          continue;

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

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

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

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

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

          for(i=index1;i<index2;i++)
            {
             Node *nodep=LookupNode(nodes,i,3);
             double lat1,lon1;
             distance_t dist1;

             if(latb==minlatbin && nodep->latoffset<minlatoff)
                continue;

             if(latb==maxlatbin && nodep->latoffset>maxlatoff)
                continue;

             if(lonb==minlonbin && nodep->lonoffset<minlonoff)
                continue;

             if(lonb==maxlonbin && nodep->lonoffset>maxlonoff)
                continue;

             lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(nodep->latoffset));
             lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(nodep->lonoffset));

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

             if(dist1<distance)
               {
                Segment *segmentp;

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

                segmentp=FirstSegment(segments,nodep,1);

                do
                  {
                   if(IsNormalSegment(segmentp) && valid_segment_for_profile(ways,segmentp,profile))
                     {
                      distance_t dist2,dist3;
                      double lat2,lon2,dist3a,dist3b,distp;

                      GetLatLong(nodes,OtherNode(segmentp,i),NULL,&lat2,&lon2);

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

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

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

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

                         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,segmentp);

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

                         bestd=(distance_t)distp;
                        }
                     }

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

               } /* dist1 < distance */
            }

          count++;
         }
      }

    delta++;
   }
 while(count);

 *bestdist=bestd;

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

 return(bests);
}


/*++++++++++++++++++++++++++++++++++++++
  Check if the transport defined by the profile is allowed on the segment.

  int valid_segment_for_profile Return 1 if it is or 0 if not.

  Ways *ways The set of ways to use.

  Segment *segmentp The segment to check.

  Profile *profile The profile to check.
  ++++++++++++++++++++++++++++++++++++++*/

static int valid_segment_for_profile(Ways *ways,Segment *segmentp,Profile *profile)
{
 Way *wayp=LookupWay(ways,segmentp->way,1);
 score_t segment_pref;
 int i;

 /* mode of transport must be allowed on the highway */
 if(!(wayp->allow&profile->allow))
    return(0);

 /* must obey weight restriction (if exists) */
 if(wayp->weight && wayp->weight<profile->weight)
    return(0);

 /* must obey height/width/length restriction (if exists) */
 if((wayp->height && wayp->height<profile->height) ||
    (wayp->width  && wayp->width <profile->width ) ||
    (wayp->length && wayp->length<profile->length))
    return(0);

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

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

 /* profile preferences must allow this highway */
 if(segment_pref==0)
    return(0);

 /* Must be OK */
 return(1);
}


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

  Nodes *nodes The set of nodes to use.

  index_t index The node index.

  Node *nodep A pointer to the node if already available.

  double *latitude Returns the latitude.

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

void GetLatLong(Nodes *nodes,index_t index,Node *nodep,double *latitude,double *longitude)
{
 ll_bin_t latbin,lonbin;
 ll_bin2_t bin=-1;
 ll_bin2_t start,end,mid;
 index_t offset;

 /* Binary search - search key nearest match below is required.
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  A lower bound match is wanted we can set end=mid-1 or
  *  # <- mid    |  start=mid 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 offset */

 start=0;
 end=nodes->file.lonbins*nodes->file.latbins;

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

    offset=LookupNodeOffset(nodes,mid);

    if(offset<index)                    /* Mid point is too low for an exact match but could be lower bound */
       start=mid;
    else if(offset>index)               /* Mid point is too high */
       end=mid?(mid-1):mid;
    else                                /* Mid point is correct */
      {bin=mid;break;}
   }
 while((end-start)>1);

 if(bin==-1)
   {
    offset=LookupNodeOffset(nodes,end);

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

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

 latbin=bin%nodes->file.latbins;
 lonbin=bin/nodes->file.latbins;

 /* Return the values */

 if(nodep==NULL)
    nodep=LookupNode(nodes,index,4);

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