trunk/src/segments.c

/***************************************
 Segment data type functions.

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2012 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 "fakes.h"
#include "files.h"
#include "profiles.h"


/*++++++++++++++++++++++++++++++++++++++
  Load in a segment list from a file.

  Segments *LoadSegmentList Returns the segment list that has just been loaded.

  const char *filename The name of the file to load.
  ++++++++++++++++++++++++++++++++++++++*/

Segments *LoadSegmentList(const char *filename)
{
 Segments *segments;
#if SLIM
 int i;
#endif

 segments=(Segments*)malloc(sizeof(Segments));

#if !SLIM

 segments->data=MapFile(filename);

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

 segments->file=*((SegmentsFile*)segments->data);

 /* Set the pointers in the Segments structure. */

 segments->segments=(Segment*)(segments->data+sizeof(SegmentsFile));

#else

 segments->fd=ReOpenFile(filename);

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

 ReadFile(segments->fd,&segments->file,sizeof(SegmentsFile));

 for(i=0;i<sizeof(segments->cached)/sizeof(segments->cached[0]);i++)
    segments->incache[i]=NO_SEGMENT;

#endif

 return(segments);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the closest segment from a specified node heading in a particular direction and optionally profile.

  index_t FindClosestSegmentHeading Returns the closest heading segment index.

  Nodes *nodes The set of nodes to use.

  Segments *segments The set of segments to use.

  Ways *ways The set of ways to use.

  index_t node1 The node to start from.

  double heading The desired heading from the node.

  Profile *profile The profile of the mode of transport (or NULL).
  ++++++++++++++++++++++++++++++++++++++*/

index_t FindClosestSegmentHeading(Nodes *nodes,Segments *segments,Ways *ways,index_t node1,double heading,Profile *profile)
{
 Segment *segmentp;
 index_t best_seg=NO_SEGMENT;
 double best_difference=360;

 if(IsFakeNode(node1))
    segmentp=FirstFakeSegment(node1);
 else
   {
    Node *nodep=LookupNode(nodes,node1,3);

    segmentp=FirstSegment(segments,nodep,1);
   }

 while(segmentp)
   {
    Way *wayp;
    index_t node2,seg2;
    double bearing,difference;

    node2=OtherNode(segmentp,node1);  /* need this here because we use node2 at the end of the loop */

    if(!IsNormalSegment(segmentp))
       goto endloop;

    if(profile->oneway && IsOnewayFrom(segmentp,node1))
       goto endloop;

    if(IsFakeNode(node1) || IsFakeNode(node2))
       seg2=IndexFakeSegment(segmentp);
    else
       seg2=IndexSegment(segments,segmentp);

    wayp=LookupWay(ways,segmentp->way,1);

    if(!(wayp->allow&profile->allow))
       goto endloop;

    bearing=BearingAngle(nodes,segmentp,node1);

    difference=(heading-bearing);

    if(difference<-180) difference+=360;
    if(difference> 180) difference-=360;

    if(difference<0) difference=-difference;

    if(difference<best_difference)
      {
       best_difference=difference;
       best_seg=seg2;
      }

   endloop:

    if(IsFakeNode(node1))
       segmentp=NextFakeSegment(segmentp,node1);
    else if(IsFakeNode(node2))
       segmentp=NULL; /* cannot call NextSegment() with a fake segment */
    else
       segmentp=NextSegment(segments,segmentp,node1);
   }

 return(best_seg);
}


/*++++++++++++++++++++++++++++++++++++++
  Calculate the distance between two locations.

  distance_t Distance Returns the distance between the locations.

  double lat1 The latitude of the first location.

  double lon1 The longitude of the first location.

  double lat2 The latitude of the second location.

  double lon2 The longitude of the second location.
  ++++++++++++++++++++++++++++++++++++++*/

distance_t Distance(double lat1,double lon1,double lat2,double lon2)
{
 double dlon = lon1 - lon2;
 double dlat = lat1 - lat2;

 double a1,a2,a,sa,c,d;

 if(dlon==0 && dlat==0)
   return 0;

 a1 = sin (dlat / 2);
 a2 = sin (dlon / 2);
 a = (a1 * a1) + cos (lat1) * cos (lat2) * a2 * a2;
 sa = sqrt (a);
 if (sa <= 1.0)
   {c = 2 * asin (sa);}
 else
   {c = 2 * asin (1.0);}
 d = 6378.137 * c;

 return km_to_distance(d);
}


/*++++++++++++++++++++++++++++++++++++++
  Calculate the duration of travel on a segment.

  duration_t Duration Returns the duration of travel.

  Segment *segmentp The segment to traverse.

  Way *wayp The way that the segment belongs to.

  Profile *profile The profile of the transport being used.
  ++++++++++++++++++++++++++++++++++++++*/

duration_t Duration(Segment *segmentp,Way *wayp,Profile *profile)
{
 speed_t   speed1=wayp->speed;
 speed_t   speed2=profile->speed[HIGHWAY(wayp->type)];
 segdist_t length=segmentp->length;

 if(speed1==0)
   {
    if(speed2==0)
       return(hours_to_duration(10));
    else
       return distance_speed_to_duration(length,speed2);
   }
 else /* if(speed1!=0) */
   {
    if(speed2==0)
       return distance_speed_to_duration(length,speed1);
    else if(speed1<=speed2)
       return distance_speed_to_duration(length,speed1);
    else
       return distance_speed_to_duration(length,speed2);
   }
}


/*++++++++++++++++++++++++++++++++++++++
  Calculate the angle to turn at a junction from segment1 to segment2 at node.

  double TurnAngle Returns a value in the range -180 to +180 indicating the angle to turn.

  Nodes *nodes The set of nodes to use.

  Segment *segment1p The current segment.

  Segment *segment2p The next segment.

  index_t node The node at which they join.

  Straight ahead is zero, turning to the right is positive (e.g. +90 degrees) and turning to the left is negative (e.g. -90 degrees).
  Angles are calculated using flat Cartesian lat/long grid approximation (after scaling longitude due to latitude).
  ++++++++++++++++++++++++++++++++++++++*/

double TurnAngle(Nodes *nodes,Segment *segment1p,Segment *segment2p,index_t node)
{
 double lat1,latm,lat2;
 double lon1,lonm,lon2;
 double angle1,angle2,angle;
 index_t node1,node2;

 node1=OtherNode(segment1p,node);
 node2=OtherNode(segment2p,node);

 if(IsFakeNode(node1))
    GetFakeLatLong(node1,&lat1,&lon1);
 else
    GetLatLong(nodes,node1,&lat1,&lon1);

 if(IsFakeNode(node))
    GetFakeLatLong(node,&latm,&lonm);
 else
    GetLatLong(nodes,node,&latm,&lonm);

 if(IsFakeNode(node2))
    GetFakeLatLong(node2,&lat2,&lon2);
 else
    GetLatLong(nodes,node2,&lat2,&lon2);

 angle1=atan2((lonm-lon1)*cos(latm),(latm-lat1));
 angle2=atan2((lon2-lonm)*cos(latm),(lat2-latm));

 angle=angle2-angle1;

 angle=radians_to_degrees(angle);

 if(angle<-180) angle+=360;
 if(angle> 180) angle-=360;

 return(angle);
}


/*++++++++++++++++++++++++++++++++++++++
  Calculate the bearing of a segment when heading to the given node.

  double BearingAngle Returns a value in the range 0 to 359 indicating the bearing.

  Nodes *nodes The set of nodes to use.

  Segment *segmentp The segment.

  index_t node The node to finish.

  Angles are calculated using flat Cartesian lat/long grid approximation (after scaling longitude due to latitude).
  ++++++++++++++++++++++++++++++++++++++*/

double BearingAngle(Nodes *nodes,Segment *segmentp,index_t node)
{
 double lat1,lat2;
 double lon1,lon2;
 double angle;
 index_t node1,node2;

 node1=node;
 node2=OtherNode(segmentp,node);

 if(IsFakeNode(node1))
    GetFakeLatLong(node1,&lat1,&lon1);
 else
    GetLatLong(nodes,node1,&lat1,&lon1);

 if(IsFakeNode(node2))
    GetFakeLatLong(node2,&lat2,&lon2);
 else
    GetLatLong(nodes,node2,&lat2,&lon2);

 angle=atan2((lat2-lat1),(lon2-lon1)*cos(lat1));

 angle=radians_to_degrees(angle);

 angle=270-angle;

 if(angle<  0) angle+=360;
 if(angle>360) angle-=360;

 return(angle);
}
1	amb	2	/***************************************
2			Segment data type functions.
3	amb	151
4			Part of the Routino routing software.
5	amb	2	****************/ /****************
6	amb	955	This file Copyright 2008-2012 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	669	#include <math.h>
25	amb	2
26	amb	109	#include "types.h"
27	amb	39	#include "nodes.h"
28	amb	26	#include "segments.h"
29	amb	109	#include "ways.h"
30	amb	955
31	amb	672	#include "fakes.h"
32	amb	449	#include "files.h"
33	amb	109	#include "profiles.h"
34	amb	2
35
36	amb	23	/*++++++++++++++++++++++++++++++++++++++
37	amb	2	Load in a segment list from a file.
38
39	amb	681	Segments *LoadSegmentList Returns the segment list that has just been loaded.
40	amb	23
41	amb	2	const char *filename The name of the file to load.
42			++++++++++++++++++++++++++++++++++++++*/
43
44	amb	23	Segments LoadSegmentList(const char filename)
45	amb	2	{
46	amb	88	Segments *segments;
47	amb	706	#if SLIM
48			int i;
49			#endif
50	amb	88
51			segments=(Segments*)malloc(sizeof(Segments));
52
53	amb	459	#if !SLIM
54	amb	88
55	amb	459	segments->data=MapFile(filename);
56	amb	88
57	amb	459	/* Copy the SegmentsFile structure from the loaded data */
58	amb	88
59	amb	459	segments->file=((SegmentsFile)segments->data);
60	amb	88
61	amb	459	/* Set the pointers in the Segments structure. */
62	amb	88
63	amb	459	segments->segments=(Segment*)(segments->data+sizeof(SegmentsFile));
64
65			#else
66
67			segments->fd=ReOpenFile(filename);
68
69			/* Copy the SegmentsFile header structure from the loaded data */
70
71			ReadFile(segments->fd,&segments->file,sizeof(SegmentsFile));
72
73	amb	706	for(i=0;i<sizeof(segments->cached)/sizeof(segments->cached[0]);i++)
74			segments->incache[i]=NO_SEGMENT;
75	amb	459
76			#endif
77
78	amb	88	return(segments);
79	amb	2	}
80
81
82			/*++++++++++++++++++++++++++++++++++++++
83	amb	675	Find the closest segment from a specified node heading in a particular direction and optionally profile.
84
85			index_t FindClosestSegmentHeading Returns the closest heading segment index.
86
87	amb	681	Nodes *nodes The set of nodes to use.
88	amb	675
89			Segments *segments The set of segments to use.
90
91			Ways *ways The set of ways to use.
92
93			index_t node1 The node to start from.
94
95			double heading The desired heading from the node.
96
97			Profile *profile The profile of the mode of transport (or NULL).
98			++++++++++++++++++++++++++++++++++++++*/
99
100	amb	681	index_t FindClosestSegmentHeading(Nodes nodes,Segments segments,Ways ways,index_t node1,double heading,Profile profile)
101	amb	675	{
102	amb	1078	Segment *segmentp;
103	amb	675	index_t best_seg=NO_SEGMENT;
104			double best_difference=360;
105
106			if(IsFakeNode(node1))
107	amb	1078	segmentp=FirstFakeSegment(node1);
108	amb	675	else
109	amb	885	{
110	amb	1078	Node *nodep=LookupNode(nodes,node1,3);
111	amb	675
112	amb	1078	segmentp=FirstSegment(segments,nodep,1);
113	amb	885	}
114
115	amb	1078	while(segmentp)
116	amb	675	{
117	amb	1078	Way *wayp;
118	amb	675	index_t node2,seg2;
119			double bearing,difference;
120
121	amb	1078	node2=OtherNode(segmentp,node1); /* need this here because we use node2 at the end of the loop */
122	amb	675
123	amb	1078	if(!IsNormalSegment(segmentp))
124	amb	675	goto endloop;
125
126	amb	1078	if(profile->oneway && IsOnewayFrom(segmentp,node1))
127	amb	675	goto endloop;
128
129			if(IsFakeNode(node1) \|\| IsFakeNode(node2))
130	amb	1078	seg2=IndexFakeSegment(segmentp);
131	amb	675	else
132	amb	1078	seg2=IndexSegment(segments,segmentp);
133	amb	675
134	amb	1078	wayp=LookupWay(ways,segmentp->way,1);
135	amb	675
136	amb	1078	if(!(wayp->allow&profile->allow))
137	amb	675	goto endloop;
138
139	amb	1078	bearing=BearingAngle(nodes,segmentp,node1);
140	amb	675
141			difference=(heading-bearing);
142
143			if(difference<-180) difference+=360;
144			if(difference> 180) difference-=360;
145
146			if(difference<0) difference=-difference;
147
148			if(difference<best_difference)
149			{
150			best_difference=difference;
151			best_seg=seg2;
152			}
153
154			endloop:
155
156			if(IsFakeNode(node1))
157	amb	1078	segmentp=NextFakeSegment(segmentp,node1);
158	amb	675	else if(IsFakeNode(node2))
159	amb	1078	segmentp=NULL; /* cannot call NextSegment() with a fake segment */
160	amb	675	else
161	amb	1078	segmentp=NextSegment(segments,segmentp,node1);
162	amb	675	}
163
164			return(best_seg);
165			}
166
167
168			/*++++++++++++++++++++++++++++++++++++++
169	amb	99	Calculate the distance between two locations.
170
171	amb	114	distance_t Distance Returns the distance between the locations.
172	amb	99
173	amb	219	double lat1 The latitude of the first location.
174	amb	99
175	amb	219	double lon1 The longitude of the first location.
176	amb	99
177	amb	219	double lat2 The latitude of the second location.
178	amb	99
179	amb	219	double lon2 The longitude of the second location.
180	amb	99	++++++++++++++++++++++++++++++++++++++*/
181
182	amb	219	distance_t Distance(double lat1,double lon1,double lat2,double lon2)
183	amb	99	{
184	amb	219	double dlon = lon1 - lon2;
185			double dlat = lat1 - lat2;
186	amb	99
187	amb	219	double a1,a2,a,sa,c,d;
188	amb	99
189			if(dlon==0 && dlat==0)
190			return 0;
191
192	amb	219	a1 = sin (dlat / 2);
193			a2 = sin (dlon / 2);
194			a = (a1 * a1) + cos (lat1) * cos (lat2) * a2 * a2;
195			sa = sqrt (a);
196	amb	99	if (sa <= 1.0)
197	amb	219	{c = 2 * asin (sa);}
198	amb	99	else
199	amb	219	{c = 2 * asin (1.0);}
200	amb	99	d = 6378.137 * c;
201
202	amb	114	return km_to_distance(d);
203	amb	99	}
204
205
206			/*++++++++++++++++++++++++++++++++++++++
207	amb	680	Calculate the duration of travel on a segment.
208	amb	63
209	amb	680	duration_t Duration Returns the duration of travel.
210	amb	63
211	amb	1078	Segment *segmentp The segment to traverse.
212	amb	63
213	amb	1078	Way *wayp The way that the segment belongs to.
214	amb	63
215	amb	82	Profile *profile The profile of the transport being used.
216	amb	63	++++++++++++++++++++++++++++++++++++++*/
217
218	amb	1078	duration_t Duration(Segment segmentp,Way wayp,Profile *profile)
219	amb	63	{
220	amb	1164	speed_t speed1=wayp->speed;
221			speed_t speed2=profile->speed[HIGHWAY(wayp->type)];
222			segdist_t length=segmentp->length;
223	amb	63
224	amb	137	if(speed1==0)
225			{
226			if(speed2==0)
227			return(hours_to_duration(10));
228			else
229	amb	1164	return distance_speed_to_duration(length,speed2);
230	amb	137	}
231			else /* if(speed1!=0) */
232			{
233			if(speed2==0)
234	amb	1164	return distance_speed_to_duration(length,speed1);
235	amb	137	else if(speed1<=speed2)
236	amb	1164	return distance_speed_to_duration(length,speed1);
237	amb	137	else
238	amb	1164	return distance_speed_to_duration(length,speed2);
239	amb	137	}
240	amb	63	}
241	amb	672
242
243			/*++++++++++++++++++++++++++++++++++++++
244			Calculate the angle to turn at a junction from segment1 to segment2 at node.
245
246			double TurnAngle Returns a value in the range -180 to +180 indicating the angle to turn.
247
248	amb	681	Nodes *nodes The set of nodes to use.
249	amb	672
250	amb	1078	Segment *segment1p The current segment.
251	amb	672
252	amb	1078	Segment *segment2p The next segment.
253	amb	672
254			index_t node The node at which they join.
255
256			Straight ahead is zero, turning to the right is positive (e.g. +90 degrees) and turning to the left is negative (e.g. -90 degrees).
257			Angles are calculated using flat Cartesian lat/long grid approximation (after scaling longitude due to latitude).
258			++++++++++++++++++++++++++++++++++++++*/
259
260	amb	1078	double TurnAngle(Nodes nodes,Segment segment1p,Segment *segment2p,index_t node)
261	amb	672	{
262			double lat1,latm,lat2;
263			double lon1,lonm,lon2;
264			double angle1,angle2,angle;
265			index_t node1,node2;
266
267	amb	1078	node1=OtherNode(segment1p,node);
268			node2=OtherNode(segment2p,node);
269	amb	672
270			if(IsFakeNode(node1))
271			GetFakeLatLong(node1,&lat1,&lon1);
272			else
273			GetLatLong(nodes,node1,&lat1,&lon1);
274
275			if(IsFakeNode(node))
276			GetFakeLatLong(node,&latm,&lonm);
277			else
278			GetLatLong(nodes,node,&latm,&lonm);
279
280			if(IsFakeNode(node2))
281			GetFakeLatLong(node2,&lat2,&lon2);
282			else
283			GetLatLong(nodes,node2,&lat2,&lon2);
284
285			angle1=atan2((lonm-lon1)*cos(latm),(latm-lat1));
286			angle2=atan2((lon2-lonm)*cos(latm),(lat2-latm));
287
288			angle=angle2-angle1;
289
290			angle=radians_to_degrees(angle);
291
292			if(angle<-180) angle+=360;
293			if(angle> 180) angle-=360;
294
295			return(angle);
296			}
297
298
299			/*++++++++++++++++++++++++++++++++++++++
300	amb	675	Calculate the bearing of a segment when heading to the given node.
301	amb	672
302			double BearingAngle Returns a value in the range 0 to 359 indicating the bearing.
303
304	amb	681	Nodes *nodes The set of nodes to use.
305	amb	672
306	amb	1078	Segment *segmentp The segment.
307	amb	672
308	amb	675	index_t node The node to finish.
309	amb	672
310			Angles are calculated using flat Cartesian lat/long grid approximation (after scaling longitude due to latitude).
311			++++++++++++++++++++++++++++++++++++++*/
312
313	amb	1078	double BearingAngle(Nodes nodes,Segment segmentp,index_t node)
314	amb	672	{
315			double lat1,lat2;
316			double lon1,lon2;
317			double angle;
318			index_t node1,node2;
319
320			node1=node;
321	amb	1078	node2=OtherNode(segmentp,node);
322	amb	672
323			if(IsFakeNode(node1))
324			GetFakeLatLong(node1,&lat1,&lon1);
325			else
326			GetLatLong(nodes,node1,&lat1,&lon1);
327
328			if(IsFakeNode(node2))
329			GetFakeLatLong(node2,&lat2,&lon2);
330			else
331			GetLatLong(nodes,node2,&lat2,&lon2);
332
333			angle=atan2((lat2-lat1),(lon2-lon1)*cos(lat1));
334
335			angle=radians_to_degrees(angle);
336
337			angle=270-angle;
338
339			if(angle< 0) angle+=360;
340			if(angle>360) angle-=360;
341
342			return(angle);
343			}