1 #include <stdlib.h>
2 #include <stdio.h>
3 #include <string.h>
4 #include <math.h>
5 #include "geodesic.h"
6
7 /* These methods implemented using the Vincenty method.
8 *
9 * Vincenty's original paper is available here:
10 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
11 *
12 * Great examples of the methods are available at these locations
13 * http://www.movable-type.co.uk/scripts/latlong-vincenty.html
14 * http://www.codeproject.com/KB/cs/Vincentys_Formula.aspx
15 *
16 * The movable-type.co.uk page contains code (C) 2002-2008 Chris Veness
17 * and available under a LGPL license.
18 *
19 * The codeproject.com page contains code available under the
20 * Code Project Open License (CPOL) 1.02.
21 *
22 * These references are a courtesy only, as the code here is original
23 * and not a derivitive work of the code provided on these pages.
24 */
25
26 /*
27 * The Vincenty method does not converge for antipodal points, but the
28 * code here handles this special case. Since the earth is a flattened
29 * sphere, the shortest route is over the poles. The trip via the North or South
30 * Pole is equivalent, so we arbitrarily pick the South Pole.
31 *
32 * The length of a great circle route halfway around the world through the poles is
33 * 0.5 of the circumference of a circle with a radius of the semi minor axis.
34 */
35
GreatCircleDistBear(double Lon1,double Lat1,double Lon2,double Lat2,double * Dist,double * Bear1,double * Bear2)36 double Geodesic::GreatCircleDistBear( double Lon1, double Lat1, double Lon2, double Lat2,
37 double *Dist, double *Bear1, double *Bear2 )
38 {
39
40 /* Geodesic parameters per WGS-84 */
41 double a = GEODESIC_WGS84_SEMI_MAJORAXIS; /* in meters */
42 double b = GEODESIC_WGS84_SEMI_MINORAXIS; /* in meters */
43 double f = ( GEODESIC_WGS84_SEMI_MAJORAXIS - GEODESIC_WGS84_SEMI_MINORAXIS )
44 / GEODESIC_WGS84_SEMI_MAJORAXIS; /* Flattening */
45
46 double dLon; /* Change in longitude */
47 double rLat1, rLat2; /* Reduced Latitude */
48 double lambda, lambdaprime; /* Counting variables */
49 double sinrLat1, cosrLat1, sinrLat2, cosrLat2, sinlambda, coslambda; /* Intermediate calculations */
50 double sinsigma, cossigma, cos2sigmam, sigma, sinalpha, cos2alpha, C; /* Intermediate calculations */
51 double u2, A, B; /* Intermediate calculations */
52 double dist; /* Great circle distance */
53 int itersleft = 50; /* Convergence attempts */
54
55 /* Initialize the output variables */
56 if( Dist ) *Dist = 0.0;
57 if( Bear1 ) *Bear1 = 0.0;
58 if( Bear2 ) *Bear2 = 0.0;
59
60 if( fabs( Lon1 - Lon2 ) < 1e-12 && fabs( Lat1 - Lat2 ) < 1e-12 ) {
61 /* The start and end points are the same - the distance is zero */
62 return 0.0;
63 }
64
65 /* Convert inputs from degrees to radians */
66 Lon1 = GEODESIC_DEG2RAD(Lon1);
67 Lat1 = GEODESIC_DEG2RAD(Lat1);
68 Lon2 = GEODESIC_DEG2RAD(Lon2);
69 Lat2 = GEODESIC_DEG2RAD(Lat2);
70
71 /* Start the algorithm */
72 rLat1 = atan( ( 1 - f ) * tan( Lat1 ) );
73 rLat2 = atan( ( 1 - f ) * tan( Lat2 ) );
74 sinrLat1 = sin( rLat1 );
75 cosrLat1 = cos( rLat1 );
76 sinrLat2 = sin( rLat2 );
77 cosrLat2 = cos( rLat2 );
78 dLon = Lon2 - Lon1;
79
80 lambda = dLon;
81 lambdaprime = 2 * M_PI;
82
83 do {
84 sinlambda = sin( lambda );
85 coslambda = cos( lambda );
86 sinsigma = sqrt(
87 pow( cosrLat2 * sinlambda, 2.0 )
88 + pow( cosrLat1 * sinrLat2 - sinrLat1 * cosrLat2 * coslambda, 2.0 ) );
89 if( sinsigma < 1e-12 ) {
90 /* The points are antipodal */
91 dist = M_PI * b;
92 if( Dist ) *Dist = dist;
93 if( Bear1 ) *Bear1 = 180.0; /* Start heading for the South Pole */
94 if( Bear2 ) *Bear2 = 0.0; /* Wind up heading for the North Pole */
95
96 return dist;
97 }
98 cossigma = sinrLat1 * sinrLat2 + cosrLat1 * cosrLat2 * coslambda;
99 sigma = atan2( sinsigma, cossigma );
100 if( sinsigma == 0.0 ) sinalpha = 0.0;
101 else
102 sinalpha = cosrLat1 * cosrLat2 * sinlambda / sinsigma;
103 cos2alpha = 1 - pow( sinalpha, 2.0 );
104 if( cos2alpha == 0.0 ) cos2sigmam = 0.0;
105 else
106 cos2sigmam = cossigma - 2 * sinrLat1 * sinrLat2 / cos2alpha;
107 //if( isnan( cos2sigmam ) ) cos2sigmam = 0.0; /* Special case to handle circles at the equator */
108 C = f / 16 * cos2alpha * ( 4 + f * ( 4 - 3 * cos2alpha ) );
109 lambdaprime = lambda;
110 lambda = dLon + ( 1.0 - C ) * f * sinalpha * ( sigma + C * sinsigma *
111 ( cos2sigmam + C * cossigma * ( -1.0 + 2.0 * pow( cos2sigmam, 2.0 ) ) ) );
112 } while( fabs( lambda - lambdaprime ) > 1e-12 && ( itersleft-- ) );
113
114 if( itersleft == 0 ) {
115 /* It didn't converge. Assume antipodal points. */
116 dist = M_PI * b;
117 if( Dist ) *Dist = dist;
118 if( Bear1 ) *Bear1 = 180.0; /* Start heading for the South Pole */
119 if( Bear2 ) *Bear2 = 0.0; /* Wind up heading for the North Pole */
120
121 return dist;
122 }
123 u2 = cos2alpha * ( pow( a, 2.0 ) - pow( b, 2.0 ) ) / pow( b, 2.0 );
124 A = 1 + u2 / 16384 * ( 4096 + u2 * ( -768 + u2 * ( 320 - 175 * u2 ) ) );
125 B = u2 / 1024 * ( 256 + u2 * ( -128 + u2 * ( 74 - 74 * u2 ) ) );
126 dist = b * A * ( sigma - B * sinsigma * ( cos2sigmam + B / 4 * ( cossigma *
127 ( -1.0 + 2.0 * pow( cos2sigmam, 2.0 ) ) - B / 6 * cos2sigmam * ( -3.0 + 4.0 * pow( sinsigma, 2.0 ) )
128 * ( -3 + 4 * pow( cos2sigmam, 2.0 ) ) ) ) );
129 if( Dist ) *Dist = dist;
130 if( Bear1 ) {
131 *Bear1 = GEODESIC_RAD2DEG(atan2(cosrLat2*sinlambda,cosrLat1*sinrLat2-sinrLat1*cosrLat2*coslambda));
132 while( *Bear1 < 0.0 )
133 *Bear1 += 360.0;
134 }
135 if( Bear2 ) {
136 *Bear2 = GEODESIC_RAD2DEG(atan2(cosrLat1*sinlambda,-sinrLat1*cosrLat2+cosrLat1*sinrLat2*coslambda));
137 while( *Bear2 < 0.0 )
138 *Bear2 += 360.0;
139 }
140 return dist;
141 }
142
143 /* Vincenty's direct solution. The equation numbers related to
144 * the equation numbers in the following:
145 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
146 */
GreatCircleTravel(double Lon1,double Lat1,double Dist,double Bear1,double * Lon2,double * Lat2,double * Bear2)147 void Geodesic::GreatCircleTravel( double Lon1, double Lat1, double Dist, double Bear1, double *Lon2,
148 double *Lat2, double *Bear2 )
149 {
150 /* Geodesic parameters per WGS-84 */
151 double a = GEODESIC_WGS84_SEMI_MAJORAXIS; /* in meters */
152 double b = GEODESIC_WGS84_SEMI_MINORAXIS; /* in meters */
153 double f = ( GEODESIC_WGS84_SEMI_MAJORAXIS - GEODESIC_WGS84_SEMI_MINORAXIS )
154 / GEODESIC_WGS84_SEMI_MAJORAXIS; /* Flattening */
155
156 /* Initialize to where we started from */
157 if( Lon2 ) *Lon2 = Lon1;
158 if( Lat2 ) *Lat2 = Lat1;
159 if( Bear2 ) *Bear2 = Bear1;
160
161 if( Dist < 1e-12 ) {
162 /* There's no distance to travel, so we're done. */
163 return;
164 }
165
166 /* Convert inputs from degrees to radians */
167 Lon1 = GEODESIC_DEG2RAD(Lon1);
168 Lat1 = GEODESIC_DEG2RAD(Lat1);
169 Bear1 = GEODESIC_DEG2RAD(Bear1);
170 if( Lon2 ) *Lon2 = Lon1;
171 if( Lat2 ) *Lat2 = Lat1;
172 if( Bear2 ) *Bear2 = Bear1;
173
174 double sigma1; /* Angular distance on the sphere from equator to Lon1/Lat1 */
175 double rLat1; /* Reduced Latitude */
176 double sinrLat1; /* Sine of reduced latitude */
177 double cosrLat1; /* Cosine of reduced latitude */
178 double sinalpha1; /* Sine of the initial bearing */
179 double cosalpha1; /* Cosine of the initial bearing */
180 double sinalpha; /* Sine of the azimuth of the geodesic at the equator */
181 double sin2alpha; /* sinalpha^2 */
182 double cos2alpha; /* cosalpha^2 */
183 double sigma; /* Angular distance on the sphere */
184 double sinsigma; /* Sine of the angular distance on the sphere */
185 double cossigma; /* Cosine of the angular distance on the sphere */
186 double sigmaprime; /* Previous value of sigma */
187 double lambda; /* Difference in longitude on an auxiliary sphere */
188 double L; /* Difference in longitude, positive East */
189 double u2, A, B, C, twosigmam, costwosigmam, cos2twosigmam, deltasigma, distoverba; /* Intermediate calculations */
190
191 /* Start the algorithm */
192 rLat1 = ( 1.0 - f ) * tan( Lat1 ); /* tan U=(1-f)*tan(phi) */
193 cosrLat1 = 1.0 / sqrt( 1.0 + rLat1 * rLat1 ); /* via trig identity */
194 sinrLat1 = rLat1 * cosrLat1; /* via trig identity */
195 sinalpha1 = sin( Bear1 );
196 cosalpha1 = cos( Bear1 );
197
198 sigma1 = atan2( rLat1, cosalpha1 ); /* Eq. 1 */
199
200 sinalpha = cosrLat1 * sinalpha1; /* Eq. 2 */
201 sin2alpha = sinalpha * sinalpha;
202
203 cos2alpha = 1 - ( sin2alpha ); /* cos2=1-sin2 */
204 u2 = cos2alpha * ( ( a * a ) - ( b * b ) ) / ( b * b );
205 A = 1 + ( u2 / 16384 ) * ( 4096 + u2 * ( -768 + u2 * ( 320 - 175 * u2 ) ) ); /* Eq. 3 */
206
207 B = ( u2 / 1024 ) * ( 256 + u2 * ( -128 + u2 * ( 74 - 47 * u2 ) ) ); /* Eq. 4 */
208
209 distoverba = Dist / ( b * A );
210 sigma = distoverba;
211 sigmaprime = sigma - 1.0; /* Something to get the loop started */
212 while( fabs( sigmaprime - sigma ) > 1e-12 ) {
213 twosigmam = 2 * sigma1 + sigma; /* Eq. 5 */
214 costwosigmam = cos( twosigmam );
215 cos2twosigmam = costwosigmam * costwosigmam;
216 sinsigma = sin( sigma );
217
218 deltasigma = B * sinsigma
219 * ( costwosigmam
220 + ( B / 4.0 )
221 * /* Eq. 6 */
222 ( cos( sigma ) * ( -1 + 2 * cos2twosigmam )
223 - ( B / 6.0 ) * costwosigmam
224 * ( -3 + 4 * sinsigma * sinsigma )
225 * ( -3 + 4 * cos2twosigmam ) ) );
226
227 sigmaprime = sigma;
228 sigma = distoverba + deltasigma; /* Eq. 7 */
229 }
230 sinsigma = sin( sigma );
231 cossigma = cos( sigma );
232 twosigmam = 2 * sigma1 + sigma; /* Eq. 5 */
233 costwosigmam = cos( twosigmam );
234 cos2twosigmam = costwosigmam * costwosigmam;
235
236 if( Lat2 ) {
237 *Lat2 = atan2( /* Eq. 8 */
238 sinrLat1 * cossigma + cosrLat1 * sinsigma * cosalpha1,
239 ( 1 - f )
240 * sqrt(
241 sin2alpha
242 + pow(
243 sinrLat1 * sinsigma
244 - cosrLat1 * cossigma * cosalpha1,
245 2.0 ) ) );
246 *Lat2 = GEODESIC_RAD2DEG(*Lat2);
247 }
248
249 if( Lon2 ) {
250 lambda = atan2( sinsigma * sinalpha1, /* Eq. 9 */
251 cosrLat1 * cossigma - sinrLat1 * sinsigma * cosalpha1 );
252
253 C = ( f / 16.0 ) * cos2alpha * ( 4 + f * ( 4 - 3 * cos2alpha ) ); /* Eq. 10 */
254
255 L = lambda - ( 1 - C ) * f * sinalpha * ( sigma + C * sinsigma * /* Eq. 11 */
256 ( costwosigmam + C * cossigma * ( -1 + 2 * cos2twosigmam ) ) );
257 *Lon2 = GEODESIC_RAD2DEG(Lon1+L);
258 }
259
260 if( Bear2 ) {
261 *Bear2 = atan2( sinalpha, /* Eq. 12 */
262 -sinrLat1 * sinsigma + cosrLat1 * cossigma * cosalpha1 );
263 *Bear2 = GEODESIC_RAD2DEG(*Bear2);
264 }
265
266 return;
267 }
268