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.

  Segments *segments The set of segments to use.

  Segment *segment The current segment.

  index_t node The wanted 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_SEGMENT)
       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 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
    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 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	amb	2	/***************************************
2			Segment data type functions.
3	amb	151
4			Part of the Routino routing software.
5	amb	2	****************/ /****************
6	amb	669	This file Copyright 2008-2011 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	amb	214	#include <sys/types.h>
24	amb	2	#include <stdlib.h>
25	amb	669	#include <math.h>
26	amb	2
27	amb	109	#include "types.h"
28	amb	39	#include "nodes.h"
29	amb	26	#include "segments.h"
30	amb	109	#include "ways.h"
31	amb	672	#include "fakes.h"
32	amb	449
33			#include "files.h"
34	amb	109	#include "profiles.h"
35	amb	2
36
37	amb	23	/*++++++++++++++++++++++++++++++++++++++
38	amb	2	Load in a segment list from a file.
39
40	amb	681	Segments *LoadSegmentList Returns the segment list that has just been loaded.
41	amb	23
42	amb	2	const char *filename The name of the file to load.
43			++++++++++++++++++++++++++++++++++++++*/
44
45	amb	23	Segments LoadSegmentList(const char filename)
46	amb	2	{
47	amb	88	Segments *segments;
48
49			segments=(Segments*)malloc(sizeof(Segments));
50
51	amb	459	#if !SLIM
52	amb	88
53	amb	459	segments->data=MapFile(filename);
54	amb	88
55	amb	459	/* Copy the SegmentsFile structure from the loaded data */
56	amb	88
57	amb	459	segments->file=((SegmentsFile)segments->data);
58	amb	88
59	amb	459	/* Set the pointers in the Segments structure. */
60	amb	88
61	amb	459	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	amb	88	return(segments);
78	amb	2	}
79
80
81			/*++++++++++++++++++++++++++++++++++++++
82			Find the next segment with a particular starting node.
83
84	amb	680	Segment *NextSegment Returns a pointer to the next segment.
85	amb	2
86	amb	681	Segments *segments The set of segments to use.
87	amb	23
88	amb	2	Segment *segment The current segment.
89	amb	228
90	amb	680	index_t node The wanted node.
91	amb	2	++++++++++++++++++++++++++++++++++++++*/
92
93	amb	681	Segment NextSegment(Segments segments,Segment *segment,index_t node)
94	amb	2	{
95	amb	208	if(segment->node1==node)
96	amb	128	{
97	amb	459	#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	amb	128	segment++;
110	amb	459	if(IndexSegment(segments,segment)>=segments->file.number \|\| segment->node1!=node)
111	amb	128	return(NULL);
112			else
113			return(segment);
114	amb	459	#endif
115	amb	128	}
116	amb	104	else
117	amb	128	{
118	amb	680	if(segment->next2==NO_SEGMENT)
119	amb	128	return(NULL);
120			else
121	amb	459	return(LookupSegment(segments,segment->next2,1));
122	amb	128	}
123	amb	89	}
124	amb	675
125
126	amb	2	/*++++++++++++++++++++++++++++++++++++++
127	amb	675	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	amb	681	Nodes *nodes The set of nodes to use.
132	amb	675
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	amb	681	index_t FindClosestSegmentHeading(Nodes nodes,Segments segments,Ways ways,index_t node1,double heading,Profile profile)
145	amb	675	{
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	amb	99	Calculate the distance between two locations.
210
211	amb	114	distance_t Distance Returns the distance between the locations.
212	amb	99
213	amb	219	double lat1 The latitude of the first location.
214	amb	99
215	amb	219	double lon1 The longitude of the first location.
216	amb	99
217	amb	219	double lat2 The latitude of the second location.
218	amb	99
219	amb	219	double lon2 The longitude of the second location.
220	amb	99	++++++++++++++++++++++++++++++++++++++*/
221
222	amb	219	distance_t Distance(double lat1,double lon1,double lat2,double lon2)
223	amb	99	{
224	amb	219	double dlon = lon1 - lon2;
225			double dlat = lat1 - lat2;
226	amb	99
227	amb	219	double a1,a2,a,sa,c,d;
228	amb	99
229			if(dlon==0 && dlat==0)
230			return 0;
231
232	amb	219	a1 = sin (dlat / 2);
233			a2 = sin (dlon / 2);
234			a = (a1 * a1) + cos (lat1) * cos (lat2) * a2 * a2;
235			sa = sqrt (a);
236	amb	99	if (sa <= 1.0)
237	amb	219	{c = 2 * asin (sa);}
238	amb	99	else
239	amb	219	{c = 2 * asin (1.0);}
240	amb	99	d = 6378.137 * c;
241
242	amb	114	return km_to_distance(d);
243	amb	99	}
244
245
246			/*++++++++++++++++++++++++++++++++++++++
247	amb	680	Calculate the duration of travel on a segment.
248	amb	63
249	amb	680	duration_t Duration Returns the duration of travel.
250	amb	63
251			Segment *segment The segment to traverse.
252
253			Way *way The way that the segment belongs to.
254
255	amb	82	Profile *profile The profile of the transport being used.
256	amb	63	++++++++++++++++++++++++++++++++++++++*/
257
258	amb	82	duration_t Duration(Segment segment,Way way,Profile *profile)
259	amb	63	{
260	amb	137	speed_t speed1=way->speed;
261			speed_t speed2=profile->speed[HIGHWAY(way->type)];
262	amb	90	distance_t distance=DISTANCE(segment->distance);
263	amb	63
264	amb	137	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	amb	63	}
281	amb	672
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	amb	681	Nodes *nodes The set of nodes to use.
289	amb	672
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	amb	675	Calculate the bearing of a segment when heading to the given node.
341	amb	672
342			double BearingAngle Returns a value in the range 0 to 359 indicating the bearing.
343
344	amb	681	Nodes *nodes The set of nodes to use.
345	amb	672
346			Segment *segment The segment.
347
348	amb	675	index_t node The node to finish.
349	amb	672
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			}