destination-access/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;

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

 segments->incache[0]=NO_SEGMENT;
 segments->incache[1]=NO_SEGMENT;
 segments->incache[2]=NO_SEGMENT;

#endif

 return(segments);
}


/*++++++++++++++++++++++++++++++++++++++
  Find the next segment with a particular starting node.

  Segment *NextSegment Returns a pointer to the next segment with the same id.

  Segments* segments The set of segments to process.

  Segment *segment The current segment.

  index_t node The current node.
  ++++++++++++++++++++++++++++++++++++++*/

Segment *NextSegment(Segments* segments,Segment *segment,index_t node)
{
 if(segment->node1==node)
   {
#if SLIM
    index_t index=IndexSegment(segments,segment);
    index++;

    if(index>=segments->file.number)
       return(NULL);
    segment=LookupSegment(segments,index,1);
    if(segment->node1!=node)
       return(NULL);
    else
       return(segment);
#else
    segment++;
    if(IndexSegment(segments,segment)>=segments->file.number || segment->node1!=node)
       return(NULL);
    else
       return(segment);
#endif
   }
 else
   {
    if(segment->next2==NO_NODE)
       return(NULL);
    else
       return(LookupSegment(segments,segment->next2,1));
   }
}


/*++++++++++++++++++++++++++++++++++++++
  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 search.

  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
    segment=FirstSegment(segments,nodes,node1);

 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 segment.

  duration_t Duration Returns the duration of travel between the nodes.

  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.

  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.

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