trunk/src/nodes.c

/***************************************
 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.25 2009-02-15 16:19:28 amb Exp $

 Node data type functions.
 ******************/ /******************
 Written by Andrew M. Bishop

 This file Copyright 2008,2009 Andrew M. Bishop
 It may be distributed under the GNU Public License, version 2, or
 any higher version.  See section COPYING of the GNU Public license
 for conditions under which this file may be redistributed.
 ***************************************/


#include <stdlib.h>
#include <math.h>

#include "nodes.h"
#include "segments.h"
#include "functions.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)
{
 void *data;
 Nodes *nodes;

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

 data=MapFile(filename);

 if(!data)
    return(NULL);

 /* Copy the Nodes structure from the loaded data */

 *nodes=*((Nodes*)data);

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

 nodes->data=data;
 nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
 nodes->nodes=(Node*)(data+(off_t)nodes->nodes);

 return(nodes);
}


/*++++++++++++++++++++++++++++++++++++++
  Find a node given its latitude and longitude.

  Node *FindNode Returns the node.

  Nodes* nodes The set of nodes to search.

  float latitude The latitude to look for.

  float longitude The longitude to look for.

  distance_t distance The maximum distance to look, returns the final distance.
  ++++++++++++++++++++++++++++++++++++++*/

Node *FindNode(Nodes* nodes,float latitude,float longitude,distance_t *distance)
{
 int32_t latbin=lat_long_to_bin(latitude )-nodes->latzero;
 int32_t lonbin=lat_long_to_bin(longitude)-nodes->lonzero;
 int     delta=0,count;
 index_t i,best=~0;

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

 do
   {
    int latb,lonb,llbin;

    count=0;
   
    for(latb=latbin-delta;latb<=latbin+delta;latb++)
       for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
         {
          if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
             continue;

          llbin=lonb*nodes->latbins+latb;

          if(llbin<0 || llbin>nodes->number)
             continue;

          if(delta>0)
            {
             float lat1=(float)((nodes->latzero+latb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
             float lon1=(float)((nodes->lonzero+lonb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
             float lat2=(float)((nodes->latzero+latb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;
             float lon2=(float)((nodes->lonzero+lonb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;

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

          for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
            {
             float lat=(float)((nodes->latzero+latbin)*LAT_LONG_BIN+nodes->nodes[i].latoffset)/LAT_LONG_SCALE;
             float lon=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+nodes->nodes[i].lonoffset)/LAT_LONG_SCALE;

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

             if(dist<*distance)
               {best=i; *distance=dist;}
            }

          count++;
         }

    delta++;
   }
 while(count);

 if(best==~0)
    return(NULL);
 else
    return(&nodes->nodes[best]);
}


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

  Nodes *nodes The set of nodes.

  Node *node The node.

  float *latitude Returns the latitude.

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

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

 /* Binary search - search key closest below is required.
  *
  *  # <- start  |  Check mid and move start or end if it doesn't match
  *  #           |
  *  #           |  Since an inexact match is wanted we must set end=mid-1
  *  # <- mid    |  or 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 longitude */

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

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

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

 if(lonbin==-1)
   {
    if(nodes->offsets[nodes->latbins*end]>index)
       lonbin=start;
    else
       lonbin=end;
   }

 while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
    lonbin++;

 /* Search for latitude */

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

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

    if(nodes->offsets[lonbin*nodes->latbins+mid]<index)      /* Mid point is too low */
       start=mid;
    else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
       end=mid-1;
    else                                                     /* Mid point is correct */
      {latbin=mid;break;}
   }
 while((end-start)>1);

 if(latbin==-1)
   {
    if(nodes->offsets[lonbin*nodes->latbins+end]>index)
       latbin=start;
    else
       latbin=end;
   }

 while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
    latbin++;

 /* Return the values */

 *latitude =(float)((nodes->latzero+latbin)*LAT_LONG_BIN+node->latoffset)/LAT_LONG_SCALE;
 *longitude=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+node->lonoffset)/LAT_LONG_SCALE;
}
1	/***************************************
2	$Header: /home/amb/CVS/routino/src/nodes.c,v 1.25 2009-02-15 16:19:28 amb Exp $
3
4	Node data type functions.
5	****************/ /****************
6	Written by Andrew M. Bishop
7
8	This file Copyright 2008,2009 Andrew M. Bishop
9	It may be distributed under the GNU Public License, version 2, or
10	any higher version. See section COPYING of the GNU Public license
11	for conditions under which this file may be redistributed.
12	***************************************/
13
14
15	#include <stdlib.h>
16	#include <math.h>
17
18	#include "nodes.h"
19	#include "segments.h"
20	#include "functions.h"
21
22
23	/*++++++++++++++++++++++++++++++++++++++
24	Load in a node list from a file.
25
26	Nodes* LoadNodeList Returns the node list.
27
28	const char *filename The name of the file to load.
29	++++++++++++++++++++++++++++++++++++++*/
30
31	Nodes LoadNodeList(const char filename)
32	{
33	void *data;
34	Nodes *nodes;
35
36	nodes=(Nodes*)malloc(sizeof(Nodes));
37
38	data=MapFile(filename);
39
40	if(!data)
41	return(NULL);
42
43	/* Copy the Nodes structure from the loaded data */
44
45	nodes=((Nodes*)data);
46
47	/* Adjust the pointers in the Nodes structure. */
48
49	nodes->data=data;
50	nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
51	nodes->nodes=(Node*)(data+(off_t)nodes->nodes);
52
53	return(nodes);
54	}
55
56
57	/*++++++++++++++++++++++++++++++++++++++
58	Find a node given its latitude and longitude.
59
60	Node *FindNode Returns the node.
61
62	Nodes* nodes The set of nodes to search.
63
64	float latitude The latitude to look for.
65
66	float longitude The longitude to look for.
67
68	distance_t distance The maximum distance to look, returns the final distance.
69	++++++++++++++++++++++++++++++++++++++*/
70
71	Node FindNode(Nodes nodes,float latitude,float longitude,distance_t *distance)
72	{
73	int32_t latbin=lat_long_to_bin(latitude )-nodes->latzero;
74	int32_t lonbin=lat_long_to_bin(longitude)-nodes->lonzero;
75	int delta=0,count;
76	index_t i,best=~0;
77
78	/* Start with the bin containing the location, then spiral outwards. */
79
80	do
81	{
82	int latb,lonb,llbin;
83
84	count=0;
85
86	for(latb=latbin-delta;latb<=latbin+delta;latb++)
87	for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
88	{
89	if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
90	continue;
91
92	llbin=lonb*nodes->latbins+latb;
93
94	if(llbin<0 \|\| llbin>nodes->number)
95	continue;
96
97	if(delta>0)
98	{
99	float lat1=(float)((nodes->latzero+latb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
100	float lon1=(float)((nodes->lonzero+lonb)*LAT_LONG_BIN)/LAT_LONG_SCALE;
101	float lat2=(float)((nodes->latzero+latb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;
102	float lon2=(float)((nodes->lonzero+lonb+1)*LAT_LONG_BIN)/LAT_LONG_SCALE;
103
104	if(latb==latbin)
105	{
106	distance_t dist1=Distance(latitude,lon1,latitude,longitude);
107	distance_t dist2=Distance(latitude,lon2,latitude,longitude);
108
109	if(dist1>distance && dist2>distance)
110	continue;
111	}
112	else if(lonb==lonbin)
113	{
114	distance_t dist1=Distance(lat1,longitude,latitude,longitude);
115	distance_t dist2=Distance(lat2,longitude,latitude,longitude);
116
117	if(dist1>distance && dist2>distance)
118	continue;
119	}
120	else
121	{
122	distance_t dist1=Distance(lat1,lon1,latitude,longitude);
123	distance_t dist2=Distance(lat2,lon1,latitude,longitude);
124	distance_t dist3=Distance(lat2,lon2,latitude,longitude);
125	distance_t dist4=Distance(lat1,lon2,latitude,longitude);
126
127	if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
128	continue;
129	}
130	}
131
132	for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
133	{
134	float lat=(float)((nodes->latzero+latbin)*LAT_LONG_BIN+nodes->nodes[i].latoffset)/LAT_LONG_SCALE;
135	float lon=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+nodes->nodes[i].lonoffset)/LAT_LONG_SCALE;
136
137	distance_t dist=Distance(lat,lon,latitude,longitude);
138
139	if(dist<*distance)
140	{best=i; *distance=dist;}
141	}
142
143	count++;
144	}
145
146	delta++;
147	}
148	while(count);
149
150	if(best==~0)
151	return(NULL);
152	else
153	return(&nodes->nodes[best]);
154	}
155
156
157	/*++++++++++++++++++++++++++++++++++++++
158	Get the latitude and longitude associated with a node.
159
160	Nodes *nodes The set of nodes.
161
162	Node *node The node.
163
164	float *latitude Returns the latitude.
165
166	float *longitude Returns the logitude.
167	++++++++++++++++++++++++++++++++++++++*/
168
169	void GetLatLong(Nodes nodes,Node node,float latitude,float longitude)
170	{
171	index_t index=IndexNode(nodes,node);
172	int latbin=-1,lonbin=-1;
173	int start,end,mid;
174
175	/* Binary search - search key closest below is required.
176	*
177	* # <- start \| Check mid and move start or end if it doesn't match
178	* # \|
179	* # \| Since an inexact match is wanted we must set end=mid-1
180	* # <- mid \| or start=mid because we know that mid doesn't match.
181	* # \|
182	* # \| Eventually either end=start or end=start+1 and one of
183	* # <- end \| start or end is the wanted one.
184	*/
185
186	/* Search for longitude */
187
188	start=0;
189	end=nodes->lonbins-1;
190
191	do
192	{
193	mid=(start+end)/2; /* Choose mid point */
194
195	if(nodes->offsets[nodes->latbinsmid]<index) / Mid point is too low */
196	start=mid;
197	else if(nodes->offsets[nodes->latbinsmid]>index) / Mid point is too high */
198	end=mid-1;
199	else /* Mid point is correct */
200	{lonbin=mid;break;}
201	}
202	while((end-start)>1);
203
204	if(lonbin==-1)
205	{
206	if(nodes->offsets[nodes->latbins*end]>index)
207	lonbin=start;
208	else
209	lonbin=end;
210	}
211
212	while(lonbin<nodes->lonbins && nodes->offsets[lonbinnodes->latbins]==nodes->offsets[(lonbin+1)nodes->latbins])
213	lonbin++;
214
215	/* Search for latitude */
216
217	start=0;
218	end=nodes->latbins-1;
219
220	do
221	{
222	mid=(start+end)/2; /* Choose mid point */
223
224	if(nodes->offsets[lonbinnodes->latbins+mid]<index) / Mid point is too low */
225	start=mid;
226	else if(nodes->offsets[lonbinnodes->latbins+mid]>index) / Mid point is too high */
227	end=mid-1;
228	else /* Mid point is correct */
229	{latbin=mid;break;}
230	}
231	while((end-start)>1);
232
233	if(latbin==-1)
234	{
235	if(nodes->offsets[lonbin*nodes->latbins+end]>index)
236	latbin=start;
237	else
238	latbin=end;
239	}
240
241	while(latbin<nodes->latbins && nodes->offsets[lonbinnodes->latbins+latbin]==nodes->offsets[lonbinnodes->latbins+latbin+1])
242	latbin++;
243
244	/* Return the values */
245
246	latitude =(float)((nodes->latzero+latbin)LAT_LONG_BIN+node->latoffset)/LAT_LONG_SCALE;
247	longitude=(float)((nodes->lonzero+lonbin)LAT_LONG_BIN+node->lonoffset)/LAT_LONG_SCALE;
248	}