geometry/formulas/vincenty_direct.hpp

0001 // Boost.Geometry
0002
0003 // Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
0004
0005 // This file was modified by Oracle on 2014-2020.
0006 // Modifications copyright (c) 2014-2020 Oracle and/or its affiliates.
0007
0008 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
0009
0010 // Use, modification and distribution is subject to the Boost Software License,
0011 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
0012 // http://www.boost.org/LICENSE_1_0.txt)
0013
0014 #ifndef BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP
0015 #define BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP
0016
0017
0018 #include <boost/math/constants/constants.hpp>
0019
0020 #include <boost/geometry/core/radius.hpp>
0021
0022 #include <boost/geometry/util/condition.hpp>
0023 #include <boost/geometry/util/math.hpp>
0024 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
0025
0026 #include <boost/geometry/formulas/differential_quantities.hpp>
0027 #include <boost/geometry/formulas/flattening.hpp>
0028 #include <boost/geometry/formulas/result_direct.hpp>
0029
0030
0031 #ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
0032 #define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
0033 #endif
0034
0035
0036 namespace boost { namespace geometry { namespace formula
0037 {
0038
0039 /*!
0040 \brief The solution of the direct problem of geodesics on latlong coordinates, after Vincenty, 1975
0041 \author See
0042     - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
0043     - http://www.icsm.gov.au/gda/gdav2.3.pdf
0044 \author Adapted from various implementations to get it close to the original document
0045     - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
0046     - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
0047     - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink
0048
0049 */
0050 template <
0051     typename CT,
0052     bool EnableCoordinates = true,
0053     bool EnableReverseAzimuth = false,
0054     bool EnableReducedLength = false,
0055     bool EnableGeodesicScale = false
0056 >
0057 class vincenty_direct
0058 {
0059     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
0060     static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
0061     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
0062
0063 public:
0064     typedef result_direct<CT> result_type;
0065
0066     template <typename T, typename Dist, typename Azi, typename Spheroid>
0067     static inline result_type apply(T const& lo1,
0068                                     T const& la1,
0069                                     Dist const& distance,
0070                                     Azi const& azimuth12,
0071                                     Spheroid const& spheroid)
0072     {
0073         result_type result;
0074
0075         CT const lon1 = lo1;
0076         CT const lat1 = la1;
0077
0078         CT const radius_a = CT(get_radius<0>(spheroid));
0079         CT const radius_b = CT(get_radius<2>(spheroid));
0080         CT const flattening = formula::flattening<CT>(spheroid);
0081
0082         CT const sin_azimuth12 = sin(azimuth12);
0083         CT const cos_azimuth12 = cos(azimuth12);
0084
0085         // U: reduced latitude, defined by tan U = (1-f) tan phi
0086         CT const one_min_f = CT(1) - flattening;
0087         CT const tan_U1 = one_min_f * tan(lat1);
0088         CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1)
0089
0090         // may be calculated from tan using 1 sqrt()
0091         CT const U1 = atan(tan_U1);
0092         CT const sin_U1 = sin(U1);
0093         CT const cos_U1 = cos(U1);
0094
0095         CT const sin_alpha = cos_U1 * sin_azimuth12; // (2)
0096         CT const sin_alpha_sqr = math::sqr(sin_alpha);
0097         CT const cos_alpha_sqr = CT(1) - sin_alpha_sqr;
0098
0099         CT const b_sqr = radius_b * radius_b;
0100         CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr;
0101         CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3)
0102         CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4)
0103
0104         CT s_div_bA = distance / (radius_b * A);
0105         CT sigma = s_div_bA; // (7)
0106
0107         CT previous_sigma;
0108         CT sin_sigma;
0109         CT cos_sigma;
0110         CT cos_2sigma_m;
0111         CT cos_2sigma_m_sqr;
0112
0113         int counter = 0; // robustness
0114
0115         do
0116         {
0117             previous_sigma = sigma;
0118
0119             CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5)
0120
0121             sin_sigma = sin(sigma);
0122             cos_sigma = cos(sigma);
0123             CT const sin_sigma_sqr = math::sqr(sin_sigma);
0124             cos_2sigma_m = cos(two_sigma_m);
0125             cos_2sigma_m_sqr = math::sqr(cos_2sigma_m);
0126
0127             CT const delta_sigma = B * sin_sigma * (cos_2sigma_m
0128                                         + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr)
0129                                             - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6)
0130
0131             sigma = s_div_bA + delta_sigma; // (7)
0132
0133             ++counter; // robustness
0134
0135         } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12)
0136                //&& geometry::math::abs(sigma) < pi
0137                && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness
0138
0139         if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
0140         {
0141             result.lat2
0142                 = atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12,
0143                          one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8)
0144
0145             CT const lambda = atan2( sin_sigma * sin_azimuth12,
0146                                      cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_azimuth12); // (9)
0147             CT const C = (flattening/CT(16)) * cos_alpha_sqr * ( CT(4) + flattening * ( CT(4) - CT(3) * cos_alpha_sqr ) ); // (10)
0148             CT const L = lambda - (CT(1) - C) * flattening * sin_alpha
0149                             * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( CT(-1) + CT(2) * cos_2sigma_m_sqr ) ) ); // (11)
0150
0151             result.lon2 = lon1 + L;
0152         }
0153
0154         if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
0155         {
0156             result.reverse_azimuth
0157                 = atan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_azimuth12); // (12)
0158         }
0159
0160         if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
0161         {
0162             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
0163             quantities::apply(lon1, lat1, result.lon2, result.lat2,
0164                               azimuth12, result.reverse_azimuth,
0165                               radius_b, flattening,
0166                               result.reduced_length, result.geodesic_scale);
0167         }
0168
0169         if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
0170         {
0171             // For longitudes close to the antimeridian the result can be out
0172             // of range. Therefore normalize.
0173             // It has to be done at the end because otherwise differential
0174             // quantities are calculated incorrectly.
0175             math::detail::normalize_angle_cond<radian>(result.lon2);
0176         }
0177
0178         return result;
0179     }
0180
0181 };
0182
0183 }}} // namespace boost::geometry::formula
0184
0185
0186 #endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP