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