geometry/formulas/geographic.hpp

0001 // Boost.Geometry
0002
0003 // Copyright (c) 2016-2021, Oracle and/or its affiliates.
0004 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
0005
0006 // Use, modification and distribution is subject to the Boost Software License,
0007 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
0008 // http://www.boost.org/LICENSE_1_0.txt)
0009
0010 #ifndef BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP
0011 #define BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP
0012
0013 #include <boost/geometry/core/coordinate_system.hpp>
0014 #include <boost/geometry/core/coordinate_type.hpp>
0015 #include <boost/geometry/core/access.hpp>
0016 #include <boost/geometry/core/radian_access.hpp>
0017
0018 #include <boost/geometry/arithmetic/arithmetic.hpp>
0019 #include <boost/geometry/arithmetic/cross_product.hpp>
0020 #include <boost/geometry/arithmetic/dot_product.hpp>
0021 #include <boost/geometry/arithmetic/normalize.hpp>
0022
0023 #include <boost/geometry/formulas/eccentricity_sqr.hpp>
0024 #include <boost/geometry/formulas/flattening.hpp>
0025 #include <boost/geometry/formulas/unit_spheroid.hpp>
0026
0027 #include <boost/geometry/util/math.hpp>
0028 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
0029 #include <boost/geometry/util/select_coordinate_type.hpp>
0030
0031 namespace boost { namespace geometry {
0032
0033 namespace formula {
0034
0035 template <typename Point3d, typename PointGeo, typename Spheroid>
0036 inline Point3d geo_to_cart3d(PointGeo const& point_geo, Spheroid const& spheroid)
0037 {
0038     typedef typename coordinate_type<Point3d>::type calc_t;
0039
0040     calc_t const c1 = 1;
0041     calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid);
0042
0043     calc_t const lon = get_as_radian<0>(point_geo);
0044     calc_t const lat = get_as_radian<1>(point_geo);
0045
0046     Point3d res;
0047
0048     calc_t const sin_lat = sin(lat);
0049
0050     // "unit" spheroid, a = 1
0051     calc_t const N = c1 / math::sqrt(c1 - e_sqr * math::sqr(sin_lat));
0052     calc_t const N_cos_lat = N * cos(lat);
0053
0054     set<0>(res, N_cos_lat * cos(lon));
0055     set<1>(res, N_cos_lat * sin(lon));
0056     set<2>(res, N * (c1 - e_sqr) * sin_lat);
0057
0058     return res;
0059 }
0060
0061 template <typename PointGeo, typename Spheroid, typename Point3d>
0062 inline void geo_to_cart3d(PointGeo const& point_geo, Point3d & result, Point3d & north, Point3d & east, Spheroid const& spheroid)
0063 {
0064     typedef typename coordinate_type<Point3d>::type calc_t;
0065
0066     calc_t const c1 = 1;
0067     calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid);
0068
0069     calc_t const lon = get_as_radian<0>(point_geo);
0070     calc_t const lat = get_as_radian<1>(point_geo);
0071
0072     calc_t const sin_lon = sin(lon);
0073     calc_t const cos_lon = cos(lon);
0074     calc_t const sin_lat = sin(lat);
0075     calc_t const cos_lat = cos(lat);
0076
0077     // "unit" spheroid, a = 1
0078     calc_t const N = c1 / math::sqrt(c1 - e_sqr * math::sqr(sin_lat));
0079     calc_t const N_cos_lat = N * cos_lat;
0080
0081     set<0>(result, N_cos_lat * cos_lon);
0082     set<1>(result, N_cos_lat * sin_lon);
0083     set<2>(result, N * (c1 - e_sqr) * sin_lat);
0084
0085     set<0>(east, -sin_lon);
0086     set<1>(east, cos_lon);
0087     set<2>(east, 0);
0088
0089     set<0>(north, -sin_lat * cos_lon);
0090     set<1>(north, -sin_lat * sin_lon);
0091     set<2>(north, cos_lat);
0092 }
0093
0094 template <typename PointGeo, typename Point3d, typename Spheroid>
0095 inline PointGeo cart3d_to_geo(Point3d const& point_3d, Spheroid const& spheroid)
0096 {
0097     typedef typename coordinate_type<PointGeo>::type coord_t;
0098     typedef typename coordinate_type<Point3d>::type calc_t;
0099
0100     calc_t const c1 = 1;
0101     //calc_t const c2 = 2;
0102     calc_t const e_sqr = eccentricity_sqr<calc_t>(spheroid);
0103
0104     calc_t const x = get<0>(point_3d);
0105     calc_t const y = get<1>(point_3d);
0106     calc_t const z = get<2>(point_3d);
0107     calc_t const xy_l = math::sqrt(math::sqr(x) + math::sqr(y));
0108
0109     calc_t const lonr = atan2(y, x);
0110
0111     // NOTE: Alternative version
0112     // http://www.iag-aig.org/attach/989c8e501d9c5b5e2736955baf2632f5/V60N2_5FT.pdf
0113     // calc_t const lonr = c2 * atan2(y, x + xy_l);
0114
0115     calc_t const latr = atan2(z, (c1 - e_sqr) * xy_l);
0116
0117     // NOTE: If h is equal to 0 then there is no need to improve value of latitude
0118     //       because then N_i / (N_i + h_i) = 1
0119     // http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
0120
0121     PointGeo res;
0122
0123     set_from_radian<0>(res, lonr);
0124     set_from_radian<1>(res, latr);
0125
0126     coord_t lon = get<0>(res);
0127     coord_t lat = get<1>(res);
0128
0129     math::normalize_spheroidal_coordinates
0130         <
0131             typename coordinate_system<PointGeo>::type::units,
0132             coord_t
0133         >(lon, lat);
0134
0135     set<0>(res, lon);
0136     set<1>(res, lat);
0137
0138     return res;
0139 }
0140
0141 template <typename Point3d, typename Spheroid>
0142 inline Point3d projected_to_xy(Point3d const& point_3d, Spheroid const& spheroid)
0143 {
0144     typedef typename coordinate_type<Point3d>::type coord_t;
0145
0146     // len_xy = sqrt(x^2 + y^2)
0147     // r = len_xy - |z / tan(lat)|
0148     // assuming h = 0
0149     // lat = atan2(z, (1 - e^2) * len_xy);
0150     // |z / tan(lat)| = (1 - e^2) * len_xy
0151     // r = e^2 * len_xy
0152     // x_res = r * cos(lon) = e^2 * len_xy * x / len_xy = e^2 * x
0153     // y_res = r * sin(lon) = e^2 * len_xy * y / len_xy = e^2 * y
0154
0155     coord_t const c0 = 0;
0156     coord_t const e_sqr = formula::eccentricity_sqr<coord_t>(spheroid);
0157
0158     Point3d res;
0159
0160     set<0>(res, e_sqr * get<0>(point_3d));
0161     set<1>(res, e_sqr * get<1>(point_3d));
0162     set<2>(res, c0);
0163
0164     return res;
0165 }
0166
0167 template <typename Point3d, typename Spheroid>
0168 inline Point3d projected_to_surface(Point3d const& direction, Spheroid const& spheroid)
0169 {
0170     typedef typename coordinate_type<Point3d>::type coord_t;
0171
0172     //coord_t const c0 = 0;
0173     coord_t const c2 = 2;
0174     coord_t const c4 = 4;
0175
0176     // calculate the point of intersection of a ray and spheroid's surface
0177     // the origin is the origin of the coordinate system
0178     //(x*x+y*y)/(a*a) + z*z/(b*b) = 1
0179     // x = d.x * t
0180     // y = d.y * t
0181     // z = d.z * t
0182     coord_t const dx = get<0>(direction);
0183     coord_t const dy = get<1>(direction);
0184     coord_t const dz = get<2>(direction);
0185
0186     //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid));
0187     //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid));
0188     // "unit" spheroid, a = 1
0189     coord_t const a_sqr = 1;
0190     coord_t const b_sqr = math::sqr(formula::unit_spheroid_b<coord_t>(spheroid));
0191
0192     coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr;
0193     coord_t const delta = c4 * param_a;
0194     // delta >= 0
0195     coord_t const t = math::sqrt(delta) / (c2 * param_a);
0196
0197     // result = direction * t
0198     Point3d result = direction;
0199     multiply_value(result, t);
0200
0201     return result;
0202 }
0203
0204 template <typename Point3d, typename Spheroid>
0205 inline bool projected_to_surface(Point3d const& origin, Point3d const& direction,
0206                                  Point3d & result1, Point3d & result2,
0207                                  Spheroid const& spheroid)
0208 {
0209     typedef typename coordinate_type<Point3d>::type coord_t;
0210
0211     coord_t const c0 = 0;
0212     coord_t const c1 = 1;
0213     coord_t const c2 = 2;
0214     coord_t const c4 = 4;
0215
0216     // calculate the point of intersection of a ray and spheroid's surface
0217     //(x*x+y*y)/(a*a) + z*z/(b*b) = 1
0218     // x = o.x + d.x * t
0219     // y = o.y + d.y * t
0220     // z = o.z + d.z * t
0221     coord_t const ox = get<0>(origin);
0222     coord_t const oy = get<1>(origin);
0223     coord_t const oz = get<2>(origin);
0224     coord_t const dx = get<0>(direction);
0225     coord_t const dy = get<1>(direction);
0226     coord_t const dz = get<2>(direction);
0227
0228     //coord_t const a_sqr = math::sqr(get_radius<0>(spheroid));
0229     //coord_t const b_sqr = math::sqr(get_radius<2>(spheroid));
0230     // "unit" spheroid, a = 1
0231     coord_t const a_sqr = 1;
0232     coord_t const b_sqr = math::sqr(formula::unit_spheroid_b<coord_t>(spheroid));
0233
0234     coord_t const param_a = (dx*dx + dy*dy) / a_sqr + dz*dz / b_sqr;
0235     coord_t const param_b = c2 * ((ox*dx + oy*dy) / a_sqr + oz*dz / b_sqr);
0236     coord_t const param_c = (ox*ox + oy*oy) / a_sqr + oz*oz / b_sqr - c1;
0237
0238     coord_t const delta = math::sqr(param_b) - c4 * param_a*param_c;
0239
0240     // equals() ?
0241     if (delta < c0 || param_a == 0)
0242     {
0243         return false;
0244     }
0245
0246     // result = origin + direction * t
0247
0248     coord_t const sqrt_delta = math::sqrt(delta);
0249     coord_t const two_a = c2 * param_a;
0250
0251     coord_t const t1 = (-param_b + sqrt_delta) / two_a;
0252     coord_t const t2 = (-param_b - sqrt_delta) / two_a;
0253     geometry::detail::for_each_dimension<Point3d>([&](auto index)
0254     {
0255         set<index>(result1, get<index>(origin) + get<index>(direction) * t1);
0256         set<index>(result2, get<index>(origin) + get<index>(direction) * t2);
0257     });
0258
0259     return true;
0260 }
0261
0262 template <typename Point3d, typename Spheroid>
0263 inline bool great_elliptic_intersection(Point3d const& a1, Point3d const& a2,
0264                                         Point3d const& b1, Point3d const& b2,
0265                                         Point3d & result,
0266                                         Spheroid const& spheroid)
0267 {
0268     typedef typename coordinate_type<Point3d>::type coord_t;
0269
0270     coord_t c0 = 0;
0271     coord_t c1 = 1;
0272
0273     Point3d n1 = cross_product(a1, a2);
0274     Point3d n2 = cross_product(b1, b2);
0275
0276     // intersection direction
0277     Point3d id = cross_product(n1, n2);
0278     coord_t id_len_sqr = dot_product(id, id);
0279
0280     if (math::equals(id_len_sqr, c0))
0281     {
0282         return false;
0283     }
0284
0285     // no need to normalize a1 and a2 because the intersection point on
0286     // the opposite side of the globe is at the same distance from the origin
0287     coord_t cos_a1i = dot_product(a1, id);
0288     coord_t cos_a2i = dot_product(a2, id);
0289     coord_t gri = math::detail::greatest(cos_a1i, cos_a2i);
0290     Point3d neg_id = id;
0291     multiply_value(neg_id, -c1);
0292     coord_t cos_a1ni = dot_product(a1, neg_id);
0293     coord_t cos_a2ni = dot_product(a2, neg_id);
0294     coord_t grni = math::detail::greatest(cos_a1ni, cos_a2ni);
0295
0296     if (gri >= grni)
0297     {
0298         result = projected_to_surface(id, spheroid);
0299     }
0300     else
0301     {
0302         result = projected_to_surface(neg_id, spheroid);
0303     }
0304
0305     return true;
0306 }
0307
0308 template <typename Point3d1, typename Point3d2>
0309 static inline int elliptic_side_value(Point3d1 const& origin, Point3d1 const& norm, Point3d2 const& pt)
0310 {
0311     typedef typename coordinate_type<Point3d1>::type calc_t;
0312     calc_t c0 = 0;
0313
0314     // vector oposite to pt - origin
0315     // only for the purpose of assigning origin
0316     Point3d1 vec = origin;
0317     subtract_point(vec, pt);
0318
0319     calc_t d = dot_product(norm, vec);
0320
0321     // since the vector is opposite the signs are opposite
0322     return math::equals(d, c0) ? 0
0323         : d < c0 ? 1
0324         : -1; // d > 0
0325 }
0326
0327 template <typename Point3d, typename Spheroid>
0328 inline bool planes_spheroid_intersection(Point3d const& o1, Point3d const& n1,
0329                                          Point3d const& o2, Point3d const& n2,
0330                                          Point3d & ip1, Point3d & ip2,
0331                                          Spheroid const& spheroid)
0332 {
0333     typedef typename coordinate_type<Point3d>::type coord_t;
0334
0335     coord_t c0 = 0;
0336     coord_t c1 = 1;
0337
0338     // Below
0339     // n . (p - o) = 0
0340     // n . p - n . o = 0
0341     // n . p + d = 0
0342     // n . p = h
0343
0344     // intersection direction
0345     Point3d id = cross_product(n1, n2);
0346
0347     if (math::equals(dot_product(id, id), c0))
0348     {
0349         return false;
0350     }
0351
0352     coord_t dot_n1_n2 = dot_product(n1, n2);
0353     coord_t dot_n1_n2_sqr = math::sqr(dot_n1_n2);
0354
0355     coord_t h1 = dot_product(n1, o1);
0356     coord_t h2 = dot_product(n2, o2);
0357
0358     coord_t denom = c1 - dot_n1_n2_sqr;
0359     coord_t C1 = (h1 - h2 * dot_n1_n2) / denom;
0360     coord_t C2 = (h2 - h1 * dot_n1_n2) / denom;
0361
0362     // C1 * n1 + C2 * n2
0363     Point3d io;
0364     geometry::detail::for_each_dimension<Point3d>([&](auto index)
0365     {
0366         set<index>(io, C1 * get<index>(n1) + C2 * get<index>(n2));
0367     });
0368
0369     if (! projected_to_surface(io, id, ip1, ip2, spheroid))
0370     {
0371         return false;
0372     }
0373
0374     return true;
0375 }
0376
0377
0378 template <typename Point3d, typename Spheroid>
0379 inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2,
0380                                         Point3d & v1, Point3d & v2,
0381                                         Point3d & origin, Point3d & normal,
0382                                         Spheroid const& spheroid)
0383 {
0384     typedef typename coordinate_type<Point3d>::type coord_t;
0385
0386     Point3d xy1 = projected_to_xy(p1, spheroid);
0387     Point3d xy2 = projected_to_xy(p2, spheroid);
0388
0389     // origin = (xy1 + xy2) / 2
0390     // v1 = p1 - origin
0391     // v2 = p2 - origin
0392     coord_t const half = coord_t(0.5);
0393     geometry::detail::for_each_dimension<Point3d>([&](auto index)
0394     {
0395         coord_t const o = (get<index>(xy1) + get<index>(xy2)) * half;
0396         set<index>(origin, o);
0397         set<index>(v1, get<index>(p1) - o);
0398         set<index>(v2, get<index>(p1) - o);
0399     });
0400
0401     normal = cross_product(v1, v2);
0402 }
0403
0404 template <typename Point3d, typename Spheroid>
0405 inline void experimental_elliptic_plane(Point3d const& p1, Point3d const& p2,
0406                                         Point3d & origin, Point3d & normal,
0407                                         Spheroid const& spheroid)
0408 {
0409     Point3d v1, v2;
0410     experimental_elliptic_plane(p1, p2, v1, v2, origin, normal, spheroid);
0411 }
0412
0413 template <typename Point3d, typename Spheroid>
0414 inline bool experimental_elliptic_intersection(Point3d const& a1, Point3d const& a2,
0415                                                Point3d const& b1, Point3d const& b2,
0416                                                Point3d & result,
0417                                                Spheroid const& spheroid)
0418 {
0419     typedef typename coordinate_type<Point3d>::type coord_t;
0420
0421     coord_t c0 = 0;
0422     coord_t c1 = 1;
0423
0424     Point3d a1v, a2v, o1, n1;
0425     experimental_elliptic_plane(a1, a2, a1v, a2v, o1, n1, spheroid);
0426     Point3d b1v, b2v, o2, n2;
0427     experimental_elliptic_plane(b1, b2, b1v, b2v, o2, n2, spheroid);
0428
0429     if (! geometry::detail::vec_normalize(n1) || ! geometry::detail::vec_normalize(n2))
0430     {
0431         return false;
0432     }
0433
0434     Point3d ip1_s, ip2_s;
0435     if (! planes_spheroid_intersection(o1, n1, o2, n2, ip1_s, ip2_s, spheroid))
0436     {
0437         return false;
0438     }
0439
0440     // NOTE: simplified test, may not work in all cases
0441     coord_t dot_a1i1 = dot_product(a1, ip1_s);
0442     coord_t dot_a2i1 = dot_product(a2, ip1_s);
0443     coord_t gri1 = math::detail::greatest(dot_a1i1, dot_a2i1);
0444     coord_t dot_a1i2 = dot_product(a1, ip2_s);
0445     coord_t dot_a2i2 = dot_product(a2, ip2_s);
0446     coord_t gri2 = math::detail::greatest(dot_a1i2, dot_a2i2);
0447
0448     result = gri1 >= gri2 ? ip1_s : ip2_s;
0449
0450     return true;
0451 }
0452
0453 } // namespace formula
0454
0455 }} // namespace boost::geometry
0456
0457 #endif // BOOST_GEOMETRY_FORMULAS_GEOGRAPHIC_HPP