]>
Commit | Line | Data |
---|---|---|
b32b8144 FG |
1 | // Boost.Geometry |
2 | ||
1e59de90 | 3 | // Copyright (c) 2016-2021, Oracle and/or its affiliates. |
b32b8144 FG |
4 | // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle |
5 | ||
6 | // Use, modification and distribution is subject to the Boost Software License, | |
7 | // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at | |
8 | // http://www.boost.org/LICENSE_1_0.txt) | |
9 | ||
10 | #ifndef BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP | |
11 | #define BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP | |
12 | ||
13 | #include <boost/geometry/core/coordinate_system.hpp> | |
14 | #include <boost/geometry/core/coordinate_type.hpp> | |
15 | #include <boost/geometry/core/access.hpp> | |
16 | #include <boost/geometry/core/radian_access.hpp> | |
17 | ||
18 | #include <boost/geometry/arithmetic/arithmetic.hpp> | |
19 | #include <boost/geometry/arithmetic/cross_product.hpp> | |
20 | #include <boost/geometry/arithmetic/dot_product.hpp> | |
21 | #include <boost/geometry/arithmetic/normalize.hpp> | |
22 | ||
23 | #include <boost/geometry/formulas/eccentricity_sqr.hpp> | |
24 | #include <boost/geometry/formulas/flattening.hpp> | |
25 | #include <boost/geometry/formulas/unit_spheroid.hpp> | |
26 | ||
27 | #include <boost/geometry/util/math.hpp> | |
28 | #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp> | |
29 | #include <boost/geometry/util/select_coordinate_type.hpp> | |
30 | ||
31 | namespace boost { namespace geometry { | |
32 | ||
33 | namespace formula { | |
34 | ||
35 | template <typename Point3d, typename PointGeo, typename Spheroid> | |
36 | inline Point3d geo_to_cart3d(PointGeo const& point_geo, Spheroid const& spheroid) | |
37 | { | |
38 | typedef typename coordinate_type<Point3d>::type calc_t; | |
39 | ||
40 | calc_t const c1 = 1; | |
41 | calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid); | |
42 | ||
43 | calc_t const lon = get_as_radian<0>(point_geo); | |
44 | calc_t const lat = get_as_radian<1>(point_geo); | |
45 | ||
46 | Point3d res; | |
47 | ||
48 | calc_t const sin_lat = sin(lat); | |
49 | ||
50 | // "unit" spheroid, a = 1 | |
51 | calc_t const N = c1 / math::sqrt(c1 - e_sqr * math::sqr(sin_lat)); | |
52 | calc_t const N_cos_lat = N * cos(lat); | |
53 | ||
54 | set<0>(res, N_cos_lat * cos(lon)); | |
55 | set<1>(res, N_cos_lat * sin(lon)); | |
56 | set<2>(res, N * (c1 - e_sqr) * sin_lat); | |
57 | ||
58 | return res; | |
59 | } | |
60 | ||
61 | template <typename PointGeo, typename Spheroid, typename Point3d> | |
62 | inline void geo_to_cart3d(PointGeo const& point_geo, Point3d & result, Point3d & north, Point3d & east, Spheroid const& spheroid) | |
63 | { | |
64 | typedef typename coordinate_type<Point3d>::type calc_t; | |
65 | ||
66 | calc_t const c1 = 1; | |
67 | calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid); | |
68 | ||
69 | calc_t const lon = get_as_radian<0>(point_geo); | |
70 | calc_t const lat = get_as_radian<1>(point_geo); | |
71 | ||
72 | calc_t const sin_lon = sin(lon); | |
73 | calc_t const cos_lon = cos(lon); | |
74 | calc_t const sin_lat = sin(lat); | |
75 | calc_t const cos_lat = cos(lat); | |
76 | ||
77 | // "unit" spheroid, a = 1 | |
78 | calc_t const N = c1 / math::sqrt(c1 - e_sqr * math::sqr(sin_lat)); | |
79 | calc_t const N_cos_lat = N * cos_lat; | |
80 | ||
81 | set<0>(result, N_cos_lat * cos_lon); | |
82 | set<1>(result, N_cos_lat * sin_lon); | |
83 | set<2>(result, N * (c1 - e_sqr) * sin_lat); | |
84 | ||
85 | set<0>(east, -sin_lon); | |
86 | set<1>(east, cos_lon); | |
87 | set<2>(east, 0); | |
88 | ||
89 | set<0>(north, -sin_lat * cos_lon); | |
90 | set<1>(north, -sin_lat * sin_lon); | |
91 | set<2>(north, cos_lat); | |
92 | } | |
93 | ||
94 | template <typename PointGeo, typename Point3d, typename Spheroid> | |
95 | inline PointGeo cart3d_to_geo(Point3d const& point_3d, Spheroid const& spheroid) | |
96 | { | |
97 | typedef typename coordinate_type<PointGeo>::type coord_t; | |
98 | typedef typename coordinate_type<Point3d>::type calc_t; | |
99 | ||
100 | calc_t const c1 = 1; | |
101 | //calc_t const c2 = 2; | |
102 | calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid); | |
103 | ||
104 | calc_t const x = get<0>(point_3d); | |
105 | calc_t const y = get<1>(point_3d); | |
106 | calc_t const z = get<2>(point_3d); | |
107 | calc_t const xy_l = math::sqrt(math::sqr(x) + math::sqr(y)); | |
108 | ||
109 | calc_t const lonr = atan2(y, x); | |
110 | ||
111 | // NOTE: Alternative version | |
112 | // http://www.iag-aig.org/attach/989c8e501d9c5b5e2736955baf2632f5/V60N2_5FT.pdf | |
113 | // calc_t const lonr = c2 * atan2(y, x + xy_l); | |
114 | ||
115 | calc_t const latr = atan2(z, (c1 - e_sqr) * xy_l); | |
116 | ||
117 | // NOTE: If h is equal to 0 then there is no need to improve value of latitude | |
118 | // because then N_i / (N_i + h_i) = 1 | |
119 | // http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion | |
120 | ||
121 | PointGeo res; | |
122 | ||
123 | set_from_radian<0>(res, lonr); | |
124 | set_from_radian<1>(res, latr); | |
125 | ||
126 | coord_t lon = get<0>(res); | |
127 | coord_t lat = get<1>(res); | |
128 | ||
129 | math::normalize_spheroidal_coordinates | |
130 | < | |
131 | typename coordinate_system<PointGeo>::type::units, | |
132 | coord_t | |
133 | >(lon, lat); | |
134 | ||
135 | set<0>(res, lon); | |
136 | set<1>(res, lat); | |
137 | ||
138 | return res; | |
139 | } | |
140 | ||
141 | template <typename Point3d, typename Spheroid> | |
142 | inline Point3d projected_to_xy(Point3d const& point_3d, Spheroid const& spheroid) | |
143 | { | |
144 | typedef typename coordinate_type<Point3d>::type coord_t; | |
145 | ||
146 | // len_xy = sqrt(x^2 + y^2) | |
147 | // r = len_xy - |z / tan(lat)| | |
148 | // assuming h = 0 | |
149 | // lat = atan2(z, (1 - e^2) * len_xy); | |
150 | // |z / tan(lat)| = (1 - e^2) * len_xy | |
151 | // r = e^2 * len_xy | |
152 | // x_res = r * cos(lon) = e^2 * len_xy * x / len_xy = e^2 * x | |
153 | // y_res = r * sin(lon) = e^2 * len_xy * y / len_xy = e^2 * y | |
154 | ||
155 | coord_t const c0 = 0; | |
156 | coord_t const e_sqr = formula::eccentricity_sqr<coord_t>(spheroid); | |
157 | ||
158 | Point3d res; | |
159 | ||
160 | set<0>(res, e_sqr * get<0>(point_3d)); | |
161 | set<1>(res, e_sqr * get<1>(point_3d)); | |
162 | set<2>(res, c0); | |
163 | ||
164 | return res; | |
165 | } | |
166 | ||
167 | template <typename Point3d, typename Spheroid> | |
168 | inline Point3d projected_to_surface(Point3d const& direction, Spheroid const& spheroid) | |
169 | { | |
170 | typedef typename coordinate_type<Point3d>::type coord_t; | |
171 | ||
172 | //coord_t const c0 = 0; | |
173 | coord_t const c2 = 2; | |
174 | coord_t const c4 = 4; | |
175 | ||
176 | // calculate the point of intersection of a ray and spheroid's surface | |
177 | // the origin is the origin of the coordinate system | |
178 | //(x*x+y*y)/(a*a) + z*z/(b*b) = 1 | |
179 | // x = d.x * t | |
180 | // y = d.y * t | |
181 | // z = d.z * t | |
182 | coord_t const dx = get<0>(direction); | |
183 | coord_t const dy = get<1>(direction); | |
184 | coord_t const dz = get<2>(direction); | |
185 | ||
186 | //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid)); | |
187 | //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid)); | |
188 | // "unit" spheroid, a = 1 | |
189 | coord_t const a_sqr = 1; | |
190 | coord_t const b_sqr = math::sqr(formula::unit_spheroid_b<coord_t>(spheroid)); | |
191 | ||
192 | coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr; | |
193 | coord_t const delta = c4 * param_a; | |
194 | // delta >= 0 | |
195 | coord_t const t = math::sqrt(delta) / (c2 * param_a); | |
196 | ||
197 | // result = direction * t | |
198 | Point3d result = direction; | |
199 | multiply_value(result, t); | |
200 | ||
201 | return result; | |
202 | } | |
203 | ||
204 | template <typename Point3d, typename Spheroid> | |
1e59de90 TL |
205 | inline bool projected_to_surface(Point3d const& origin, Point3d const& direction, |
206 | Point3d & result1, Point3d & result2, | |
207 | Spheroid const& spheroid) | |
b32b8144 FG |
208 | { |
209 | typedef typename coordinate_type<Point3d>::type coord_t; | |
210 | ||
211 | coord_t const c0 = 0; | |
212 | coord_t const c1 = 1; | |
213 | coord_t const c2 = 2; | |
214 | coord_t const c4 = 4; | |
215 | ||
216 | // calculate the point of intersection of a ray and spheroid's surface | |
217 | //(x*x+y*y)/(a*a) + z*z/(b*b) = 1 | |
218 | // x = o.x + d.x * t | |
219 | // y = o.y + d.y * t | |
220 | // z = o.z + d.z * t | |
221 | coord_t const ox = get<0>(origin); | |
222 | coord_t const oy = get<1>(origin); | |
223 | coord_t const oz = get<2>(origin); | |
224 | coord_t const dx = get<0>(direction); | |
225 | coord_t const dy = get<1>(direction); | |
226 | coord_t const dz = get<2>(direction); | |
227 | ||
228 | //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid)); | |
229 | //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid)); | |
230 | // "unit" spheroid, a = 1 | |
231 | coord_t const a_sqr = 1; | |
232 | coord_t const b_sqr = math::sqr(formula::unit_spheroid_b<coord_t>(spheroid)); | |
233 | ||
234 | coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr; | |
235 | coord_t const param_b = c2 * ((ox*dx + oy*dy) / a_sqr + oz*dz / b_sqr); | |
236 | coord_t const param_c = (ox*ox + oy*oy) / a_sqr + oz*oz / b_sqr - c1; | |
237 | ||
238 | coord_t const delta = math::sqr(param_b) - c4 * param_a*param_c; | |
239 | ||
240 | // equals() ? | |
241 | if (delta < c0 || param_a == 0) | |
242 | { | |
243 | return false; | |
244 | } | |
245 | ||
246 | // result = origin + direction * t | |
247 | ||
248 | coord_t const sqrt_delta = math::sqrt(delta); | |
249 | coord_t const two_a = c2 * param_a; | |
250 | ||
251 | coord_t const t1 = (-param_b + sqrt_delta) / two_a; | |
b32b8144 | 252 | coord_t const t2 = (-param_b - sqrt_delta) / two_a; |
1e59de90 TL |
253 | geometry::detail::for_each_dimension<Point3d>([&](auto index) |
254 | { | |
255 | set<index>(result1, get<index>(origin) + get<index>(direction) * t1); | |
256 | set<index>(result2, get<index>(origin) + get<index>(direction) * t2); | |
257 | }); | |
b32b8144 FG |
258 | |
259 | return true; | |
260 | } | |
261 | ||
262 | template <typename Point3d, typename Spheroid> | |
263 | inline bool great_elliptic_intersection(Point3d const& a1, Point3d const& a2, | |
264 | Point3d const& b1, Point3d const& b2, | |
265 | Point3d & result, | |
266 | Spheroid const& spheroid) | |
267 | { | |
268 | typedef typename coordinate_type<Point3d>::type coord_t; | |
269 | ||
270 | coord_t c0 = 0; | |
271 | coord_t c1 = 1; | |
272 | ||
273 | Point3d n1 = cross_product(a1, a2); | |
274 | Point3d n2 = cross_product(b1, b2); | |
275 | ||
276 | // intersection direction | |
277 | Point3d id = cross_product(n1, n2); | |
278 | coord_t id_len_sqr = dot_product(id, id); | |
279 | ||
280 | if (math::equals(id_len_sqr, c0)) | |
281 | { | |
282 | return false; | |
283 | } | |
284 | ||
285 | // no need to normalize a1 and a2 because the intersection point on | |
286 | // the opposite side of the globe is at the same distance from the origin | |
287 | coord_t cos_a1i = dot_product(a1, id); | |
288 | coord_t cos_a2i = dot_product(a2, id); | |
289 | coord_t gri = math::detail::greatest(cos_a1i, cos_a2i); | |
290 | Point3d neg_id = id; | |
291 | multiply_value(neg_id, -c1); | |
292 | coord_t cos_a1ni = dot_product(a1, neg_id); | |
293 | coord_t cos_a2ni = dot_product(a2, neg_id); | |
294 | coord_t grni = math::detail::greatest(cos_a1ni, cos_a2ni); | |
295 | ||
296 | if (gri >= grni) | |
297 | { | |
298 | result = projected_to_surface(id, spheroid); | |
299 | } | |
300 | else | |
301 | { | |
302 | result = projected_to_surface(neg_id, spheroid); | |
303 | } | |
304 | ||
305 | return true; | |
306 | } | |
307 | ||
308 | template <typename Point3d1, typename Point3d2> | |
309 | static inline int elliptic_side_value(Point3d1 const& origin, Point3d1 const& norm, Point3d2 const& pt) | |
310 | { | |
311 | typedef typename coordinate_type<Point3d1>::type calc_t; | |
312 | calc_t c0 = 0; | |
313 | ||
314 | // vector oposite to pt - origin | |
315 | // only for the purpose of assigning origin | |
316 | Point3d1 vec = origin; | |
317 | subtract_point(vec, pt); | |
318 | ||
319 | calc_t d = dot_product(norm, vec); | |
320 | ||
321 | // since the vector is opposite the signs are opposite | |
322 | return math::equals(d, c0) ? 0 | |
323 | : d < c0 ? 1 | |
324 | : -1; // d > 0 | |
325 | } | |
326 | ||
327 | template <typename Point3d, typename Spheroid> | |
328 | inline bool planes_spheroid_intersection(Point3d const& o1, Point3d const& n1, | |
329 | Point3d const& o2, Point3d const& n2, | |
330 | Point3d & ip1, Point3d & ip2, | |
331 | Spheroid const& spheroid) | |
332 | { | |
333 | typedef typename coordinate_type<Point3d>::type coord_t; | |
334 | ||
335 | coord_t c0 = 0; | |
336 | coord_t c1 = 1; | |
337 | ||
338 | // Below | |
339 | // n . (p - o) = 0 | |
340 | // n . p - n . o = 0 | |
341 | // n . p + d = 0 | |
342 | // n . p = h | |
343 | ||
344 | // intersection direction | |
345 | Point3d id = cross_product(n1, n2); | |
346 | ||
347 | if (math::equals(dot_product(id, id), c0)) | |
348 | { | |
349 | return false; | |
350 | } | |
351 | ||
352 | coord_t dot_n1_n2 = dot_product(n1, n2); | |
353 | coord_t dot_n1_n2_sqr = math::sqr(dot_n1_n2); | |
354 | ||
355 | coord_t h1 = dot_product(n1, o1); | |
356 | coord_t h2 = dot_product(n2, o2); | |
357 | ||
358 | coord_t denom = c1 - dot_n1_n2_sqr; | |
359 | coord_t C1 = (h1 - h2 * dot_n1_n2) / denom; | |
360 | coord_t C2 = (h2 - h1 * dot_n1_n2) / denom; | |
361 | ||
362 | // C1 * n1 + C2 * n2 | |
1e59de90 TL |
363 | Point3d io; |
364 | geometry::detail::for_each_dimension<Point3d>([&](auto index) | |
365 | { | |
366 | set<index>(io, C1 * get<index>(n1) + C2 * get<index>(n2)); | |
367 | }); | |
b32b8144 FG |
368 | |
369 | if (! projected_to_surface(io, id, ip1, ip2, spheroid)) | |
370 | { | |
371 | return false; | |
372 | } | |
373 | ||
374 | return true; | |
375 | } | |
376 | ||
377 | ||
378 | template <typename Point3d, typename Spheroid> | |
379 | inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2, | |
380 | Point3d & v1, Point3d & v2, | |
381 | Point3d & origin, Point3d & normal, | |
382 | Spheroid const& spheroid) | |
383 | { | |
384 | typedef typename coordinate_type<Point3d>::type coord_t; | |
385 | ||
386 | Point3d xy1 = projected_to_xy(p1, spheroid); | |
387 | Point3d xy2 = projected_to_xy(p2, spheroid); | |
388 | ||
389 | // origin = (xy1 + xy2) / 2 | |
b32b8144 | 390 | // v1 = p1 - origin |
b32b8144 | 391 | // v2 = p2 - origin |
1e59de90 TL |
392 | coord_t const half = coord_t(0.5); |
393 | geometry::detail::for_each_dimension<Point3d>([&](auto index) | |
394 | { | |
395 | coord_t const o = (get<index>(xy1) + get<index>(xy2)) * half; | |
396 | set<index>(origin, o); | |
397 | set<index>(v1, get<index>(p1) - o); | |
398 | set<index>(v2, get<index>(p1) - o); | |
399 | }); | |
b32b8144 FG |
400 | |
401 | normal = cross_product(v1, v2); | |
402 | } | |
403 | ||
404 | template <typename Point3d, typename Spheroid> | |
405 | inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2, | |
406 | Point3d & origin, Point3d & normal, | |
407 | Spheroid const& spheroid) | |
408 | { | |
409 | Point3d v1, v2; | |
410 | experimental_elliptic_plane(p1, p2, v1, v2, origin, normal, spheroid); | |
411 | } | |
412 | ||
413 | template <typename Point3d, typename Spheroid> | |
414 | inline bool experimental_elliptic_intersection(Point3d const& a1, Point3d const& a2, | |
415 | Point3d const& b1, Point3d const& b2, | |
416 | Point3d & result, | |
417 | Spheroid const& spheroid) | |
418 | { | |
419 | typedef typename coordinate_type<Point3d>::type coord_t; | |
420 | ||
421 | coord_t c0 = 0; | |
422 | coord_t c1 = 1; | |
423 | ||
424 | Point3d a1v, a2v, o1, n1; | |
425 | experimental_elliptic_plane(a1, a2, a1v, a2v, o1, n1, spheroid); | |
426 | Point3d b1v, b2v, o2, n2; | |
427 | experimental_elliptic_plane(b1, b2, b1v, b2v, o2, n2, spheroid); | |
428 | ||
1e59de90 | 429 | if (! geometry::detail::vec_normalize(n1) || ! geometry::detail::vec_normalize(n2)) |
b32b8144 FG |
430 | { |
431 | return false; | |
432 | } | |
433 | ||
434 | Point3d ip1_s, ip2_s; | |
435 | if (! planes_spheroid_intersection(o1, n1, o2, n2, ip1_s, ip2_s, spheroid)) | |
436 | { | |
437 | return false; | |
438 | } | |
439 | ||
440 | // NOTE: simplified test, may not work in all cases | |
441 | coord_t dot_a1i1 = dot_product(a1, ip1_s); | |
442 | coord_t dot_a2i1 = dot_product(a2, ip1_s); | |
443 | coord_t gri1 = math::detail::greatest(dot_a1i1, dot_a2i1); | |
444 | coord_t dot_a1i2 = dot_product(a1, ip2_s); | |
445 | coord_t dot_a2i2 = dot_product(a2, ip2_s); | |
446 | coord_t gri2 = math::detail::greatest(dot_a1i2, dot_a2i2); | |
447 | ||
448 | result = gri1 >= gri2 ? ip1_s : ip2_s; | |
449 | ||
450 | return true; | |
451 | } | |
452 | ||
453 | } // namespace formula | |
454 | ||
455 | }} // namespace boost::geometry | |
456 | ||
457 | #endif // BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP |