1 // Boost.Geometry
2 
3 // Copyright (c) 2016-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_INVERSE_DIFFERENTIAL_QUANTITIES_HPP
12 #define BOOST_GEOMETRY_FORMULAS_INVERSE_DIFFERENTIAL_QUANTITIES_HPP
13 
14 
15 #include <boost/geometry/util/condition.hpp>
16 #include <boost/geometry/util/math.hpp>
17 
18 
19 namespace boost { namespace geometry { namespace formula
20 {
21 
22 /*!
23 \brief The solution of a part of the inverse problem - differential quantities.
24 \author See
25 - Charles F.F Karney, Algorithms for geodesics, 2011
26 https://arxiv.org/pdf/1109.4448.pdf
27 */
28 template <
29     typename CT,
30     bool EnableReducedLength,
31     bool EnableGeodesicScale,
32     unsigned int Order = 2,
33     bool ApproxF = true
34 >
35 class differential_quantities
36 {
37 public:
apply(CT const & lon1,CT const & lat1,CT const & lon2,CT const & lat2,CT const & azimuth,CT const & reverse_azimuth,CT const & b,CT const & f,CT & reduced_length,CT & geodesic_scale)38     static inline void apply(CT const& lon1, CT const& lat1,
39                              CT const& lon2, CT const& lat2,
40                              CT const& azimuth, CT const& reverse_azimuth,
41                              CT const& b, CT const& f,
42                              CT & reduced_length, CT & geodesic_scale)
43     {
44         CT const dlon = lon2 - lon1;
45         CT const sin_lat1 = sin(lat1);
46         CT const cos_lat1 = cos(lat1);
47         CT const sin_lat2 = sin(lat2);
48         CT const cos_lat2 = cos(lat2);
49 
50         apply(dlon, sin_lat1, cos_lat1, sin_lat2, cos_lat2,
51               azimuth, reverse_azimuth,
52               b, f,
53               reduced_length, geodesic_scale);
54     }
55 
apply(CT const & dlon,CT const & sin_lat1,CT const & cos_lat1,CT const & sin_lat2,CT const & cos_lat2,CT const & azimuth,CT const & reverse_azimuth,CT const & b,CT const & f,CT & reduced_length,CT & geodesic_scale)56     static inline void apply(CT const& dlon,
57                              CT const& sin_lat1, CT const& cos_lat1,
58                              CT const& sin_lat2, CT const& cos_lat2,
59                              CT const& azimuth, CT const& reverse_azimuth,
60                              CT const& b, CT const& f,
61                              CT & reduced_length, CT & geodesic_scale)
62     {
63         CT const c0 = 0;
64         CT const c1 = 1;
65         CT const one_minus_f = c1 - f;
66 
67         CT sin_bet1 = one_minus_f * sin_lat1;
68         CT sin_bet2 = one_minus_f * sin_lat2;
69 
70         // equator
71         if (math::equals(sin_bet1, c0) && math::equals(sin_bet2, c0))
72         {
73             CT const sig_12 = math::abs(dlon) / one_minus_f;
74             if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
75             {
76                 CT m12 = sin(sig_12) * b;
77                 reduced_length = m12;
78             }
79 
80             if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
81             {
82                 CT M12 = cos(sig_12);
83                 geodesic_scale = M12;
84             }
85         }
86         else
87         {
88             CT const c2 = 2;
89             CT const e2 = f * (c2 - f);
90             CT const ep2 = e2 / math::sqr(one_minus_f);
91 
92             CT const sin_alp1 = sin(azimuth);
93             CT const cos_alp1 = cos(azimuth);
94             //CT const sin_alp2 = sin(reverse_azimuth);
95             CT const cos_alp2 = cos(reverse_azimuth);
96 
97             CT cos_bet1 = cos_lat1;
98             CT cos_bet2 = cos_lat2;
99 
100             normalize(sin_bet1, cos_bet1);
101             normalize(sin_bet2, cos_bet2);
102 
103             CT sin_sig1 = sin_bet1;
104             CT cos_sig1 = cos_alp1 * cos_bet1;
105             CT sin_sig2 = sin_bet2;
106             CT cos_sig2 = cos_alp2 * cos_bet2;
107 
108             normalize(sin_sig1, cos_sig1);
109             normalize(sin_sig2, cos_sig2);
110 
111             CT const sin_alp0 = sin_alp1 * cos_bet1;
112             CT const cos_alp0_sqr = c1 - math::sqr(sin_alp0);
113 
114             CT const J12 = BOOST_GEOMETRY_CONDITION(ApproxF) ?
115                            J12_f(sin_sig1, cos_sig1, sin_sig2, cos_sig2, cos_alp0_sqr, f) :
116                            J12_ep_sqr(sin_sig1, cos_sig1, sin_sig2, cos_sig2, cos_alp0_sqr, ep2) ;
117 
118             CT const dn1 = math::sqrt(c1 + ep2 * math::sqr(sin_bet1));
119             CT const dn2 = math::sqrt(c1 + ep2 * math::sqr(sin_bet2));
120 
121             if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
122             {
123                 CT const m12_b = dn2 * (cos_sig1 * sin_sig2)
124                                - dn1 * (sin_sig1 * cos_sig2)
125                                - cos_sig1 * cos_sig2 * J12;
126                 CT const m12 = m12_b * b;
127 
128                 reduced_length = m12;
129             }
130 
131             if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
132             {
133                 CT const cos_sig12 = cos_sig1 * cos_sig2 + sin_sig1 * sin_sig2;
134                 CT const t = ep2 * (cos_bet1 - cos_bet2) * (cos_bet1 + cos_bet2) / (dn1 + dn2);
135                 CT const M12 = cos_sig12 + (t * sin_sig2 - cos_sig2 * J12) * sin_sig1 / dn1;
136 
137                 geodesic_scale = M12;
138             }
139         }
140     }
141 
142 private:
143     /*! Approximation of J12, expanded into taylor series in f
144         Maxima script:
145         ep2: f * (2-f) / ((1-f)^2);
146         k2: ca02 * ep2;
147         assume(f < 1);
148         assume(sig > 0);
149         I1(sig):= integrate(sqrt(1 + k2 * sin(s)^2), s, 0, sig);
150         I2(sig):= integrate(1/sqrt(1 + k2 * sin(s)^2), s, 0, sig);
151         J(sig):= I1(sig) - I2(sig);
152         S: taylor(J(sig), f, 0, 3);
153         S1: factor( 2*integrate(sin(s)^2,s,0,sig)*ca02*f );
154         S2: factor( ((integrate(-6*ca02^2*sin(s)^4+6*ca02*sin(s)^2,s,0,sig)+integrate(-2*ca02^2*sin(s)^4+6*ca02*sin(s)^2,s,0,sig))*f^2)/4 );
155         S3: factor( ((integrate(30*ca02^3*sin(s)^6-54*ca02^2*sin(s)^4+24*ca02*sin(s)^2,s,0,sig)+integrate(6*ca02^3*sin(s)^6-18*ca02^2*sin(s)^4+24*ca02*sin(s)^2,s,0,sig))*f^3)/12 );
156     */
J12_f(CT const & sin_sig1,CT const & cos_sig1,CT const & sin_sig2,CT const & cos_sig2,CT const & cos_alp0_sqr,CT const & f)157     static inline CT J12_f(CT const& sin_sig1, CT const& cos_sig1,
158                            CT const& sin_sig2, CT const& cos_sig2,
159                            CT const& cos_alp0_sqr, CT const& f)
160     {
161         if (Order == 0)
162         {
163             return 0;
164         }
165 
166         CT const c2 = 2;
167 
168         CT const sig_12 = atan2(cos_sig1 * sin_sig2 - sin_sig1 * cos_sig2,
169                                 cos_sig1 * cos_sig2 + sin_sig1 * sin_sig2);
170         CT const sin_2sig1 = c2 * cos_sig1 * sin_sig1; // sin(2sig1)
171         CT const sin_2sig2 = c2 * cos_sig2 * sin_sig2; // sin(2sig2)
172         CT const sin_2sig_12 = sin_2sig2 - sin_2sig1;
173         CT const L1 = sig_12 - sin_2sig_12 / c2;
174 
175         if (Order == 1)
176         {
177             return cos_alp0_sqr * f * L1;
178         }
179 
180         CT const sin_4sig1 = c2 * sin_2sig1 * (math::sqr(cos_sig1) - math::sqr(sin_sig1)); // sin(4sig1)
181         CT const sin_4sig2 = c2 * sin_2sig2 * (math::sqr(cos_sig2) - math::sqr(sin_sig2)); // sin(4sig2)
182         CT const sin_4sig_12 = sin_4sig2 - sin_4sig1;
183 
184         CT const c8 = 8;
185         CT const c12 = 12;
186         CT const c16 = 16;
187         CT const c24 = 24;
188 
189         CT const L2 = -( cos_alp0_sqr * sin_4sig_12
190                          + (-c8 * cos_alp0_sqr + c12) * sin_2sig_12
191                          + (c12 * cos_alp0_sqr - c24) * sig_12)
192                        / c16;
193 
194         if (Order == 2)
195         {
196             return cos_alp0_sqr * f * (L1 + f * L2);
197         }
198 
199         CT const c4 = 4;
200         CT const c9 = 9;
201         CT const c48 = 48;
202         CT const c60 = 60;
203         CT const c64 = 64;
204         CT const c96 = 96;
205         CT const c128 = 128;
206         CT const c144 = 144;
207 
208         CT const cos_alp0_quad = math::sqr(cos_alp0_sqr);
209         CT const sin3_2sig1 = math::sqr(sin_2sig1) * sin_2sig1;
210         CT const sin3_2sig2 = math::sqr(sin_2sig2) * sin_2sig2;
211         CT const sin3_2sig_12 = sin3_2sig2 - sin3_2sig1;
212 
213         CT const A = (c9 * cos_alp0_quad - c12 * cos_alp0_sqr) * sin_4sig_12;
214         CT const B = c4 * cos_alp0_quad * sin3_2sig_12;
215         CT const C = (-c48 * cos_alp0_quad + c96 * cos_alp0_sqr - c64) * sin_2sig_12;
216         CT const D = (c60 * cos_alp0_quad - c144 * cos_alp0_sqr + c128) * sig_12;
217 
218         CT const L3 = (A + B + C + D) / c64;
219 
220         // Order 3 and higher
221         return cos_alp0_sqr * f * (L1 + f * (L2 + f * L3));
222     }
223 
224     /*! Approximation of J12, expanded into taylor series in e'^2
225         Maxima script:
226         k2: ca02 * ep2;
227         assume(sig > 0);
228         I1(sig):= integrate(sqrt(1 + k2 * sin(s)^2), s, 0, sig);
229         I2(sig):= integrate(1/sqrt(1 + k2 * sin(s)^2), s, 0, sig);
230         J(sig):= I1(sig) - I2(sig);
231         S: taylor(J(sig), ep2, 0, 3);
232         S1: factor( integrate(sin(s)^2,s,0,sig)*ca02*ep2 );
233         S2: factor( (integrate(sin(s)^4,s,0,sig)*ca02^2*ep2^2)/2 );
234         S3: factor( (3*integrate(sin(s)^6,s,0,sig)*ca02^3*ep2^3)/8 );
235     */
J12_ep_sqr(CT const & sin_sig1,CT const & cos_sig1,CT const & sin_sig2,CT const & cos_sig2,CT const & cos_alp0_sqr,CT const & ep_sqr)236     static inline CT J12_ep_sqr(CT const& sin_sig1, CT const& cos_sig1,
237                                 CT const& sin_sig2, CT const& cos_sig2,
238                                 CT const& cos_alp0_sqr, CT const& ep_sqr)
239     {
240         if (Order == 0)
241         {
242             return 0;
243         }
244 
245         CT const c2 = 2;
246         CT const c4 = 4;
247 
248         CT const c2a0ep2 = cos_alp0_sqr * ep_sqr;
249 
250         CT const sig_12 = atan2(cos_sig1 * sin_sig2 - sin_sig1 * cos_sig2,
251                                 cos_sig1 * cos_sig2 + sin_sig1 * sin_sig2); // sig2 - sig1
252         CT const sin_2sig1 = c2 * cos_sig1 * sin_sig1; // sin(2sig1)
253         CT const sin_2sig2 = c2 * cos_sig2 * sin_sig2; // sin(2sig2)
254         CT const sin_2sig_12 = sin_2sig2 - sin_2sig1;
255 
256         CT const L1 = (c2 * sig_12 - sin_2sig_12) / c4;
257 
258         if (Order == 1)
259         {
260             return c2a0ep2 * L1;
261         }
262 
263         CT const c8 = 8;
264         CT const c64 = 64;
265 
266         CT const sin_4sig1 = c2 * sin_2sig1 * (math::sqr(cos_sig1) - math::sqr(sin_sig1)); // sin(4sig1)
267         CT const sin_4sig2 = c2 * sin_2sig2 * (math::sqr(cos_sig2) - math::sqr(sin_sig2)); // sin(4sig2)
268         CT const sin_4sig_12 = sin_4sig2 - sin_4sig1;
269 
270         CT const L2 = (sin_4sig_12 - c8 * sin_2sig_12 + 12 * sig_12) / c64;
271 
272         if (Order == 2)
273         {
274             return c2a0ep2 * (L1 + c2a0ep2 * L2);
275         }
276 
277         CT const sin3_2sig1 = math::sqr(sin_2sig1) * sin_2sig1;
278         CT const sin3_2sig2 = math::sqr(sin_2sig2) * sin_2sig2;
279         CT const sin3_2sig_12 = sin3_2sig2 - sin3_2sig1;
280 
281         CT const c9 = 9;
282         CT const c48 = 48;
283         CT const c60 = 60;
284         CT const c512 = 512;
285 
286         CT const L3 = (c9 * sin_4sig_12 + c4 * sin3_2sig_12 - c48 * sin_2sig_12 + c60 * sig_12) / c512;
287 
288         // Order 3 and higher
289         return c2a0ep2 * (L1 + c2a0ep2 * (L2 + c2a0ep2 * L3));
290     }
291 
normalize(CT & x,CT & y)292     static inline void normalize(CT & x, CT & y)
293     {
294         CT const len = math::sqrt(math::sqr(x) + math::sqr(y));
295         x /= len;
296         y /= len;
297     }
298 };
299 
300 }}} // namespace boost::geometry::formula
301 
302 
303 #endif // BOOST_GEOMETRY_FORMULAS_INVERSE_DIFFERENTIAL_QUANTITIES_HPP
304