MS-Windows/src/nodes.c

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

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2013 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=ReOpenFile(filename);

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

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

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

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

 ReadFile(nodes->fd,nodes->offsets,sizeoffsets);

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

 nodes->cache=NewNodeCache();

#endif

 return(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;

 /* 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 *nodep=LookupNode(nodes,i,3);
             double lat=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(nodep->latoffset));
             double lon=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(nodep->lonoffset));

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

             if(dist<distance)
               {
                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=distance=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;

 /* 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 *nodep=LookupNode(nodes,i,3);
             double lat1=latlong_to_radians(bin_to_latlong(nodes->file.latzero+latb)+off_to_latlong(nodep->latoffset));
             double lon1=latlong_to_radians(bin_to_latlong(nodes->file.lonzero+lonb)+off_to_latlong(nodep->lonoffset));
             distance_t dist1;

             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) */

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