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