boost/geometry/formulas/andoyer_inverse.hpp

   1 // Boost.Geometry
   2
   3 // Copyright (c) 2015-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_ANDOYER_INVERSE_HPP
  12 #define BOOST_GEOMETRY_FORMULAS_ANDOYER_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/formulas/differential_quantities.hpp>
  24 #include <boost/geometry/formulas/flattening.hpp>
  25 #include <boost/geometry/formulas/result_inverse.hpp>
  26
  27
  28 namespace boost { namespace geometry { namespace formula
  29 {
  30
  31 /*!
  32 \brief The solution of the inverse problem of geodesics on latlong coordinates,
  33        Forsyth-Andoyer-Lambert type approximation with first order terms.
  34 \author See
  35     - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
  36       http://www.dtic.mil/docs/citations/AD0627893
  37     - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
  38       http://www.dtic.mil/docs/citations/AD703541
  39 */
  40 template <
  41     typename CT,
  42     bool EnableDistance,
  43     bool EnableAzimuth,
  44     bool EnableReverseAzimuth = false,
  45     bool EnableReducedLength = false,
  46     bool EnableGeodesicScale = false
  47 >
  48 class andoyer_inverse
  49 {
  50     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
  51     static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
  52     static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
  53     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
  54
  55 public:
  56     typedef result_inverse<CT> result_type;
  57
  58     template <typename T1, typename T2, typename Spheroid>
  59     static inline result_type apply(T1 const& lon1,
  60                                     T1 const& lat1,
  61                                     T2 const& lon2,
  62                                     T2 const& lat2,
  63                                     Spheroid const& spheroid)
  64     {
  65         result_type result;
  66
  67         // coordinates in radians
  68
  69         if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
  70         {
  71             return result;
  72         }
  73
  74         CT const c0 = CT(0);
  75         CT const c1 = CT(1);
  76         CT const pi = math::pi<CT>();
  77         CT const f = formula::flattening<CT>(spheroid);
  78
  79         CT const dlon = lon2 - lon1;
  80         CT const sin_dlon = sin(dlon);
  81         CT const cos_dlon = cos(dlon);
  82         CT const sin_lat1 = sin(lat1);
  83         CT const cos_lat1 = cos(lat1);
  84         CT const sin_lat2 = sin(lat2);
  85         CT const cos_lat2 = cos(lat2);
  86
  87         // H,G,T = infinity if cos_d = 1 or cos_d = -1
  88         // lat1 == +-90 && lat2 == +-90
  89         // lat1 == lat2 && lon1 == lon2
  90         CT cos_d = sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon;
  91         // on some platforms cos_d may be outside valid range
  92         if (cos_d < -c1)
  93             cos_d = -c1;
  94         else if (cos_d > c1)
  95             cos_d = c1;
  96
  97         CT const d = acos(cos_d); // [0, pi]
  98         CT const sin_d = sin(d);  // [-1, 1]
  99
 100         if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
 101         {
 102             CT const K = math::sqr(sin_lat1-sin_lat2);
 103             CT const L = math::sqr(sin_lat1+sin_lat2);
 104             CT const three_sin_d = CT(3) * sin_d;
 105
 106             CT const one_minus_cos_d = c1 - cos_d;
 107             CT const one_plus_cos_d = c1 + cos_d;
 108             // cos_d = 1 or cos_d = -1 means that the points are antipodal
 109
 110             CT const H = math::equals(one_minus_cos_d, c0) ?
 111                             c0 :
 112                             (d + three_sin_d) / one_minus_cos_d;
 113             CT const G = math::equals(one_plus_cos_d, c0) ?
 114                             c0 :
 115                             (d - three_sin_d) / one_plus_cos_d;
 116
 117             CT const dd = -(f/CT(4))*(H*K+G*L);
 118
 119             CT const a = get_radius<0>(spheroid);
 120
 121             result.distance = a * (d + dd);
 122         }
 123
 124         if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
 125         {
 126             // sin_d = 0 <=> antipodal points
 127             if (math::equals(sin_d, c0))
 128             {
 129                 // T = inf
 130                 // dA = inf
 131                 // azimuth = -inf
 132                 result.azimuth = lat1 <= lat2 ? c0 : pi;
 133             }
 134             else
 135             {
 136                 CT const c2 = CT(2);
 137
 138                 CT A = c0;
 139                 CT U = c0;
 140                 if (math::equals(cos_lat2, c0))
 141                 {
 142                     if (sin_lat2 < c0)
 143                     {
 144                         A = pi;
 145                     }
 146                 }
 147                 else
 148                 {
 149                     CT const tan_lat2 = sin_lat2/cos_lat2;
 150                     CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon;
 151                     A = atan2(sin_dlon, M);
 152                     CT const sin_2A = sin(c2*A);
 153                     U = (f/ c2)*math::sqr(cos_lat1)*sin_2A;
 154                 }
 155
 156                 CT B = c0;
 157                 CT V = c0;
 158                 if (math::equals(cos_lat1, c0))
 159                 {
 160                     if (sin_lat1 < c0)
 161                     {
 162                         B = pi;
 163                     }
 164                 }
 165                 else
 166                 {
 167                     CT const tan_lat1 = sin_lat1/cos_lat1;
 168                     CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon;
 169                     B = atan2(sin_dlon, N);
 170                     CT const sin_2B = sin(c2*B);
 171                     V = (f/ c2)*math::sqr(cos_lat2)*sin_2B;
 172                 }
 173
 174                 CT const T = d / sin_d;
 175
 176                 // even with sin_d == 0 checked above if the second point
 177                 // is somewhere in the antipodal area T may still be great
 178                 // therefore dA and dB may be great and the resulting azimuths
 179                 // may be some more or less arbitrary angles
 180
 181                 if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
 182                 {
 183                     CT const dA = V*T - U;
 184                     result.azimuth = A - dA;
 185                     normalize_azimuth(result.azimuth, A, dA);
 186                 }
 187
 188                 if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
 189                 {
 190                     CT const dB = -U*T + V;
 191                     result.reverse_azimuth = pi - B - dB;
 192                     if (result.reverse_azimuth > pi)
 193                     {
 194                         result.reverse_azimuth -= 2 * pi;
 195                     }
 196                     normalize_azimuth(result.reverse_azimuth, B, dB);
 197                 }
 198             }
 199         }
 200
 201         if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
 202         {
 203             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 1> quantities;
 204             quantities::apply(dlon, sin_lat1, cos_lat1, sin_lat2, cos_lat2,
 205                               result.azimuth, result.reverse_azimuth,
 206                               get_radius<2>(spheroid), f,
 207                               result.reduced_length, result.geodesic_scale);
 208         }
 209
 210         return result;
 211     }
 212
 213 private:
 214     static inline void normalize_azimuth(CT & azimuth, CT const& A, CT const& dA)
 215     {
 216         CT const c0 = 0;
 217
 218         if (A >= c0) // A indicates Eastern hemisphere
 219         {
 220             if (dA >= c0) // A altered towards 0
 221             {
 222                 if (azimuth < c0)
 223                 {
 224                     azimuth = c0;
 225                 }
 226             }
 227             else // dA < 0, A altered towards pi
 228             {
 229                 CT const pi = math::pi<CT>();
 230                 if (azimuth > pi)
 231                 {
 232                     azimuth = pi;
 233                 }
 234             }
 235         }
 236         else // A indicates Western hemisphere
 237         {
 238             if (dA <= c0) // A altered towards 0
 239             {
 240                 if (azimuth > c0)
 241                 {
 242                     azimuth = c0;
 243                 }
 244             }
 245             else // dA > 0, A altered towards -pi
 246             {
 247                 CT const minus_pi = -math::pi<CT>();
 248                 if (azimuth < minus_pi)
 249                 {
 250                     azimuth = minus_pi;
 251                 }
 252             }
 253         }
 254     }
 255 };
 256
 257 }}} // namespace boost::geometry::formula
 258
 259
 260 #endif // BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP