1 // Boost.Geometry
2
3 // Copyright (c) 2016-2020, Oracle and/or its affiliates.
4 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
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_SPHERICAL_HPP
12 #define BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP
13
14 #include <boost/geometry/core/coordinate_system.hpp>
15 #include <boost/geometry/core/coordinate_type.hpp>
16 #include <boost/geometry/core/cs.hpp>
17 #include <boost/geometry/core/access.hpp>
18 #include <boost/geometry/core/radian_access.hpp>
19 #include <boost/geometry/core/radius.hpp>
20
21 //#include <boost/geometry/arithmetic/arithmetic.hpp>
22 #include <boost/geometry/arithmetic/cross_product.hpp>
23 #include <boost/geometry/arithmetic/dot_product.hpp>
24
25 #include <boost/geometry/util/math.hpp>
26 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
27 #include <boost/geometry/util/select_coordinate_type.hpp>
28
29 #include <boost/geometry/formulas/result_direct.hpp>
30
31 namespace boost { namespace geometry {
32
33 namespace formula {
34
35 template <typename T>
36 struct result_spherical
37 {
result_sphericalboost::geometry::formula::result_spherical38 result_spherical()
39 : azimuth(0)
40 , reverse_azimuth(0)
41 {}
42
43 T azimuth;
44 T reverse_azimuth;
45 };
46
47 template <typename T>
sph_to_cart3d(T const & lon,T const & lat,T & x,T & y,T & z)48 static inline void sph_to_cart3d(T const& lon, T const& lat, T & x, T & y, T & z)
49 {
50 T const cos_lat = cos(lat);
51 x = cos_lat * cos(lon);
52 y = cos_lat * sin(lon);
53 z = sin(lat);
54 }
55
56 template <typename Point3d, typename PointSph>
sph_to_cart3d(PointSph const & point_sph)57 static inline Point3d sph_to_cart3d(PointSph const& point_sph)
58 {
59 typedef typename coordinate_type<Point3d>::type calc_t;
60
61 calc_t const lon = get_as_radian<0>(point_sph);
62 calc_t const lat = get_as_radian<1>(point_sph);
63 calc_t x, y, z;
64 sph_to_cart3d(lon, lat, x, y, z);
65
66 Point3d res;
67 set<0>(res, x);
68 set<1>(res, y);
69 set<2>(res, z);
70
71 return res;
72 }
73
74 template <typename T>
cart3d_to_sph(T const & x,T const & y,T const & z,T & lon,T & lat)75 static inline void cart3d_to_sph(T const& x, T const& y, T const& z, T & lon, T & lat)
76 {
77 lon = atan2(y, x);
78 lat = asin(z);
79 }
80
81 template <typename PointSph, typename Point3d>
cart3d_to_sph(Point3d const & point_3d)82 static inline PointSph cart3d_to_sph(Point3d const& point_3d)
83 {
84 typedef typename coordinate_type<PointSph>::type coord_t;
85 typedef typename coordinate_type<Point3d>::type calc_t;
86
87 calc_t const x = get<0>(point_3d);
88 calc_t const y = get<1>(point_3d);
89 calc_t const z = get<2>(point_3d);
90 calc_t lonr, latr;
91 cart3d_to_sph(x, y, z, lonr, latr);
92
93 PointSph res;
94 set_from_radian<0>(res, lonr);
95 set_from_radian<1>(res, latr);
96
97 coord_t lon = get<0>(res);
98 coord_t lat = get<1>(res);
99
100 math::normalize_spheroidal_coordinates
101 <
102 typename detail::cs_angular_units<PointSph>::type,
103 coord_t
104 >(lon, lat);
105
106 set<0>(res, lon);
107 set<1>(res, lat);
108
109 return res;
110 }
111
112 // -1 right
113 // 1 left
114 // 0 on
115 template <typename Point3d1, typename Point3d2>
sph_side_value(Point3d1 const & norm,Point3d2 const & pt)116 static inline int sph_side_value(Point3d1 const& norm, Point3d2 const& pt)
117 {
118 typedef typename select_coordinate_type<Point3d1, Point3d2>::type calc_t;
119 calc_t c0 = 0;
120 calc_t d = dot_product(norm, pt);
121 return math::equals(d, c0) ? 0
122 : d > c0 ? 1
123 : -1; // d < 0
124 }
125
126 template <typename CT, bool ReverseAzimuth, typename T1, typename T2>
spherical_azimuth(T1 const & lon1,T1 const & lat1,T2 const & lon2,T2 const & lat2)127 static inline result_spherical<CT> spherical_azimuth(T1 const& lon1,
128 T1 const& lat1,
129 T2 const& lon2,
130 T2 const& lat2)
131 {
132 typedef result_spherical<CT> result_type;
133 result_type result;
134
135 // http://williams.best.vwh.net/avform.htm#Crs
136 // https://en.wikipedia.org/wiki/Great-circle_navigation
137 CT dlon = lon2 - lon1;
138
139 // An optimization which should kick in often for Boxes
140 //if ( math::equals(dlon, ReturnType(0)) )
141 //if ( get<0>(p1) == get<0>(p2) )
142 //{
143 // return - sin(get_as_radian<1>(p1)) * cos_p2lat);
144 //}
145
146 CT const cos_dlon = cos(dlon);
147 CT const sin_dlon = sin(dlon);
148 CT const cos_lat1 = cos(lat1);
149 CT const cos_lat2 = cos(lat2);
150 CT const sin_lat1 = sin(lat1);
151 CT const sin_lat2 = sin(lat2);
152
153 {
154 // "An alternative formula, not requiring the pre-computation of d"
155 // In the formula below dlon is used as "d"
156 CT const y = sin_dlon * cos_lat2;
157 CT const x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon;
158 result.azimuth = atan2(y, x);
159 }
160
161 if (ReverseAzimuth)
162 {
163 CT const y = sin_dlon * cos_lat1;
164 CT const x = sin_lat2 * cos_lat1 * cos_dlon - cos_lat2 * sin_lat1;
165 result.reverse_azimuth = atan2(y, x);
166 }
167
168 return result;
169 }
170
171 template <typename ReturnType, typename T1, typename T2>
spherical_azimuth(T1 const & lon1,T1 const & lat1,T2 const & lon2,T2 const & lat2)172 inline ReturnType spherical_azimuth(T1 const& lon1, T1 const& lat1,
173 T2 const& lon2, T2 const& lat2)
174 {
175 return spherical_azimuth<ReturnType, false>(lon1, lat1, lon2, lat2).azimuth;
176 }
177
178 template <typename T>
spherical_azimuth(T const & lon1,T const & lat1,T const & lon2,T const & lat2)179 inline T spherical_azimuth(T const& lon1, T const& lat1, T const& lon2, T const& lat2)
180 {
181 return spherical_azimuth<T, false>(lon1, lat1, lon2, lat2).azimuth;
182 }
183
184 template <typename T>
azimuth_side_value(T const & azi_a1_p,T const & azi_a1_a2)185 inline int azimuth_side_value(T const& azi_a1_p, T const& azi_a1_a2)
186 {
187 T const c0 = 0;
188 T const pi = math::pi<T>();
189
190 // instead of the formula from XTD
191 //calc_t a_diff = asin(sin(azi_a1_p - azi_a1_a2));
192
193 T a_diff = azi_a1_p - azi_a1_a2;
194 // normalize, angle in (-pi, pi]
195 math::detail::normalize_angle_loop<radian>(a_diff);
196
197 // NOTE: in general it shouldn't be required to support the pi/-pi case
198 // because in non-cartesian systems it makes sense to check the side
199 // only "between" the endpoints.
200 // However currently the winding strategy calls the side strategy
201 // for vertical segments to check if the point is "between the endpoints.
202 // This could be avoided since the side strategy is not required for that
203 // because meridian is the shortest path. So a difference of
204 // longitudes would be sufficient (of course normalized to (-pi, pi]).
205
206 // NOTE: with the above said, the pi/-pi check is temporary
207 // however in case if this was required
208 // the geodesics on ellipsoid aren't "symmetrical"
209 // therefore instead of comparing a_diff to pi and -pi
210 // one should probably use inverse azimuths and compare
211 // the difference to 0 as well
212
213 // positive azimuth is on the right side
214 return math::equals(a_diff, c0)
215 || math::equals(a_diff, pi)
216 || math::equals(a_diff, -pi) ? 0
217 : a_diff > 0 ? -1 // right
218 : 1; // left
219 }
220
221 template
222 <
223 bool Coordinates,
224 bool ReverseAzimuth,
225 typename CT,
226 typename Sphere
227 >
spherical_direct(CT const & lon1,CT const & lat1,CT const & sig12,CT const & alp1,Sphere const & sphere)228 inline result_direct<CT> spherical_direct(CT const& lon1,
229 CT const& lat1,
230 CT const& sig12,
231 CT const& alp1,
232 Sphere const& sphere)
233 {
234 result_direct<CT> result;
235
236 CT const sin_alp1 = sin(alp1);
237 CT const sin_lat1 = sin(lat1);
238 CT const cos_alp1 = cos(alp1);
239 CT const cos_lat1 = cos(lat1);
240
241 CT const norm = math::sqrt(cos_alp1 * cos_alp1 + sin_alp1 * sin_alp1
242 * sin_lat1 * sin_lat1);
243 CT const alp0 = atan2(sin_alp1 * cos_lat1, norm);
244 CT const sig1 = atan2(sin_lat1, cos_alp1 * cos_lat1);
245 CT const sig2 = sig1 + sig12 / get_radius<0>(sphere);
246
247 CT const cos_sig2 = cos(sig2);
248 CT const sin_alp0 = sin(alp0);
249 CT const cos_alp0 = cos(alp0);
250
251 if (Coordinates)
252 {
253 CT const sin_sig2 = sin(sig2);
254 CT const sin_sig1 = sin(sig1);
255 CT const cos_sig1 = cos(sig1);
256
257 CT const norm2 = math::sqrt(cos_alp0 * cos_alp0 * cos_sig2 * cos_sig2
258 + sin_alp0 * sin_alp0);
259 CT const lat2 = atan2(cos_alp0 * sin_sig2, norm2);
260
261 CT const omg1 = atan2(sin_alp0 * sin_sig1, cos_sig1);
262 CT const lon2 = atan2(sin_alp0 * sin_sig2, cos_sig2);
263
264 result.lon2 = lon1 + lon2 - omg1;
265 result.lat2 = lat2;
266
267 // For longitudes close to the antimeridian the result can be out
268 // of range. Therefore normalize.
269 math::detail::normalize_angle_cond<radian>(result.lon2);
270 }
271
272 if (ReverseAzimuth)
273 {
274 CT const alp2 = atan2(sin_alp0, cos_alp0 * cos_sig2);
275 result.reverse_azimuth = alp2;
276 }
277
278 return result;
279 }
280
281 } // namespace formula
282
283 }} // namespace boost::geometry
284
285 #endif // BOOST_GEOMETRY_FORMULAS_SPHERICAL_HPP
286