]>
Commit | Line | Data |
---|---|---|
7c673cae FG |
1 | // Boost.Geometry |
2 | ||
3 | // Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands. | |
4 | ||
11fdf7f2 TL |
5 | // This file was modified by Oracle on 2014, 2016, 2017. |
6 | // Modifications copyright (c) 2014-2017 Oracle and/or its affiliates. | |
7c673cae FG |
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 | ||
b32b8144 FG |
14 | #ifndef BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP |
15 | #define BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP | |
7c673cae FG |
16 | |
17 | ||
18 | #include <boost/math/constants/constants.hpp> | |
19 | ||
20 | #include <boost/geometry/core/radius.hpp> | |
7c673cae | 21 | |
b32b8144 | 22 | #include <boost/geometry/util/condition.hpp> |
7c673cae FG |
23 | #include <boost/geometry/util/math.hpp> |
24 | ||
b32b8144 FG |
25 | #include <boost/geometry/formulas/differential_quantities.hpp> |
26 | #include <boost/geometry/formulas/flattening.hpp> | |
27 | #include <boost/geometry/formulas/result_direct.hpp> | |
7c673cae FG |
28 | |
29 | ||
30 | #ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS | |
31 | #define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000 | |
32 | #endif | |
33 | ||
34 | ||
b32b8144 | 35 | namespace boost { namespace geometry { namespace formula |
7c673cae FG |
36 | { |
37 | ||
7c673cae FG |
38 | /*! |
39 | \brief The solution of the direct 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 | */ | |
b32b8144 FG |
49 | template < |
50 | typename CT, | |
51 | bool EnableCoordinates = true, | |
52 | bool EnableReverseAzimuth = false, | |
53 | bool EnableReducedLength = false, | |
54 | bool EnableGeodesicScale = false | |
55 | > | |
56 | class vincenty_direct | |
7c673cae | 57 | { |
b32b8144 FG |
58 | static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale; |
59 | static const bool CalcCoordinates = EnableCoordinates || CalcQuantities; | |
60 | static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities; | |
7c673cae FG |
61 | |
62 | public: | |
b32b8144 FG |
63 | typedef result_direct<CT> result_type; |
64 | ||
7c673cae FG |
65 | template <typename T, typename Dist, typename Azi, typename Spheroid> |
66 | static inline result_type apply(T const& lo1, | |
67 | T const& la1, | |
68 | Dist const& distance, | |
69 | Azi const& azimuth12, | |
70 | Spheroid const& spheroid) | |
71 | { | |
72 | result_type result; | |
73 | ||
74 | CT const lon1 = lo1; | |
75 | CT const lat1 = la1; | |
76 | ||
77 | if ( math::equals(distance, Dist(0)) || distance < Dist(0) ) | |
78 | { | |
b32b8144 FG |
79 | result.lon2 = lon1; |
80 | result.lat2 = lat1; | |
7c673cae FG |
81 | return result; |
82 | } | |
83 | ||
84 | CT const radius_a = CT(get_radius<0>(spheroid)); | |
85 | CT const radius_b = CT(get_radius<2>(spheroid)); | |
b32b8144 | 86 | CT const flattening = formula::flattening<CT>(spheroid); |
7c673cae FG |
87 | |
88 | CT const sin_azimuth12 = sin(azimuth12); | |
89 | CT const cos_azimuth12 = cos(azimuth12); | |
90 | ||
91 | // U: reduced latitude, defined by tan U = (1-f) tan phi | |
92 | CT const one_min_f = CT(1) - flattening; | |
93 | CT const tan_U1 = one_min_f * tan(lat1); | |
94 | CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1) | |
95 | ||
96 | // may be calculated from tan using 1 sqrt() | |
97 | CT const U1 = atan(tan_U1); | |
98 | CT const sin_U1 = sin(U1); | |
99 | CT const cos_U1 = cos(U1); | |
100 | ||
101 | CT const sin_alpha = cos_U1 * sin_azimuth12; // (2) | |
102 | CT const sin_alpha_sqr = math::sqr(sin_alpha); | |
103 | CT const cos_alpha_sqr = CT(1) - sin_alpha_sqr; | |
104 | ||
105 | CT const b_sqr = radius_b * radius_b; | |
106 | CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr; | |
107 | CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3) | |
108 | CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4) | |
109 | ||
110 | CT s_div_bA = distance / (radius_b * A); | |
111 | CT sigma = s_div_bA; // (7) | |
112 | ||
113 | CT previous_sigma; | |
114 | CT sin_sigma; | |
115 | CT cos_sigma; | |
116 | CT cos_2sigma_m; | |
117 | CT cos_2sigma_m_sqr; | |
118 | ||
119 | int counter = 0; // robustness | |
120 | ||
121 | do | |
122 | { | |
123 | previous_sigma = sigma; | |
124 | ||
125 | CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5) | |
126 | ||
127 | sin_sigma = sin(sigma); | |
128 | cos_sigma = cos(sigma); | |
129 | CT const sin_sigma_sqr = math::sqr(sin_sigma); | |
130 | cos_2sigma_m = cos(two_sigma_m); | |
131 | cos_2sigma_m_sqr = math::sqr(cos_2sigma_m); | |
132 | ||
133 | CT const delta_sigma = B * sin_sigma * (cos_2sigma_m | |
134 | + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr) | |
135 | - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6) | |
136 | ||
137 | sigma = s_div_bA + delta_sigma; // (7) | |
138 | ||
139 | ++counter; // robustness | |
140 | ||
141 | } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12) | |
142 | //&& geometry::math::abs(sigma) < pi | |
143 | && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness | |
144 | ||
b32b8144 | 145 | if (BOOST_GEOMETRY_CONDITION(CalcCoordinates)) |
7c673cae FG |
146 | { |
147 | result.lat2 | |
148 | = atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12, | |
149 | one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8) | |
b32b8144 | 150 | |
7c673cae FG |
151 | CT const lambda = atan2( sin_sigma * sin_azimuth12, |
152 | cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_azimuth12); // (9) | |
153 | CT const C = (flattening/CT(16)) * cos_alpha_sqr * ( CT(4) + flattening * ( CT(4) - CT(3) * cos_alpha_sqr ) ); // (10) | |
154 | CT const L = lambda - (CT(1) - C) * flattening * sin_alpha | |
155 | * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( CT(-1) + CT(2) * cos_2sigma_m_sqr ) ) ); // (11) | |
156 | ||
157 | result.lon2 = lon1 + L; | |
158 | } | |
159 | ||
b32b8144 FG |
160 | if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth)) |
161 | { | |
162 | result.reverse_azimuth | |
163 | = atan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_azimuth12); // (12) | |
164 | } | |
165 | ||
166 | if (BOOST_GEOMETRY_CONDITION(CalcQuantities)) | |
167 | { | |
168 | typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities; | |
169 | quantities::apply(lon1, lat1, result.lon2, result.lat2, | |
170 | azimuth12, result.reverse_azimuth, | |
171 | radius_b, flattening, | |
172 | result.reduced_length, result.geodesic_scale); | |
173 | } | |
174 | ||
7c673cae FG |
175 | return result; |
176 | } | |
177 | ||
7c673cae FG |
178 | }; |
179 | ||
b32b8144 | 180 | }}} // namespace boost::geometry::formula |
7c673cae FG |
181 | |
182 | ||
b32b8144 | 183 | #endif // BOOST_GEOMETRY_FORMULAS_VINCENTY_DIRECT_HPP |