]>
Commit | Line | Data |
---|---|---|
7c673cae FG |
1 | // Boost.Geometry |
2 | ||
3 | // Copyright (c) 2015 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_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP | |
12 | #define BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_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/algorithms/detail/flattening.hpp> | |
24 | #include <boost/geometry/algorithms/detail/result_inverse.hpp> | |
25 | ||
26 | namespace boost { namespace geometry { namespace detail | |
27 | { | |
28 | ||
29 | /*! | |
30 | \brief The solution of the inverse problem of geodesics on latlong coordinates, | |
31 | Forsyth-Andoyer-Lambert type approximation with second order terms. | |
32 | \author See | |
33 | - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965 | |
34 | http://www.dtic.mil/docs/citations/AD0627893 | |
35 | - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970 | |
36 | http://www.dtic.mil/docs/citations/AD703541 | |
37 | */ | |
38 | template <typename CT, bool EnableDistance, bool EnableAzimuth> | |
39 | struct thomas_inverse | |
40 | { | |
41 | typedef result_inverse<CT> result_type; | |
42 | ||
43 | template <typename T1, typename T2, typename Spheroid> | |
44 | static inline result_type apply(T1 const& lon1, | |
45 | T1 const& lat1, | |
46 | T2 const& lon2, | |
47 | T2 const& lat2, | |
48 | Spheroid const& spheroid) | |
49 | { | |
50 | result_type result; | |
51 | ||
52 | // coordinates in radians | |
53 | ||
54 | if ( math::equals(lon1, lon2) | |
55 | && math::equals(lat1, lat2) ) | |
56 | { | |
57 | result.set(CT(0), CT(0)); | |
58 | return result; | |
59 | } | |
60 | ||
61 | CT const f = detail::flattening<CT>(spheroid); | |
62 | CT const one_minus_f = CT(1) - f; | |
63 | ||
64 | // CT const tan_theta1 = one_minus_f * tan(lat1); | |
65 | // CT const tan_theta2 = one_minus_f * tan(lat2); | |
66 | // CT const theta1 = atan(tan_theta1); | |
67 | // CT const theta2 = atan(tan_theta2); | |
68 | ||
69 | CT const pi_half = math::pi<CT>() / CT(2); | |
70 | CT const theta1 = math::equals(lat1, pi_half) ? lat1 : | |
71 | math::equals(lat1, -pi_half) ? lat1 : | |
72 | atan(one_minus_f * tan(lat1)); | |
73 | CT const theta2 = math::equals(lat2, pi_half) ? lat2 : | |
74 | math::equals(lat2, -pi_half) ? lat2 : | |
75 | atan(one_minus_f * tan(lat2)); | |
76 | ||
77 | CT const theta_m = (theta1 + theta2) / CT(2); | |
78 | CT const d_theta_m = (theta2 - theta1) / CT(2); | |
79 | CT const d_lambda = lon2 - lon1; | |
80 | CT const d_lambda_m = d_lambda / CT(2); | |
81 | ||
82 | CT const sin_theta_m = sin(theta_m); | |
83 | CT const cos_theta_m = cos(theta_m); | |
84 | CT const sin_d_theta_m = sin(d_theta_m); | |
85 | CT const cos_d_theta_m = cos(d_theta_m); | |
86 | CT const sin2_theta_m = math::sqr(sin_theta_m); | |
87 | CT const cos2_theta_m = math::sqr(cos_theta_m); | |
88 | CT const sin2_d_theta_m = math::sqr(sin_d_theta_m); | |
89 | CT const cos2_d_theta_m = math::sqr(cos_d_theta_m); | |
90 | CT const sin_d_lambda_m = sin(d_lambda_m); | |
91 | CT const sin2_d_lambda_m = math::sqr(sin_d_lambda_m); | |
92 | ||
93 | CT const H = cos2_theta_m - sin2_d_theta_m; | |
94 | CT const L = sin2_d_theta_m + H * sin2_d_lambda_m; | |
95 | CT const cos_d = CT(1) - CT(2) * L; | |
96 | CT const d = acos(cos_d); | |
97 | CT const sin_d = sin(d); | |
98 | ||
99 | CT const one_minus_L = CT(1) - L; | |
100 | ||
101 | if ( math::equals(sin_d, CT(0)) | |
102 | || math::equals(L, CT(0)) | |
103 | || math::equals(one_minus_L, CT(0)) ) | |
104 | { | |
105 | result.set(CT(0), CT(0)); | |
106 | return result; | |
107 | } | |
108 | ||
109 | CT const U = CT(2) * sin2_theta_m * cos2_d_theta_m / one_minus_L; | |
110 | CT const V = CT(2) * sin2_d_theta_m * cos2_theta_m / L; | |
111 | CT const X = U + V; | |
112 | CT const Y = U - V; | |
113 | CT const T = d / sin_d; | |
114 | //CT const D = CT(4) * math::sqr(T); | |
115 | //CT const E = CT(2) * cos_d; | |
116 | //CT const A = D * E; | |
117 | //CT const B = CT(2) * D; | |
118 | //CT const C = T - (A - E) / CT(2); | |
119 | ||
120 | if ( BOOST_GEOMETRY_CONDITION(EnableDistance) ) | |
121 | { | |
122 | //CT const n1 = X * (A + C*X); | |
123 | //CT const n2 = Y * (B + E*Y); | |
124 | //CT const n3 = D*X*Y; | |
125 | ||
126 | //CT const f_sqr = math::sqr(f); | |
127 | //CT const f_sqr_per_64 = f_sqr / CT(64); | |
128 | ||
129 | CT const delta1d = f * (T*X-Y) / CT(4); | |
130 | //CT const delta2d = f_sqr_per_64 * (n1 - n2 + n3); | |
131 | ||
132 | CT const a = get_radius<0>(spheroid); | |
133 | ||
134 | result.distance = a * sin_d * (T - delta1d); | |
135 | //double S2 = a * sin_d * (T - delta1d + delta2d); | |
136 | } | |
137 | else | |
138 | { | |
139 | result.distance = CT(0); | |
140 | } | |
141 | ||
142 | if ( BOOST_GEOMETRY_CONDITION(EnableAzimuth) ) | |
143 | { | |
144 | // NOTE: if both cos_latX == 0 then below we'd have 0 * INF | |
145 | // it's a situation when the endpoints are on the poles +-90 deg | |
146 | // in this case the azimuth could either be 0 or +-pi | |
147 | // but above always 0 is returned | |
148 | ||
149 | // may also be used to calculate distance21 | |
150 | //CT const D = CT(4) * math::sqr(T); | |
151 | CT const E = CT(2) * cos_d; | |
152 | //CT const A = D * E; | |
153 | //CT const B = CT(2) * D; | |
154 | // may also be used to calculate distance21 | |
155 | CT const f_sqr = math::sqr(f); | |
156 | CT const f_sqr_per_64 = f_sqr / CT(64); | |
157 | ||
158 | CT const F = CT(2)*Y-E*(CT(4)-X); | |
159 | //CT const M = CT(32)*T-(CT(20)*T-A)*X-(B+CT(4))*Y; | |
160 | CT const G = f*T/CT(2) + f_sqr_per_64; | |
161 | CT const tan_d_lambda = tan(d_lambda); | |
162 | CT const Q = -(F*G*tan_d_lambda) / CT(4); | |
163 | ||
164 | CT const d_lambda_p = (d_lambda + Q) / CT(2); | |
165 | CT const tan_d_lambda_p = tan(d_lambda_p); | |
166 | ||
167 | CT const v = atan2(cos_d_theta_m, sin_theta_m * tan_d_lambda_p); | |
168 | CT const u = atan2(-sin_d_theta_m, cos_theta_m * tan_d_lambda_p); | |
169 | ||
170 | CT const pi = math::pi<CT>(); | |
171 | CT alpha1 = v + u; | |
172 | if ( alpha1 > pi ) | |
173 | { | |
174 | alpha1 -= CT(2) * pi; | |
175 | } | |
176 | ||
177 | result.azimuth = alpha1; | |
178 | } | |
179 | else | |
180 | { | |
181 | result.azimuth = CT(0); | |
182 | } | |
183 | ||
184 | return result; | |
185 | } | |
186 | }; | |
187 | ||
188 | }}} // namespace boost::geometry::detail | |
189 | ||
190 | ||
191 | #endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP |