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