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	/***************************************
2	Segment data type functions.
3
4	Part of the Routino routing software.
5	****************/ /****************
6	This file Copyright 2008-2012 Andrew M. Bishop
7
8	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	***************************************/
21
22
23	#include <stdlib.h>
24	#include <math.h>
25
26	#include "types.h"
27	#include "nodes.h"
28	#include "segments.h"
29	#include "ways.h"
30
31	#include "fakes.h"
32	#include "files.h"
33	#include "profiles.h"
34
35
36	/*++++++++++++++++++++++++++++++++++++++
37	Load in a segment list from a file.
38
39	Segments *LoadSegmentList Returns the segment list that has just been loaded.
40
41	const char *filename The name of the file to load.
42	++++++++++++++++++++++++++++++++++++++*/
43
44	Segments LoadSegmentList(const char filename)
45	{
46	Segments *segments;
47	#if SLIM
48	int i;
49	#endif
50
51	segments=(Segments*)malloc(sizeof(Segments));
52
53	#if !SLIM
54
55	segments->data=MapFile(filename);
56
57	/* Copy the SegmentsFile structure from the loaded data */
58
59	segments->file=((SegmentsFile)segments->data);
60
61	/* Set the pointers in the Segments structure. */
62
63	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	for(i=0;i<sizeof(segments->cached)/sizeof(segments->cached[0]);i++)
74	segments->incache[i]=NO_SEGMENT;
75
76	#endif
77
78	return(segments);
79	}
80
81
82	/*++++++++++++++++++++++++++++++++++++++
83	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	Nodes *nodes The set of nodes to use.
88
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	index_t FindClosestSegmentHeading(Nodes nodes,Segments segments,Ways ways,index_t node1,double heading,Profile profile)
101	{
102	Segment *segmentp;
103	index_t best_seg=NO_SEGMENT;
104	double best_difference=360;
105
106	if(IsFakeNode(node1))
107	segmentp=FirstFakeSegment(node1);
108	else
109	{
110	Node *nodep=LookupNode(nodes,node1,3);
111
112	segmentp=FirstSegment(segments,nodep,1);
113	}
114
115	while(segmentp)
116	{
117	Way *wayp;
118	index_t node2,seg2;
119	double bearing,difference;
120
121	node2=OtherNode(segmentp,node1); /* need this here because we use node2 at the end of the loop */
122
123	if(!IsNormalSegment(segmentp))
124	goto endloop;
125
126	if(profile->oneway && IsOnewayFrom(segmentp,node1))
127	goto endloop;
128
129	if(IsFakeNode(node1) \|\| IsFakeNode(node2))
130	seg2=IndexFakeSegment(segmentp);
131	else
132	seg2=IndexSegment(segments,segmentp);
133
134	wayp=LookupWay(ways,segmentp->way,1);
135
136	if(!(wayp->allow&profile->allow))
137	goto endloop;
138
139	bearing=BearingAngle(nodes,segmentp,node1);
140
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	segmentp=NextFakeSegment(segmentp,node1);
158	else if(IsFakeNode(node2))
159	segmentp=NULL; /* cannot call NextSegment() with a fake segment */
160	else
161	segmentp=NextSegment(segments,segmentp,node1);
162	}
163
164	return(best_seg);
165	}
166
167
168	/*++++++++++++++++++++++++++++++++++++++
169	Calculate the distance between two locations.
170
171	distance_t Distance Returns the distance between the locations.
172
173	double lat1 The latitude of the first location.
174
175	double lon1 The longitude of the first location.
176
177	double lat2 The latitude of the second location.
178
179	double lon2 The longitude of the second location.
180	++++++++++++++++++++++++++++++++++++++*/
181
182	distance_t Distance(double lat1,double lon1,double lat2,double lon2)
183	{
184	double dlon = lon1 - lon2;
185	double dlat = lat1 - lat2;
186
187	double a1,a2,a,sa,c,d;
188
189	if(dlon==0 && dlat==0)
190	return 0;
191
192	a1 = sin (dlat / 2);
193	a2 = sin (dlon / 2);
194	a = (a1 * a1) + cos (lat1) * cos (lat2) * a2 * a2;
195	sa = sqrt (a);
196	if (sa <= 1.0)
197	{c = 2 * asin (sa);}
198	else
199	{c = 2 * asin (1.0);}
200	d = 6378.137 * c;
201
202	return km_to_distance(d);
203	}
204
205
206	/*++++++++++++++++++++++++++++++++++++++
207	Calculate the duration of travel on a segment.
208
209	duration_t Duration Returns the duration of travel.
210
211	Segment *segmentp The segment to traverse.
212
213	Way *wayp The way that the segment belongs to.
214
215	Profile *profile The profile of the transport being used.
216	++++++++++++++++++++++++++++++++++++++*/
217
218	duration_t Duration(Segment segmentp,Way wayp,Profile *profile)
219	{
220	speed_t speed1=wayp->speed;
221	speed_t speed2=profile->speed[HIGHWAY(wayp->type)];
222	segdist_t length=segmentp->length;
223
224	if(speed1==0)
225	{
226	if(speed2==0)
227	return(hours_to_duration(10));
228	else
229	return distance_speed_to_duration(length,speed2);
230	}
231	else /* if(speed1!=0) */
232	{
233	if(speed2==0)
234	return distance_speed_to_duration(length,speed1);
235	else if(speed1<=speed2)
236	return distance_speed_to_duration(length,speed1);
237	else
238	return distance_speed_to_duration(length,speed2);
239	}
240	}
241
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	Nodes *nodes The set of nodes to use.
249
250	Segment *segment1p The current segment.
251
252	Segment *segment2p The next segment.
253
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	double TurnAngle(Nodes nodes,Segment segment1p,Segment *segment2p,index_t node)
261	{
262	double lat1,latm,lat2;
263	double lon1,lonm,lon2;
264	double angle1,angle2,angle;
265	index_t node1,node2;
266
267	node1=OtherNode(segment1p,node);
268	node2=OtherNode(segment2p,node);
269
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	Calculate the bearing of a segment when heading to the given node.
301
302	double BearingAngle Returns a value in the range 0 to 359 indicating the bearing.
303
304	Nodes *nodes The set of nodes to use.
305
306	Segment *segmentp The segment.
307
308	index_t node The node to finish.
309
310	Angles are calculated using flat Cartesian lat/long grid approximation (after scaling longitude due to latitude).
311	++++++++++++++++++++++++++++++++++++++*/
312
313	double BearingAngle(Nodes nodes,Segment segmentp,index_t node)
314	{
315	double lat1,lat2;
316	double lon1,lon2;
317	double angle;
318	index_t node1,node2;
319
320	node1=node;
321	node2=OtherNode(segmentp,node);
322
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	}