trunk/src/nodes.c

/***************************************
 $Header: /home/amb/CVS/routino/src/nodes.c,v 1.24 2009-02-15 14:30:10 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.
  ++++++++++++++++++++++++++++++++++++++*/

Node *FindNode(Nodes* nodes,float latitude,float longitude)
{
 int32_t latbin=lat_long_to_bin(latitude )-nodes->latzero;
 int32_t lonbin=lat_long_to_bin(longitude)-nodes->lonzero;
 int llbin=lonbin*nodes->latbins+latbin;
 int i,best=~0;
 distance_t distance=km_to_distance(10);

 // FIXME - closest could be outside of this square

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

 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;

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

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

 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	amb	2	/***************************************
2	amb	121	$Header: /home/amb/CVS/routino/src/nodes.c,v 1.24 2009-02-15 14:30:10 amb Exp $
3	amb	2
4			Node data type functions.
5			****************/ /****************
6			Written by Andrew M. Bishop
7
8	amb	4	This file Copyright 2008,2009 Andrew M. Bishop
9	amb	2	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	amb	118	#include <math.h>
17	amb	2
18	amb	109	#include "nodes.h"
19	amb	114	#include "segments.h"
20	amb	2	#include "functions.h"
21
22
23	amb	17	/*++++++++++++++++++++++++++++++++++++++
24	amb	2	Load in a node list from a file.
25
26	amb	19	Nodes* LoadNodeList Returns the node list.
27	amb	17
28	amb	2	const char *filename The name of the file to load.
29			++++++++++++++++++++++++++++++++++++++*/
30
31	amb	19	Nodes LoadNodeList(const char filename)
32	amb	2	{
33	amb	88	void *data;
34			Nodes *nodes;
35
36			nodes=(Nodes*)malloc(sizeof(Nodes));
37
38			data=MapFile(filename);
39
40	amb	100	if(!data)
41			return(NULL);
42
43	amb	88	/* Copy the Nodes structure from the loaded data */
44
45			nodes=((Nodes*)data);
46
47			/* Adjust the pointers in the Nodes structure. */
48
49	amb	99	nodes->data=data;
50			nodes->offsets=(index_t*)(data+(off_t)nodes->offsets);
51	amb	88	nodes->nodes=(Node*)(data+(off_t)nodes->nodes);
52
53			return(nodes);
54	amb	2	}
55
56
57			/*++++++++++++++++++++++++++++++++++++++
58	amb	99	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
69			Node FindNode(Nodes nodes,float latitude,float longitude)
70			{
71	amb	118	int32_t latbin=lat_long_to_bin(latitude )-nodes->latzero;
72			int32_t lonbin=lat_long_to_bin(longitude)-nodes->lonzero;
73	amb	99	int llbin=lonbin*nodes->latbins+latbin;
74	amb	108	int i,best=~0;
75	amb	114	distance_t distance=km_to_distance(10);
76	amb	99
77	amb	118	// FIXME - closest could be outside of this square
78
79	amb	121	if(llbin<0 \|\| llbin>nodes->number)
80			return(NULL);
81
82	amb	99	for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
83			{
84	amb	118	float lat=(float)((nodes->latzero+latbin)*LAT_LONG_BIN+nodes->nodes[i].latoffset)/LAT_LONG_SCALE;
85			float lon=(float)((nodes->lonzero+lonbin)*LAT_LONG_BIN+nodes->nodes[i].lonoffset)/LAT_LONG_SCALE;
86	amb	99
87			float dist=Distance(lat,lon,latitude,longitude);
88
89			if(dist<distance)
90			{best=i; distance=dist;}
91			}
92
93	amb	108	if(best==~0)
94			return(NULL);
95			else
96			return(&nodes->nodes[best]);
97	amb	99	}
98
99
100			/*++++++++++++++++++++++++++++++++++++++
101			Get the latitude and longitude associated with a node.
102
103			Nodes *nodes The set of nodes.
104
105			Node *node The node.
106
107			float *latitude Returns the latitude.
108
109			float *longitude Returns the logitude.
110			++++++++++++++++++++++++++++++++++++++*/
111
112			void GetLatLong(Nodes nodes,Node node,float latitude,float longitude)
113			{
114			index_t index=IndexNode(nodes,node);
115			int latbin=-1,lonbin=-1;
116			int start,end,mid;
117
118			/* Binary search - search key closest below is required.
119			*
120			* # <- start \| Check mid and move start or end if it doesn't match
121			* # \|
122			* # \| Since an inexact match is wanted we must set end=mid-1
123			* # <- mid \| or start=mid because we know that mid doesn't match.
124			* # \|
125			* # \| Eventually either end=start or end=start+1 and one of
126			* # <- end \| start or end is the wanted one.
127			*/
128
129			/* Search for longitude */
130
131			start=0;
132			end=nodes->lonbins-1;
133
134			do
135			{
136			mid=(start+end)/2; /* Choose mid point */
137
138			if(nodes->offsets[nodes->latbinsmid]<index) / Mid point is too low */
139			start=mid;
140			else if(nodes->offsets[nodes->latbinsmid]>index) / Mid point is too high */
141			end=mid-1;
142			else /* Mid point is correct */
143			{lonbin=mid;break;}
144			}
145			while((end-start)>1);
146
147			if(lonbin==-1)
148			{
149			if(nodes->offsets[nodes->latbins*end]>index)
150			lonbin=start;
151			else
152			lonbin=end;
153			}
154
155			while(lonbin<nodes->lonbins && nodes->offsets[lonbinnodes->latbins]==nodes->offsets[(lonbin+1)nodes->latbins])
156			lonbin++;
157
158			/* Search for latitude */
159
160			start=0;
161			end=nodes->latbins-1;
162
163			do
164			{
165			mid=(start+end)/2; /* Choose mid point */
166
167			if(nodes->offsets[lonbinnodes->latbins+mid]<index) / Mid point is too low */
168			start=mid;
169			else if(nodes->offsets[lonbinnodes->latbins+mid]>index) / Mid point is too high */
170			end=mid-1;
171			else /* Mid point is correct */
172			{latbin=mid;break;}
173			}
174			while((end-start)>1);
175
176			if(latbin==-1)
177			{
178			if(nodes->offsets[lonbin*nodes->latbins+end]>index)
179			latbin=start;
180			else
181			latbin=end;
182			}
183
184			while(latbin<nodes->latbins && nodes->offsets[lonbinnodes->latbins+latbin]==nodes->offsets[lonbinnodes->latbins+latbin+1])
185			latbin++;
186
187			/* Return the values */
188
189	amb	118	latitude =(float)((nodes->latzero+latbin)LAT_LONG_BIN+node->latoffset)/LAT_LONG_SCALE;
190			longitude=(float)((nodes->lonzero+lonbin)LAT_LONG_BIN+node->lonoffset)/LAT_LONG_SCALE;
191	amb	99	}