ceph/src/boost/libs/geometry/include/boost/geometry/algorithms/detail/vincenty_inverse.hpp

   1 // Boost.Geometry
   2
   3 // Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
   4
   5 // This file was modified by Oracle on 2014.
   6 // Modifications copyright (c) 2014 Oracle and/or its affiliates.
   7
   8 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
   9
  10 // Use, modification and distribution is subject to the Boost Software License,
  11 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
  12 // http://www.boost.org/LICENSE_1_0.txt)
  13
  14 #ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_INVERSE_HPP
  15 #define BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_INVERSE_HPP
  16
  17
  18 #include <boost/math/constants/constants.hpp>
  19
  20 #include <boost/geometry/core/radius.hpp>
  21 #include <boost/geometry/core/srs.hpp>
  22
  23 #include <boost/geometry/util/condition.hpp>
  24 #include <boost/geometry/util/math.hpp>
  25
  26 #include <boost/geometry/algorithms/detail/flattening.hpp>
  27 #include <boost/geometry/algorithms/detail/result_inverse.hpp>
  28
  29
  30 #ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
  31 #define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
  32 #endif
  33
  34
  35 namespace boost { namespace geometry { namespace detail
  36 {
  37
  38 /*!
  39 \brief The solution of the inverse problem of geodesics on latlong coordinates, after Vincenty, 1975
  40 \author See
  41     - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
  42     - http://www.icsm.gov.au/gda/gdav2.3.pdf
  43 \author Adapted from various implementations to get it close to the original document
  44     - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
  45     - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
  46     - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink
  47
  48 */
  49 template <typename CT, bool EnableDistance, bool EnableAzimuth>
  50 struct vincenty_inverse
  51 {
  52     typedef result_inverse<CT> result_type;
  53
  54 public:
  55     template <typename T1, typename T2, typename Spheroid>
  56     static inline result_type apply(T1 const& lon1,
  57                                     T1 const& lat1,
  58                                     T2 const& lon2,
  59                                     T2 const& lat2,
  60                                     Spheroid const& spheroid)
  61     {
  62         result_type result;
  63
  64         if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
  65         {
  66             result.set(CT(0), CT(0));
  67             return result;
  68         }
  69
  70         CT const c1 = 1;
  71         CT const c2 = 2;
  72         CT const c3 = 3;
  73         CT const c4 = 4;
  74         CT const c16 = 16;
  75         CT const c_e_12 = CT(1e-12);
  76
  77         CT const pi = geometry::math::pi<CT>();
  78         CT const two_pi = c2 * pi;
  79
  80         // lambda: difference in longitude on an auxiliary sphere
  81         CT L = lon2 - lon1;
  82         CT lambda = L;
  83
  84         if (L < -pi) L += two_pi;
  85         if (L > pi) L -= two_pi;
  86
  87         CT const radius_a = CT(get_radius<0>(spheroid));
  88         CT const radius_b = CT(get_radius<2>(spheroid));
  89         CT const flattening = geometry::detail::flattening<CT>(spheroid);
  90
  91         // U: reduced latitude, defined by tan U = (1-f) tan phi
  92         CT const one_min_f = c1 - flattening;
  93         CT const tan_U1 = one_min_f * tan(lat1); // above (1)
  94         CT const tan_U2 = one_min_f * tan(lat2); // above (1)
  95
  96         // calculate sin U and cos U using trigonometric identities
  97         CT const temp_den_U1 = math::sqrt(c1 + math::sqr(tan_U1));
  98         CT const temp_den_U2 = math::sqrt(c1 + math::sqr(tan_U2));
  99         // cos = 1 / sqrt(1 + tan^2)
 100         CT const cos_U1 = c1 / temp_den_U1;
 101         CT const cos_U2 = c1 / temp_den_U2;
 102         // sin = tan / sqrt(1 + tan^2)
 103         CT const sin_U1 = tan_U1 / temp_den_U1;
 104         CT const sin_U2 = tan_U2 / temp_den_U2;
 105
 106         // calculate sin U and cos U directly
 107         //CT const U1 = atan(tan_U1);
 108         //CT const U2 = atan(tan_U2);
 109         //cos_U1 = cos(U1);
 110         //cos_U2 = cos(U2);
 111         //sin_U1 = tan_U1 * cos_U1; // sin(U1);
 112         //sin_U2 = tan_U2 * cos_U2; // sin(U2);
 113
 114         CT previous_lambda;
 115         CT sin_lambda;
 116         CT cos_lambda;
 117         CT sin_sigma;
 118         CT sin_alpha;
 119         CT cos2_alpha;
 120         CT cos2_sigma_m;
 121         CT sigma;
 122
 123         int counter = 0; // robustness
 124
 125         do
 126         {
 127             previous_lambda = lambda; // (13)
 128             sin_lambda = sin(lambda);
 129             cos_lambda = cos(lambda);
 130             sin_sigma = math::sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
 131             CT cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
 132             sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
 133             cos2_alpha = c1 - math::sqr(sin_alpha);
 134             cos2_sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)
 135
 136             CT C = flattening/c16 * cos2_alpha * (c4 + flattening * (c4 - c3 * cos2_alpha)); // (10)
 137             sigma = atan2(sin_sigma, cos_sigma); // (16)
 138             lambda = L + (c1 - C) * flattening * sin_alpha *
 139                 (sigma + C * sin_sigma * ( cos2_sigma_m + C * cos_sigma * (-c1 + c2 * math::sqr(cos2_sigma_m)))); // (11)
 140
 141             ++counter; // robustness
 142
 143         } while ( geometry::math::abs(previous_lambda - lambda) > c_e_12
 144                && geometry::math::abs(lambda) < pi
 145                && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness
 146
 147         if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
 148         {
 149             // Oops getting hard here
 150             // (again, problem is that ttmath cannot divide by doubles, which is OK)
 151             CT const c1 = 1;
 152             CT const c2 = 2;
 153             CT const c3 = 3;
 154             CT const c4 = 4;
 155             CT const c6 = 6;
 156             CT const c47 = 47;
 157             CT const c74 = 74;
 158             CT const c128 = 128;
 159             CT const c256 = 256;
 160             CT const c175 = 175;
 161             CT const c320 = 320;
 162             CT const c768 = 768;
 163             CT const c1024 = 1024;
 164             CT const c4096 = 4096;
 165             CT const c16384 = 16384;
 166
 167             //CT sqr_u = cos2_alpha * (math::sqr(radius_a) - math::sqr(radius_b)) / math::sqr(radius_b); // above (1)
 168             CT sqr_u = cos2_alpha * ( math::sqr(radius_a / radius_b) - c1 ); // above (1)
 169
 170             CT A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
 171             CT B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
 172             CT delta_sigma = B * sin_sigma * ( cos2_sigma_m + (B/c4) * (cos(sigma)* (-c1 + c2 * cos2_sigma_m)
 173                 - (B/c6) * cos2_sigma_m * (-c3 + c4 * math::sqr(sin_sigma)) * (-c3 + c4 * cos2_sigma_m))); // (6)
 174
 175             result.distance = radius_b * A * (sigma - delta_sigma); // (19)
 176         }
 177         else
 178         {
 179             result.distance = CT(0);
 180         }
 181
 182         if ( BOOST_GEOMETRY_CONDITION(EnableAzimuth) )
 183         {
 184             result.azimuth = atan2(cos_U2 * sin_lambda, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); // (20)
 185         }
 186         else
 187         {
 188             result.azimuth = CT(0);
 189         }
 190
 191         return result;
 192     }
 193
 194 //    inline CT azimuth21() const
 195 //    {
 196 //        // NOTE: signs of X and Y are different than in the original paper
 197 //        atan2(-cos_U1 * sin_lambda, sin_U1 * cos_U2 - cos_U1 * sin_U2 * cos_lambda); // (21)
 198 //    }
 199 };
 200
 201 }}} // namespace boost::geometry::detail
 202
 203
 204 #endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_INVERSE_HPP