ceph/src/boost/boost/geometry/formulas/thomas_direct.hpp

   1 // Boost.Geometry
   2
   3 // Copyright (c) 2016-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_THOMAS_DIRECT_HPP
  12 #define BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_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_direct.hpp>
  26
  27
  28 namespace boost { namespace geometry { namespace formula
  29 {
  30
  31
  32 /*!
  33 \brief The solution of the direct problem of geodesics on latlong coordinates,
  34        Forsyth-Andoyer-Lambert type approximation with second 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/AD0703541
  40
  41 */
  42 template <
  43     typename CT,
  44     bool EnableCoordinates = true,
  45     bool EnableReverseAzimuth = false,
  46     bool EnableReducedLength = false,
  47     bool EnableGeodesicScale = false
  48 >
  49 class thomas_direct
  50 {
  51     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
  52     static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
  53     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcCoordinates || CalcQuantities;
  54
  55 public:
  56     typedef result_direct<CT> result_type;
  57
  58     template <typename T, typename Dist, typename Azi, typename Spheroid>
  59     static inline result_type apply(T const& lo1,
  60                                     T const& la1,
  61                                     Dist const& distance,
  62                                     Azi const& azimuth12,
  63                                     Spheroid const& spheroid)
  64     {
  65         result_type result;
  66
  67         CT const lon1 = lo1;
  68         CT const lat1 = la1;
  69
  70         if ( math::equals(distance, Dist(0)) || distance < Dist(0) )
  71         {
  72             result.lon2 = lon1;
  73             result.lat2 = lat1;
  74             return result;
  75         }
  76
  77         CT const c0 = 0;
  78         CT const c1 = 1;
  79         CT const c2 = 2;
  80         CT const c4 = 4;
  81
  82         CT const a = CT(get_radius<0>(spheroid));
  83         CT const b = CT(get_radius<2>(spheroid));
  84         CT const f = formula::flattening<CT>(spheroid);
  85         CT const one_minus_f = c1 - f;
  86
  87         CT const pi = math::pi<CT>();
  88         CT const pi_half = pi / c2;
  89
  90         // keep azimuth small - experiments show low accuracy
  91         // if the azimuth is closer to (+-)180 deg.
  92         CT azi12_alt = azimuth12;
  93         CT lat1_alt = lat1;
  94         bool alter_result = vflip_if_south(lat1, azimuth12, lat1_alt, azi12_alt);
  95
  96         CT const theta1 = math::equals(lat1_alt, pi_half) ? lat1_alt :
  97                           math::equals(lat1_alt, -pi_half) ? lat1_alt :
  98                           atan(one_minus_f * tan(lat1_alt));
  99         CT const sin_theta1 = sin(theta1);
 100         CT const cos_theta1 = cos(theta1);
 101
 102         CT const sin_a12 = sin(azi12_alt);
 103         CT const cos_a12 = cos(azi12_alt);
 104
 105         CT const M = cos_theta1 * sin_a12; // cos_theta0
 106         CT const theta0 = acos(M);
 107         CT const sin_theta0 = sin(theta0);
 108
 109         CT const N = cos_theta1 * cos_a12;
 110         CT const C1 = f * M; // lower-case c1 in the technical report
 111         CT const C2 = f * (c1 - math::sqr(M)) / c4; // lower-case c2 in the technical report
 112         CT const D = (c1 - C2) * (c1 - C2 - C1 * M);
 113         CT const P = C2 * (c1 + C1 * M / c2) / D;
 114
 115         // special case for equator:
 116         // sin_theta0 = 0 <=> lat1 = 0 ^ |azimuth12| = pi/2
 117         // NOTE: in this case it doesn't matter what's the value of cos_sigma1 because
 118         //       theta1=0, theta0=0, M=1|-1, C2=0 so X=0 and Y=0 so d_sigma=d
 119         //       cos_a12=0 so N=0, therefore
 120         //       lat2=0, azi21=pi/2|-pi/2
 121         //       d_eta = atan2(sin_d_sigma, cos_d_sigma)
 122         //       H = C1 * d_sigma
 123         CT const cos_sigma1 = math::equals(sin_theta0, c0)
 124                                 ? c1
 125                                 : normalized1_1(sin_theta1 / sin_theta0);
 126         CT const sigma1 = acos(cos_sigma1);
 127         CT const d = distance / (a * D);
 128         CT const u = 2 * (sigma1 - d);
 129         CT const cos_d = cos(d);
 130         CT const sin_d = sin(d);
 131         CT const cos_u = cos(u);
 132         CT const sin_u = sin(u);
 133
 134         CT const W = c1 - c2 * P * cos_u;
 135         CT const V = cos_u * cos_d - sin_u * sin_d;
 136         CT const X = math::sqr(C2) * sin_d * cos_d * (2 * math::sqr(V) - c1);
 137         CT const Y = c2 * P * V * W * sin_d;
 138         CT const d_sigma = d + X - Y;
 139         CT const sin_d_sigma = sin(d_sigma);
 140         CT const cos_d_sigma = cos(d_sigma);
 141
 142         if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
 143         {
 144             result.reverse_azimuth = atan2(M, N * cos_d_sigma - sin_theta1 * sin_d_sigma);
 145
 146             if (alter_result)
 147             {
 148                 vflip_rev_azi(result.reverse_azimuth, azimuth12);
 149             }
 150         }
 151
 152         if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
 153         {
 154             CT const S_sigma = c2 * sigma1 - d_sigma;
 155             CT const cos_S_sigma = cos(S_sigma);
 156             CT const d_eta = atan2(sin_d_sigma * sin_a12, cos_theta1 * cos_d_sigma - sin_theta1 * sin_d_sigma * cos_a12);
 157             CT const H = C1 * (c1 - C2) * d_sigma - C1 * C2 * sin_d_sigma * cos_S_sigma;
 158             CT const d_lambda = d_eta - H;
 159
 160             result.lon2 = lon1 + d_lambda;
 161
 162             if (! math::equals(M, c0))
 163             {
 164                 CT const sin_a21 = sin(result.reverse_azimuth);
 165                 CT const tan_theta2 = (sin_theta1 * cos_d_sigma + N * sin_d_sigma) * sin_a21 / M;
 166                 result.lat2 = atan(tan_theta2 / one_minus_f);
 167             }
 168             else
 169             {
 170                 CT const sigma2 = S_sigma - sigma1;
 171                 //theta2 = asin(cos(sigma2)) <=> sin_theta0 = 1
 172                 // NOTE: cos(sigma2) defines the sign of tan_theta2
 173                 CT const tan_theta2 = cos(sigma2) / math::abs(sin(sigma2));
 174                 result.lat2 = atan(tan_theta2 / one_minus_f);
 175             }
 176
 177             if (alter_result)
 178             {
 179                 result.lat2 = -result.lat2;
 180             }
 181         }
 182
 183         if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
 184         {
 185             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
 186             quantities::apply(lon1, lat1, result.lon2, result.lat2,
 187                               azimuth12, result.reverse_azimuth,
 188                               b, f,
 189                               result.reduced_length, result.geodesic_scale);
 190         }
 191
 192         return result;
 193     }
 194
 195 private:
 196     static inline bool vflip_if_south(CT const& lat1, CT const& azi12, CT & lat1_alt, CT & azi12_alt)
 197     {
 198         CT const c2 = 2;
 199         CT const pi = math::pi<CT>();
 200         CT const pi_half = pi / c2;
 201
 202         if (azi12 > pi_half)
 203         {
 204             azi12_alt = pi - azi12;
 205             lat1_alt = -lat1;
 206             return true;
 207         }
 208         else if (azi12 < -pi_half)
 209         {
 210             azi12_alt = -pi - azi12;
 211             lat1_alt = -lat1;
 212             return true;
 213         }
 214
 215         return false;
 216     }
 217
 218     static inline void vflip_rev_azi(CT & rev_azi, CT const& azimuth12)
 219     {
 220         CT const c0 = 0;
 221         CT const pi = math::pi<CT>();
 222
 223         if (rev_azi == c0)
 224         {
 225             rev_azi = azimuth12 >= 0 ? pi : -pi;
 226         }
 227         else if (rev_azi > c0)
 228         {
 229             rev_azi = pi - rev_azi;
 230         }
 231         else
 232         {
 233             rev_azi = -pi - rev_azi;
 234         }
 235     }
 236
 237     static inline CT normalized1_1(CT const& value)
 238     {
 239         CT const c1 = 1;
 240         return value > c1 ? c1 :
 241                value < -c1 ? -c1 :
 242                value;
 243     }
 244 };
 245
 246 }}} // namespace boost::geometry::formula
 247
 248
 249 #endif // BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP