1 // Boost.Geometry
2 
3 // Copyright (c) 2018 Adam Wulkiewicz, Lodz, Poland.
4 
5 // Copyright (c) 2015-2017 Oracle and/or its affiliates.
6 
7 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
8 
9 // Use, modification and distribution is subject to the Boost Software License,
10 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
11 // http://www.boost.org/LICENSE_1_0.txt)
12 
13 #ifndef BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP
14 #define BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP
15 
16 
17 #include <boost/math/constants/constants.hpp>
18 
19 #include <boost/geometry/core/radius.hpp>
20 
21 #include <boost/geometry/util/condition.hpp>
22 #include <boost/geometry/util/math.hpp>
23 
24 #include <boost/geometry/formulas/differential_quantities.hpp>
25 #include <boost/geometry/formulas/flattening.hpp>
26 #include <boost/geometry/formulas/result_inverse.hpp>
27 
28 
29 namespace boost { namespace geometry { namespace formula
30 {
31 
32 /*!
33 \brief The solution of the inverse problem of geodesics on latlong coordinates,
34        Forsyth-Andoyer-Lambert type approximation with first order terms.
35 \author See
36     - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
37       http://www.dtic.mil/docs/citations/AD0627893
38     - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
39       http://www.dtic.mil/docs/citations/AD703541
40 */
41 template <
42     typename CT,
43     bool EnableDistance,
44     bool EnableAzimuth,
45     bool EnableReverseAzimuth = false,
46     bool EnableReducedLength = false,
47     bool EnableGeodesicScale = false
48 >
49 class andoyer_inverse
50 {
51     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
52     static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
53     static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
54     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
55 
56 public:
57     typedef result_inverse<CT> result_type;
58 
59     template <typename T1, typename T2, typename Spheroid>
apply(T1 const & lon1,T1 const & lat1,T2 const & lon2,T2 const & lat2,Spheroid const & spheroid)60     static inline result_type apply(T1 const& lon1,
61                                     T1 const& lat1,
62                                     T2 const& lon2,
63                                     T2 const& lat2,
64                                     Spheroid const& spheroid)
65     {
66         result_type result;
67 
68         // coordinates in radians
69 
70         if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
71         {
72             return result;
73         }
74 
75         CT const c0 = CT(0);
76         CT const c1 = CT(1);
77         CT const pi = math::pi<CT>();
78         CT const f = formula::flattening<CT>(spheroid);
79 
80         CT const dlon = lon2 - lon1;
81         CT const sin_dlon = sin(dlon);
82         CT const cos_dlon = cos(dlon);
83         CT const sin_lat1 = sin(lat1);
84         CT const cos_lat1 = cos(lat1);
85         CT const sin_lat2 = sin(lat2);
86         CT const cos_lat2 = cos(lat2);
87 
88         // H,G,T = infinity if cos_d = 1 or cos_d = -1
89         // lat1 == +-90 && lat2 == +-90
90         // lat1 == lat2 && lon1 == lon2
91         CT cos_d = sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon;
92         // on some platforms cos_d may be outside valid range
93         if (cos_d < -c1)
94             cos_d = -c1;
95         else if (cos_d > c1)
96             cos_d = c1;
97 
98         CT const d = acos(cos_d); // [0, pi]
99         CT const sin_d = sin(d);  // [-1, 1]
100 
101         if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
102         {
103             CT const K = math::sqr(sin_lat1-sin_lat2);
104             CT const L = math::sqr(sin_lat1+sin_lat2);
105             CT const three_sin_d = CT(3) * sin_d;
106 
107             CT const one_minus_cos_d = c1 - cos_d;
108             CT const one_plus_cos_d = c1 + cos_d;
109             // cos_d = 1 or cos_d = -1 means that the points are antipodal
110 
111             CT const H = math::equals(one_minus_cos_d, c0) ?
112                             c0 :
113                             (d + three_sin_d) / one_minus_cos_d;
114             CT const G = math::equals(one_plus_cos_d, c0) ?
115                             c0 :
116                             (d - three_sin_d) / one_plus_cos_d;
117 
118             CT const dd = -(f/CT(4))*(H*K+G*L);
119 
120             CT const a = CT(get_radius<0>(spheroid));
121 
122             result.distance = a * (d + dd);
123         }
124 
125         if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
126         {
127             // sin_d = 0 <=> antipodal points (incl. poles)
128             if (math::equals(sin_d, c0))
129             {
130                 // T = inf
131                 // dA = inf
132                 // azimuth = -inf
133 
134                 // TODO: The following azimuths are inconsistent with distance
135                 // i.e. according to azimuths below a segment with antipodal endpoints
136                 // travels through the north pole, however the distance returned above
137                 // is the length of a segment traveling along the equator.
138                 // Furthermore, this special case handling is only done in andoyer
139                 // formula.
140                 // The most correct way of fixing it is to handle antipodal regions
141                 // correctly and consistently across all formulas.
142 
143                 // Set azimuth to 0 unless the first endpoint is the north pole
144                 if (! math::equals(sin_lat1, c1))
145                 {
146                     result.azimuth = c0;
147                     result.reverse_azimuth = pi;
148                 }
149                 else
150                 {
151                     result.azimuth = pi;
152                     result.reverse_azimuth = 0;
153                 }
154             }
155             else
156             {
157                 CT const c2 = CT(2);
158 
159                 CT A = c0;
160                 CT U = c0;
161                 if (math::equals(cos_lat2, c0))
162                 {
163                     if (sin_lat2 < c0)
164                     {
165                         A = pi;
166                     }
167                 }
168                 else
169                 {
170                     CT const tan_lat2 = sin_lat2/cos_lat2;
171                     CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon;
172                     A = atan2(sin_dlon, M);
173                     CT const sin_2A = sin(c2*A);
174                     U = (f/ c2)*math::sqr(cos_lat1)*sin_2A;
175                 }
176 
177                 CT B = c0;
178                 CT V = c0;
179                 if (math::equals(cos_lat1, c0))
180                 {
181                     if (sin_lat1 < c0)
182                     {
183                         B = pi;
184                     }
185                 }
186                 else
187                 {
188                     CT const tan_lat1 = sin_lat1/cos_lat1;
189                     CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon;
190                     B = atan2(sin_dlon, N);
191                     CT const sin_2B = sin(c2*B);
192                     V = (f/ c2)*math::sqr(cos_lat2)*sin_2B;
193                 }
194 
195                 CT const T = d / sin_d;
196 
197                 // even with sin_d == 0 checked above if the second point
198                 // is somewhere in the antipodal area T may still be great
199                 // therefore dA and dB may be great and the resulting azimuths
200                 // may be some more or less arbitrary angles
201 
202                 if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
203                 {
204                     CT const dA = V*T - U;
205                     result.azimuth = A - dA;
206                     normalize_azimuth(result.azimuth, A, dA);
207                 }
208 
209                 if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
210                 {
211                     CT const dB = -U*T + V;
212                     if (B >= 0)
213                         result.reverse_azimuth = pi - B - dB;
214                     else
215                         result.reverse_azimuth = -pi - B - dB;
216                     normalize_azimuth(result.reverse_azimuth, B, dB);
217                 }
218             }
219         }
220 
221         if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
222         {
223             CT const b = CT(get_radius<2>(spheroid));
224 
225             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 1> quantities;
226             quantities::apply(dlon, sin_lat1, cos_lat1, sin_lat2, cos_lat2,
227                               result.azimuth, result.reverse_azimuth,
228                               b, f,
229                               result.reduced_length, result.geodesic_scale);
230         }
231 
232         return result;
233     }
234 
235 private:
normalize_azimuth(CT & azimuth,CT const & A,CT const & dA)236     static inline void normalize_azimuth(CT & azimuth, CT const& A, CT const& dA)
237     {
238         CT const c0 = 0;
239 
240         if (A >= c0) // A indicates Eastern hemisphere
241         {
242             if (dA >= c0) // A altered towards 0
243             {
244                 if (azimuth < c0)
245                 {
246                     azimuth = c0;
247                 }
248             }
249             else // dA < 0, A altered towards pi
250             {
251                 CT const pi = math::pi<CT>();
252                 if (azimuth > pi)
253                 {
254                     azimuth = pi;
255                 }
256             }
257         }
258         else // A indicates Western hemisphere
259         {
260             if (dA <= c0) // A altered towards 0
261             {
262                 if (azimuth > c0)
263                 {
264                     azimuth = c0;
265                 }
266             }
267             else // dA > 0, A altered towards -pi
268             {
269                 CT const minus_pi = -math::pi<CT>();
270                 if (azimuth < minus_pi)
271                 {
272                     azimuth = minus_pi;
273                 }
274             }
275         }
276     }
277 };
278 
279 }}} // namespace boost::geometry::formula
280 
281 
282 #endif // BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP
283