]>
Commit | Line | Data |
---|---|---|
1 | // Boost.Geometry | |
2 | ||
3 | // Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands. | |
4 | ||
5 | // This file was modified by Oracle on 2014. | |
6 | // Modifications copyright (c) 2014 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_ALGORITHMS_DETAIL_VINCENTY_DIRECT_HPP | |
15 | #define BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_DIRECT_HPP | |
16 | ||
17 | ||
18 | #include <boost/math/constants/constants.hpp> | |
19 | ||
20 | #include <boost/geometry/core/radius.hpp> | |
21 | #include <boost/geometry/core/srs.hpp> | |
22 | ||
23 | #include <boost/geometry/util/math.hpp> | |
24 | ||
25 | #include <boost/geometry/algorithms/detail/flattening.hpp> | |
26 | ||
27 | ||
28 | #ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS | |
29 | #define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000 | |
30 | #endif | |
31 | ||
32 | ||
33 | namespace boost { namespace geometry { namespace detail | |
34 | { | |
35 | ||
36 | template <typename T> | |
37 | struct result_direct | |
38 | { | |
39 | void set(T const& lo2, T const& la2) | |
40 | { | |
41 | lon2 = lo2; | |
42 | lat2 = la2; | |
43 | } | |
44 | T lon2; | |
45 | T lat2; | |
46 | }; | |
47 | ||
48 | /*! | |
49 | \brief The solution of the direct problem of geodesics on latlong coordinates, after Vincenty, 1975 | |
50 | \author See | |
51 | - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf | |
52 | - http://www.icsm.gov.au/gda/gdav2.3.pdf | |
53 | \author Adapted from various implementations to get it close to the original document | |
54 | - http://www.movable-type.co.uk/scripts/LatLongVincenty.html | |
55 | - http://exogen.case.edu/projects/geopy/source/geopy.distance.html | |
56 | - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink | |
57 | ||
58 | */ | |
59 | template <typename CT> | |
60 | struct vincenty_direct | |
61 | { | |
62 | typedef result_direct<CT> result_type; | |
63 | ||
64 | public: | |
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 | { | |
79 | result.set(lon1, lat1); | |
80 | return result; | |
81 | } | |
82 | ||
83 | CT const radius_a = CT(get_radius<0>(spheroid)); | |
84 | CT const radius_b = CT(get_radius<2>(spheroid)); | |
85 | CT const flattening = geometry::detail::flattening<CT>(spheroid); | |
86 | ||
87 | CT const sin_azimuth12 = sin(azimuth12); | |
88 | CT const cos_azimuth12 = cos(azimuth12); | |
89 | ||
90 | // U: reduced latitude, defined by tan U = (1-f) tan phi | |
91 | CT const one_min_f = CT(1) - flattening; | |
92 | CT const tan_U1 = one_min_f * tan(lat1); | |
93 | CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1) | |
94 | ||
95 | // may be calculated from tan using 1 sqrt() | |
96 | CT const U1 = atan(tan_U1); | |
97 | CT const sin_U1 = sin(U1); | |
98 | CT const cos_U1 = cos(U1); | |
99 | ||
100 | CT const sin_alpha = cos_U1 * sin_azimuth12; // (2) | |
101 | CT const sin_alpha_sqr = math::sqr(sin_alpha); | |
102 | CT const cos_alpha_sqr = CT(1) - sin_alpha_sqr; | |
103 | ||
104 | CT const b_sqr = radius_b * radius_b; | |
105 | CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr; | |
106 | CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3) | |
107 | CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4) | |
108 | ||
109 | CT s_div_bA = distance / (radius_b * A); | |
110 | CT sigma = s_div_bA; // (7) | |
111 | ||
112 | CT previous_sigma; | |
113 | CT sin_sigma; | |
114 | CT cos_sigma; | |
115 | CT cos_2sigma_m; | |
116 | CT cos_2sigma_m_sqr; | |
117 | ||
118 | int counter = 0; // robustness | |
119 | ||
120 | do | |
121 | { | |
122 | previous_sigma = sigma; | |
123 | ||
124 | CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5) | |
125 | ||
126 | sin_sigma = sin(sigma); | |
127 | cos_sigma = cos(sigma); | |
128 | CT const sin_sigma_sqr = math::sqr(sin_sigma); | |
129 | cos_2sigma_m = cos(two_sigma_m); | |
130 | cos_2sigma_m_sqr = math::sqr(cos_2sigma_m); | |
131 | ||
132 | CT const delta_sigma = B * sin_sigma * (cos_2sigma_m | |
133 | + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr) | |
134 | - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6) | |
135 | ||
136 | sigma = s_div_bA + delta_sigma; // (7) | |
137 | ||
138 | ++counter; // robustness | |
139 | ||
140 | } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12) | |
141 | //&& geometry::math::abs(sigma) < pi | |
142 | && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness | |
143 | ||
144 | { | |
145 | result.lat2 | |
146 | = atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12, | |
147 | one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8) | |
148 | } | |
149 | ||
150 | { | |
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 | ||
160 | return result; | |
161 | } | |
162 | ||
163 | /* | |
164 | inline CT azimuth21() const | |
165 | { | |
166 | // NOTE: signs of X and Y are different than in the original paper | |
167 | return is_distance_zero ? | |
168 | CT(0) : | |
169 | atan2(-sin_alpha, sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12); // (12) | |
170 | } | |
171 | */ | |
172 | }; | |
173 | ||
174 | }}} // namespace boost::geometry::detail | |
175 | ||
176 | ||
177 | #endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_DIRECT_HPP |