|
| 1 | +// Boost.Geometry |
| 2 | + |
| 3 | +// Copyright (c) 2018 Adam Wulkiewicz, Lodz, Poland. |
| 4 | + |
| 5 | +// Copyright (c) 2015-2020 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> |
| 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 means that the points are very close |
| 110 | + // cos_d = -1 means that the points are antipodal |
| 111 | + |
| 112 | + CT const H = math::equals(one_minus_cos_d, c0) ? |
| 113 | + c0 : |
| 114 | + (d + three_sin_d) / one_minus_cos_d; |
| 115 | + CT const G = math::equals(one_plus_cos_d, c0) ? |
| 116 | + c0 : |
| 117 | + (d - three_sin_d) / one_plus_cos_d; |
| 118 | + |
| 119 | + CT const dd = -(f/CT(4))*(H*K+G*L); |
| 120 | + |
| 121 | + CT const a = CT(get_radius<0>(spheroid)); |
| 122 | + |
| 123 | + result.distance = a * (d + dd); |
| 124 | + } |
| 125 | + |
| 126 | + if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) ) |
| 127 | + { |
| 128 | + // sin_d = 0 <=> antipodal points (incl. poles) or very close |
| 129 | + if (math::equals(sin_d, c0)) |
| 130 | + { |
| 131 | + // T = inf |
| 132 | + // dA = inf |
| 133 | + // azimuth = -inf |
| 134 | + |
| 135 | + // TODO: The following azimuths are inconsistent with distance |
| 136 | + // i.e. according to azimuths below a segment with antipodal endpoints |
| 137 | + // travels through the north pole, however the distance returned above |
| 138 | + // is the length of a segment traveling along the equator. |
| 139 | + // Furthermore, this special case handling is only done in andoyer |
| 140 | + // formula. |
| 141 | + // The most correct way of fixing it is to handle antipodal regions |
| 142 | + // correctly and consistently across all formulas. |
| 143 | + |
| 144 | + // points very close |
| 145 | + if (cos_d >= c0) |
| 146 | + { |
| 147 | + result.azimuth = c0; |
| 148 | + result.reverse_azimuth = c0; |
| 149 | + } |
| 150 | + // antipodal points |
| 151 | + else |
| 152 | + { |
| 153 | + // Set azimuth to 0 unless the first endpoint is the north pole |
| 154 | + if (! math::equals(sin_lat1, c1)) |
| 155 | + { |
| 156 | + result.azimuth = c0; |
| 157 | + result.reverse_azimuth = pi; |
| 158 | + } |
| 159 | + else |
| 160 | + { |
| 161 | + result.azimuth = pi; |
| 162 | + result.reverse_azimuth = 0; |
| 163 | + } |
| 164 | + } |
| 165 | + } |
| 166 | + else |
| 167 | + { |
| 168 | + CT const c2 = CT(2); |
| 169 | + |
| 170 | + CT A = c0; |
| 171 | + CT U = c0; |
| 172 | + if (math::equals(cos_lat2, c0)) |
| 173 | + { |
| 174 | + if (sin_lat2 < c0) |
| 175 | + { |
| 176 | + A = pi; |
| 177 | + } |
| 178 | + } |
| 179 | + else |
| 180 | + { |
| 181 | + CT const tan_lat2 = sin_lat2/cos_lat2; |
| 182 | + CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon; |
| 183 | + A = atan2(sin_dlon, M); |
| 184 | + CT const sin_2A = sin(c2*A); |
| 185 | + U = (f/ c2)*math::sqr(cos_lat1)*sin_2A; |
| 186 | + } |
| 187 | + |
| 188 | + CT B = c0; |
| 189 | + CT V = c0; |
| 190 | + if (math::equals(cos_lat1, c0)) |
| 191 | + { |
| 192 | + if (sin_lat1 < c0) |
| 193 | + { |
| 194 | + B = pi; |
| 195 | + } |
| 196 | + } |
| 197 | + else |
| 198 | + { |
| 199 | + CT const tan_lat1 = sin_lat1/cos_lat1; |
| 200 | + CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon; |
| 201 | + B = atan2(sin_dlon, N); |
| 202 | + CT const sin_2B = sin(c2*B); |
| 203 | + V = (f/ c2)*math::sqr(cos_lat2)*sin_2B; |
| 204 | + } |
| 205 | + |
| 206 | + CT const T = d / sin_d; |
| 207 | + |
| 208 | + // even with sin_d == 0 checked above if the second point |
| 209 | + // is somewhere in the antipodal area T may still be great |
| 210 | + // therefore dA and dB may be great and the resulting azimuths |
| 211 | + // may be some more or less arbitrary angles |
| 212 | + |
| 213 | + if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth)) |
| 214 | + { |
| 215 | + CT const dA = V*T - U; |
| 216 | + result.azimuth = A - dA; |
| 217 | + normalize_azimuth(result.azimuth, A, dA); |
| 218 | + } |
| 219 | + |
| 220 | + if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth)) |
| 221 | + { |
| 222 | + CT const dB = -U*T + V; |
| 223 | + if (B >= 0) |
| 224 | + result.reverse_azimuth = pi - B - dB; |
| 225 | + else |
| 226 | + result.reverse_azimuth = -pi - B - dB; |
| 227 | + normalize_azimuth(result.reverse_azimuth, B, dB); |
| 228 | + } |
| 229 | + } |
| 230 | + } |
| 231 | + |
| 232 | + if (BOOST_GEOMETRY_CONDITION(CalcQuantities)) |
| 233 | + { |
| 234 | + CT const b = CT(get_radius<2>(spheroid)); |
| 235 | + |
| 236 | + typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 1> quantities; |
| 237 | + quantities::apply(dlon, sin_lat1, cos_lat1, sin_lat2, cos_lat2, |
| 238 | + result.azimuth, result.reverse_azimuth, |
| 239 | + b, f, |
| 240 | + result.reduced_length, result.geodesic_scale); |
| 241 | + } |
| 242 | + |
| 243 | + return result; |
| 244 | + } |
| 245 | + |
| 246 | +private: |
| 247 | + static inline void normalize_azimuth(CT & azimuth, CT const& A, CT const& dA) |
| 248 | + { |
| 249 | + CT const c0 = 0; |
| 250 | + |
| 251 | + if (A >= c0) // A indicates Eastern hemisphere |
| 252 | + { |
| 253 | + if (dA >= c0) // A altered towards 0 |
| 254 | + { |
| 255 | + if (azimuth < c0) |
| 256 | + { |
| 257 | + azimuth = c0; |
| 258 | + } |
| 259 | + } |
| 260 | + else // dA < 0, A altered towards pi |
| 261 | + { |
| 262 | + CT const pi = math::pi<CT>(); |
| 263 | + if (azimuth > pi) |
| 264 | + { |
| 265 | + azimuth = pi; |
| 266 | + } |
| 267 | + } |
| 268 | + } |
| 269 | + else // A indicates Western hemisphere |
| 270 | + { |
| 271 | + if (dA <= c0) // A altered towards 0 |
| 272 | + { |
| 273 | + if (azimuth > c0) |
| 274 | + { |
| 275 | + azimuth = c0; |
| 276 | + } |
| 277 | + } |
| 278 | + else // dA > 0, A altered towards -pi |
| 279 | + { |
| 280 | + CT const minus_pi = -math::pi<CT>(); |
| 281 | + if (azimuth < minus_pi) |
| 282 | + { |
| 283 | + azimuth = minus_pi; |
| 284 | + } |
| 285 | + } |
| 286 | + } |
| 287 | + } |
| 288 | +}; |
| 289 | + |
| 290 | +}}} // namespace boost::geometry::formula |
| 291 | + |
| 292 | + |
| 293 | +#endif // BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP |
0 commit comments