1 // Boost.Geometry 2 3 // Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands. 4 5 // This file was modified by Oracle on 2014-2020. 6 // Modifications copyright (c) 2014-2020 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_FORMULAS_VINCENTY_DIRECT_HPP 15 #define BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP 16 17 18 #include <boost/math/constants/constants.hpp> 19 20 #include <boost/geometry/core/radius.hpp> 21 22 #include <boost/geometry/util/condition.hpp> 23 #include <boost/geometry/util/math.hpp> 24 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp> 25 26 #include <boost/geometry/formulas/differential_quantities.hpp> 27 #include <boost/geometry/formulas/flattening.hpp> 28 #include <boost/geometry/formulas/result_direct.hpp> 29 30 31 #ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 32 #define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000 33 #endif 34 35 36 namespace boost { namespace geometry { namespace formula 37 { 38 39 /*! 40 \brief The solution of the direct problem of geodesics on latlong coordinates, after Vincenty, 1975 41 \author See 42 - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf 43 - http://www.icsm.gov.au/gda/gdav2.3.pdf 44 \author Adapted from various implementations to get it close to the original document 45 - http://www.movable-type.co.uk/scripts/LatLongVincenty.html 46 - http://exogen.case.edu/projects/geopy/source/geopy.distance.html 47 - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink 48 49 */ 50 template < 51 typename CT, 52 bool EnableCoordinates = true, 53 bool EnableReverseAzimuth = false, 54 bool EnableReducedLength = false, 55 bool EnableGeodesicScale = false 56 > 57 class vincenty_direct 58 { 59 static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale; 60 static const bool CalcCoordinates = EnableCoordinates || CalcQuantities; 61 static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities; 62 63 public: 64 typedef result_direct<CT> result_type; 65 66 template <typename T, typename Dist, typename Azi, typename Spheroid> apply(T const & lo1,T const & la1,Dist const & distance,Azi const & azimuth12,Spheroid const & spheroid)67 static inline result_type apply(T const& lo1, 68 T const& la1, 69 Dist const& distance, 70 Azi const& azimuth12, 71 Spheroid const& spheroid) 72 { 73 result_type result; 74 75 CT const lon1 = lo1; 76 CT const lat1 = la1; 77 78 CT const radius_a = CT(get_radius<0>(spheroid)); 79 CT const radius_b = CT(get_radius<2>(spheroid)); 80 CT const flattening = formula::flattening<CT>(spheroid); 81 82 CT const sin_azimuth12 = sin(azimuth12); 83 CT const cos_azimuth12 = cos(azimuth12); 84 85 // U: reduced latitude, defined by tan U = (1-f) tan phi 86 CT const one_min_f = CT(1) - flattening; 87 CT const tan_U1 = one_min_f * tan(lat1); 88 CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1) 89 90 // may be calculated from tan using 1 sqrt() 91 CT const U1 = atan(tan_U1); 92 CT const sin_U1 = sin(U1); 93 CT const cos_U1 = cos(U1); 94 95 CT const sin_alpha = cos_U1 * sin_azimuth12; // (2) 96 CT const sin_alpha_sqr = math::sqr(sin_alpha); 97 CT const cos_alpha_sqr = CT(1) - sin_alpha_sqr; 98 99 CT const b_sqr = radius_b * radius_b; 100 CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr; 101 CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3) 102 CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4) 103 104 CT s_div_bA = distance / (radius_b * A); 105 CT sigma = s_div_bA; // (7) 106 107 CT previous_sigma; 108 CT sin_sigma; 109 CT cos_sigma; 110 CT cos_2sigma_m; 111 CT cos_2sigma_m_sqr; 112 113 int counter = 0; // robustness 114 115 do 116 { 117 previous_sigma = sigma; 118 119 CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5) 120 121 sin_sigma = sin(sigma); 122 cos_sigma = cos(sigma); 123 CT const sin_sigma_sqr = math::sqr(sin_sigma); 124 cos_2sigma_m = cos(two_sigma_m); 125 cos_2sigma_m_sqr = math::sqr(cos_2sigma_m); 126 127 CT const delta_sigma = B * sin_sigma * (cos_2sigma_m 128 + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr) 129 - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6) 130 131 sigma = s_div_bA + delta_sigma; // (7) 132 133 ++counter; // robustness 134 135 } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12) 136 //&& geometry::math::abs(sigma) < pi 137 && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness 138 139 if (BOOST_GEOMETRY_CONDITION(CalcCoordinates)) 140 { 141 result.lat2 142 = atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12, 143 one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8) 144 145 CT const lambda = atan2( sin_sigma * sin_azimuth12, 146 cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_azimuth12); // (9) 147 CT const C = (flattening/CT(16)) * cos_alpha_sqr * ( CT(4) + flattening * ( CT(4) - CT(3) * cos_alpha_sqr ) ); // (10) 148 CT const L = lambda - (CT(1) - C) * flattening * sin_alpha 149 * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( CT(-1) + CT(2) * cos_2sigma_m_sqr ) ) ); // (11) 150 151 result.lon2 = lon1 + L; 152 } 153 154 if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth)) 155 { 156 result.reverse_azimuth 157 = atan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_azimuth12); // (12) 158 } 159 160 if (BOOST_GEOMETRY_CONDITION(CalcQuantities)) 161 { 162 typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities; 163 quantities::apply(lon1, lat1, result.lon2, result.lat2, 164 azimuth12, result.reverse_azimuth, 165 radius_b, flattening, 166 result.reduced_length, result.geodesic_scale); 167 } 168 169 if (BOOST_GEOMETRY_CONDITION(CalcCoordinates)) 170 { 171 // For longitudes close to the antimeridian the result can be out 172 // of range. Therefore normalize. 173 // It has to be done at the end because otherwise differential 174 // quantities are calculated incorrectly. 175 math::detail::normalize_angle_cond<radian>(result.lon2); 176 } 177 178 return result; 179 } 180 181 }; 182 183 }}} // namespace boost::geometry::formula 184 185 186 #endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP 187