trunk/src/segments.c

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

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2011 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 <sys/types.h>
#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 *segment;
 index_t best_seg=NO_SEGMENT;
 double best_difference=360;

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

    segment=FirstSegment(segments,node,1);
   }

 while(segment)
   {
    Way *way;
    index_t node2,seg2;
    double bearing,difference;

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

    if(!IsNormalSegment(segment))
       goto endloop;

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

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

    way=LookupWay(ways,segment->way,1);

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

    bearing=BearingAngle(nodes,segment,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))
       segment=NextFakeSegment(segment,node1);
    else if(IsFakeNode(node2))
       segment=NULL; /* cannot call NextSegment() with a fake segment */
    else
       segment=NextSegment(segments,segment,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 *segment The segment to traverse.

  Way *way The way that the segment belongs to.

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

duration_t Duration(Segment *segment,Way *way,Profile *profile)
{
 speed_t    speed1=way->speed;
 speed_t    speed2=profile->speed[HIGHWAY(way->type)];
 distance_t distance=DISTANCE(segment->distance);

 if(speed1==0)
   {
    if(speed2==0)
       return(hours_to_duration(10));
    else
       return distance_speed_to_duration(distance,speed2);
   }
 else /* if(speed1!=0) */
   {
    if(speed2==0)
       return distance_speed_to_duration(distance,speed1);
    else if(speed1<=speed2)
       return distance_speed_to_duration(distance,speed1);
    else
       return distance_speed_to_duration(distance,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 *segment1 The current segment.

  Segment *segment2 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 *segment1,Segment *segment2,index_t node)
{
 double lat1,latm,lat2;
 double lon1,lonm,lon2;
 double angle1,angle2,angle;
 index_t node1,node2;

 node1=OtherNode(segment1,node);
 node2=OtherNode(segment2,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 *segment 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 *segment,index_t node)
{
 double lat1,lat2;
 double lon1,lon2;
 double angle;
 index_t node1,node2;

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