trunk/src/segments.c

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

 Part of the Routino routing software.
 ******************/ /******************
 This file Copyright 2008-2015 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;

 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=SlimMapFile(filename);

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

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

 segments->cache=NewSegmentCache();
#ifndef LIBROUTINO
 log_malloc(segments->cache,sizeof(*segments->cache));
#endif

#endif

 return(segments);
}


/*++++++++++++++++++++++++++++++++++++++
  Destroy the segment list.

  Segments *segments The segment list to destroy.
  ++++++++++++++++++++++++++++++++++++++*/

void DestroySegmentList(Segments *segments)
{
#if !SLIM

 segments->data=UnmapFile(segments->data);

#else

 segments->fd=SlimUnmapFile(segments->fd);

#ifndef LIBROUTINO
 log_free(segments->cache);
#endif
 DeleteSegmentCache(segments->cache);

#endif

 free(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(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;

    if(profile->oneway && IsOnewayFrom(segmentp,node1))
      {
       if(profile->allow!=Transports_Bicycle)
          goto endloop;

       if(!(wayp->type&Highway_CycleBothWays))
          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 change in latitude (same longitude) between two locations a known distance apart.

  double DeltaLat Returns the difference in latitude between the locations.

  double lon The longitude of the locations.

  distance_t distance The distance between the locations.
  ++++++++++++++++++++++++++++++++++++++*/

double DeltaLat(double lon,distance_t distance)
{
 double dlat;

 double c,d;

 if(distance==0)
   return 0;

 d = distance_to_km(distance);

 c = d / 6378.137;

 dlat = c;

 return dlat;
}


/*++++++++++++++++++++++++++++++++++++++
  Calculate the change in longitude (same latitude) between two locations a known distance apart.

  double DeltaLon Returns the difference in longitude between the locations.

  double lat The latitude of the locations.

  distance_t distance The distance between the locations.
  ++++++++++++++++++++++++++++++++++++++*/

double DeltaLon(double lat,distance_t distance)
{
 double dlon;

 double a2,sa,c,d;

 if(distance==0)
   return 0;

 d = distance_to_km(distance);

 c = d / 6378.137;

 sa = sin(c/2);

 a2 = sa / cos(lat);

 dlon = 2*asin(a2);

 return dlon;
}


/*++++++++++++++++++++++++++++++++++++++
  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)];
 distance_t distance=DISTANCE(segmentp->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 *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,NULL,&lat1,&lon1);

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

 if(IsFakeNode(node2))
    GetFakeLatLong(node2,&lat2,&lon2);
 else
    GetLatLong(nodes,node2,NULL,&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,NULL,&lat1,&lon1);

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