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> 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 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 = c0; 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: normalize_azimuth(CT & azimuth,CT const & A,CT const & dA)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 294