boost/geometry/formulas/thomas_inverse.hpp

   1 // Boost.Geometry
   2
   3 // Copyright (c) 2015-2016 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_INVERSE_HPP
  12 #define BOOST_GEOMETRY_FORMULAS_THOMAS_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 second 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/AD0703541
  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 thomas_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 = 0;
  75         CT const c1 = 1;
  76         CT const c2 = 2;
  77         CT const c4 = 4;
  78
  79         CT const pi_half = math::pi<CT>() / c2;
  80         CT const f = formula::flattening<CT>(spheroid);
  81         CT const one_minus_f = c1 - f;
  82
  83 //        CT const tan_theta1 = one_minus_f * tan(lat1);
  84 //        CT const tan_theta2 = one_minus_f * tan(lat2);
  85 //        CT const theta1 = atan(tan_theta1);
  86 //        CT const theta2 = atan(tan_theta2);
  87
  88         CT const theta1 = math::equals(lat1, pi_half) ? lat1 :
  89                           math::equals(lat1, -pi_half) ? lat1 :
  90                           atan(one_minus_f * tan(lat1));
  91         CT const theta2 = math::equals(lat2, pi_half) ? lat2 :
  92                           math::equals(lat2, -pi_half) ? lat2 :
  93                           atan(one_minus_f * tan(lat2));
  94
  95         CT const theta_m = (theta1 + theta2) / c2;
  96         CT const d_theta_m = (theta2 - theta1) / c2;
  97         CT const d_lambda = lon2 - lon1;
  98         CT const d_lambda_m = d_lambda / c2;
  99
 100         CT const sin_theta_m = sin(theta_m);
 101         CT const cos_theta_m = cos(theta_m);
 102         CT const sin_d_theta_m = sin(d_theta_m);
 103         CT const cos_d_theta_m = cos(d_theta_m);
 104         CT const sin2_theta_m = math::sqr(sin_theta_m);
 105         CT const cos2_theta_m = math::sqr(cos_theta_m);
 106         CT const sin2_d_theta_m = math::sqr(sin_d_theta_m);
 107         CT const cos2_d_theta_m = math::sqr(cos_d_theta_m);
 108         CT const sin_d_lambda_m = sin(d_lambda_m);
 109         CT const sin2_d_lambda_m = math::sqr(sin_d_lambda_m);
 110
 111         CT const H = cos2_theta_m - sin2_d_theta_m;
 112         CT const L = sin2_d_theta_m + H * sin2_d_lambda_m;
 113         CT const cos_d = c1 - c2 * L;
 114         CT const d = acos(cos_d);
 115         CT const sin_d = sin(d);
 116
 117         CT const one_minus_L = c1 - L;
 118
 119         if ( math::equals(sin_d, c0)
 120           || math::equals(L, c0)
 121           || math::equals(one_minus_L, c0) )
 122         {
 123             return result;
 124         }
 125
 126         CT const U = c2 * sin2_theta_m * cos2_d_theta_m / one_minus_L;
 127         CT const V = c2 * sin2_d_theta_m * cos2_theta_m / L;
 128         CT const X = U + V;
 129         CT const Y = U - V;
 130         CT const T = d / sin_d;
 131         CT const D = c4 * math::sqr(T);
 132         CT const E = c2 * cos_d;
 133         CT const A = D * E;
 134         CT const B = c2 * D;
 135         CT const C = T - (A - E) / c2;
 136
 137         CT const f_sqr = math::sqr(f);
 138         CT const f_sqr_per_64 = f_sqr / CT(64);
 139
 140         if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
 141         {
 142             CT const n1 = X * (A + C*X);
 143             CT const n2 = Y * (B + E*Y);
 144             CT const n3 = D*X*Y;
 145
 146             CT const delta1d = f * (T*X-Y) / c4;
 147             CT const delta2d = f_sqr_per_64 * (n1 - n2 + n3);
 148
 149             CT const a = get_radius<0>(spheroid);
 150
 151             //result.distance = a * sin_d * (T - delta1d);
 152             result.distance = a * sin_d * (T - delta1d + delta2d);
 153         }
 154
 155         if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
 156         {
 157             // NOTE: if both cos_latX == 0 then below we'd have 0 * INF
 158             // it's a situation when the endpoints are on the poles +-90 deg
 159             // in this case the azimuth could either be 0 or +-pi
 160             // but above always 0 is returned
 161
 162             CT const F = c2*Y-E*(c4-X);
 163             CT const M = CT(32)*T-(CT(20)*T-A)*X-(B+c4)*Y;
 164             CT const G = f*T/c2 + f_sqr_per_64 * M;
 165
 166             // TODO:
 167             // If d_lambda is close to 90 or -90 deg then tan(d_lambda) is big
 168             // and F is small. The result is not accurate.
 169             // In the edge case the result may be 2 orders of magnitude less
 170             // accurate than Andoyer's.
 171             CT const tan_d_lambda = tan(d_lambda);
 172             CT const Q = -(F*G*tan_d_lambda) / c4;
 173             CT const d_lambda_m_p = (d_lambda + Q) / c2;
 174             CT const tan_d_lambda_m_p = tan(d_lambda_m_p);
 175
 176             CT const v = atan2(cos_d_theta_m, sin_theta_m * tan_d_lambda_m_p);
 177             CT const u = atan2(-sin_d_theta_m, cos_theta_m * tan_d_lambda_m_p);
 178
 179             CT const pi = math::pi<CT>();
 180
 181             if (BOOST_GEOMETRY_CONDITION(EnableAzimuth))
 182             {
 183                 CT alpha1 = v + u;
 184                 if (alpha1 > pi)
 185                 {
 186                     alpha1 -= c2 * pi;
 187                 }
 188
 189                 result.azimuth = alpha1;
 190             }
 191
 192             if (BOOST_GEOMETRY_CONDITION(EnableReverseAzimuth))
 193             {
 194                 CT alpha2 = pi - (v - u);
 195                 if (alpha2 > pi)
 196                 {
 197                     alpha2 -= c2 * pi;
 198                 }
 199
 200                 result.reverse_azimuth = alpha2;
 201             }
 202         }
 203
 204         if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
 205         {
 206             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
 207             quantities::apply(lon1, lat1, lon2, lat2,
 208                               result.azimuth, result.reverse_azimuth,
 209                               get_radius<2>(spheroid), f,
 210                               result.reduced_length, result.geodesic_scale);
 211         }
 212
 213         return result;
 214     }
 215 };
 216
 217 }}} // namespace boost::geometry::formula
 218
 219
 220 #endif // BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP