geometry/formulas/andoyer_inverse.hpp

0001 // Boost.Geometry
0002
0003 // Copyright (c) 2018-2023 Adam Wulkiewicz, Lodz, Poland.
0004
0005 // Copyright (c) 2015-2020 Oracle and/or its affiliates.
0006 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
0007
0008 // Use, modification and distribution is subject to the Boost Software License,
0009 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
0010 // http://www.boost.org/LICENSE_1_0.txt)
0011
0012 #ifndef BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP
0013 #define BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP
0014
0015
0016 #include <boost/math/constants/constants.hpp>
0017
0018 #include <boost/geometry/core/radius.hpp>
0019
0020 #include <boost/geometry/util/constexpr.hpp>
0021 #include <boost/geometry/util/math.hpp>
0022
0023 #include <boost/geometry/formulas/differential_quantities.hpp>
0024 #include <boost/geometry/formulas/flattening.hpp>
0025 #include <boost/geometry/formulas/result_inverse.hpp>
0026
0027
0028 namespace boost { namespace geometry { namespace formula
0029 {
0030
0031 /*!
0032 \brief The solution of the inverse problem of geodesics on latlong coordinates,
0033        Forsyth-Andoyer-Lambert type approximation with first order terms.
0034 \author See
0035     - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
0036       http://www.dtic.mil/docs/citations/AD0627893
0037     - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
0038       http://www.dtic.mil/docs/citations/AD703541
0039 */
0040 template <
0041     typename CT,
0042     bool EnableDistance,
0043     bool EnableAzimuth,
0044     bool EnableReverseAzimuth = false,
0045     bool EnableReducedLength = false,
0046     bool EnableGeodesicScale = false
0047 >
0048 class andoyer_inverse
0049 {
0050     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
0051     static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
0052     static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
0053     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
0054
0055 public:
0056     typedef result_inverse<CT> result_type;
0057
0058     template <typename T1, typename T2, typename Spheroid>
0059     static inline result_type apply(T1 const& lon1,
0060                                     T1 const& lat1,
0061                                     T2 const& lon2,
0062                                     T2 const& lat2,
0063                                     Spheroid const& spheroid)
0064     {
0065         result_type result;
0066
0067         // coordinates in radians
0068
0069         if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
0070         {
0071             return result;
0072         }
0073
0074         CT const c0 = CT(0);
0075         CT const c1 = CT(1);
0076         CT const pi = math::pi<CT>();
0077         CT const f = formula::flattening<CT>(spheroid);
0078
0079         CT const dlon = lon2 - lon1;
0080         CT const sin_dlon = sin(dlon);
0081         CT const cos_dlon = cos(dlon);
0082         CT const sin_lat1 = sin(lat1);
0083         CT const cos_lat1 = cos(lat1);
0084         CT const sin_lat2 = sin(lat2);
0085         CT const cos_lat2 = cos(lat2);
0086
0087         // H,G,T = infinity if cos_d = 1 or cos_d = -1
0088         // lat1 == +-90 && lat2 == +-90
0089         // lat1 == lat2 && lon1 == lon2
0090         CT cos_d = sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon;
0091         // on some platforms cos_d may be outside valid range
0092         if (cos_d < -c1)
0093             cos_d = -c1;
0094         else if (cos_d > c1)
0095             cos_d = c1;
0096
0097         CT const d = acos(cos_d); // [0, pi]
0098         CT const sin_d = sin(d);  // [-1, 1]
0099
0100         if BOOST_GEOMETRY_CONSTEXPR (EnableDistance)
0101         {
0102             CT const K = math::sqr(sin_lat1-sin_lat2);
0103             CT const L = math::sqr(sin_lat1+sin_lat2);
0104             CT const three_sin_d = CT(3) * sin_d;
0105
0106             CT const one_minus_cos_d = c1 - cos_d;
0107             CT const one_plus_cos_d = c1 + cos_d;
0108             // cos_d = 1 means that the points are very close
0109             // cos_d = -1 means that the points are antipodal
0110
0111             CT const H = math::equals(one_minus_cos_d, c0) ?
0112                             c0 :
0113                             (d + three_sin_d) / one_minus_cos_d;
0114             CT const G = math::equals(one_plus_cos_d, c0) ?
0115                             c0 :
0116                             (d - three_sin_d) / one_plus_cos_d;
0117
0118             CT const dd = -(f/CT(4))*(H*K+G*L);
0119
0120             CT const a = CT(get_radius<0>(spheroid));
0121
0122             result.distance = a * (d + dd);
0123         }
0124
0125         if BOOST_GEOMETRY_CONSTEXPR (CalcAzimuths)
0126         {
0127             // sin_d = 0 <=> antipodal points (incl. poles) or very close
0128             if (math::equals(sin_d, c0))
0129             {
0130                 // T = inf
0131                 // dA = inf
0132                 // azimuth = -inf
0133
0134                 // TODO: The following azimuths are inconsistent with distance
0135                 // i.e. according to azimuths below a segment with antipodal endpoints
0136                 // travels through the north pole, however the distance returned above
0137                 // is the length of a segment traveling along the equator.
0138                 // Furthermore, this special case handling is only done in andoyer
0139                 // formula.
0140                 // The most correct way of fixing it is to handle antipodal regions
0141                 // correctly and consistently across all formulas.
0142
0143                 // points very close
0144                 if (cos_d >= c0)
0145                 {
0146                     result.azimuth = c0;
0147                     result.reverse_azimuth = c0;
0148                 }
0149                 // antipodal points
0150                 else
0151                 {
0152                     // Set azimuth to 0 unless the first endpoint is the north pole
0153                     if (! math::equals(sin_lat1, c1))
0154                     {
0155                         result.azimuth = c0;
0156                         result.reverse_azimuth = pi;
0157                     }
0158                     else
0159                     {
0160                         result.azimuth = pi;
0161                         result.reverse_azimuth = c0;
0162                     }
0163                 }
0164             }
0165             else
0166             {
0167                 CT const c2 = CT(2);
0168
0169                 CT A = c0;
0170                 CT U = c0;
0171                 if (math::equals(cos_lat2, c0))
0172                 {
0173                     if (sin_lat2 < c0)
0174                     {
0175                         A = pi;
0176                     }
0177                 }
0178                 else
0179                 {
0180                     CT const tan_lat2 = sin_lat2/cos_lat2;
0181                     CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon;
0182                     A = atan2(sin_dlon, M);
0183                     CT const sin_2A = sin(c2*A);
0184                     U = (f/ c2)*math::sqr(cos_lat1)*sin_2A;
0185                 }
0186
0187                 CT B = c0;
0188                 CT V = c0;
0189                 if (math::equals(cos_lat1, c0))
0190                 {
0191                     if (sin_lat1 < c0)
0192                     {
0193                         B = pi;
0194                     }
0195                 }
0196                 else
0197                 {
0198                     CT const tan_lat1 = sin_lat1/cos_lat1;
0199                     CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon;
0200                     B = atan2(sin_dlon, N);
0201                     CT const sin_2B = sin(c2*B);
0202                     V = (f/ c2)*math::sqr(cos_lat2)*sin_2B;
0203                 }
0204
0205                 CT const T = d / sin_d;
0206
0207                 // even with sin_d == 0 checked above if the second point
0208                 // is somewhere in the antipodal area T may still be great
0209                 // therefore dA and dB may be great and the resulting azimuths
0210                 // may be some more or less arbitrary angles
0211
0212                 if BOOST_GEOMETRY_CONSTEXPR (CalcFwdAzimuth)
0213                 {
0214                     CT const dA = V*T - U;
0215                     result.azimuth = A - dA;
0216                     normalize_azimuth(result.azimuth, A, dA);
0217                 }
0218
0219                 if BOOST_GEOMETRY_CONSTEXPR (CalcRevAzimuth)
0220                 {
0221                     CT const dB = -U*T + V;
0222                     if (B >= 0)
0223                         result.reverse_azimuth = pi - B - dB;
0224                     else
0225                         result.reverse_azimuth = -pi - B - dB;
0226                     normalize_azimuth(result.reverse_azimuth, B, dB);
0227                 }
0228             }
0229         }
0230
0231         if BOOST_GEOMETRY_CONSTEXPR (CalcQuantities)
0232         {
0233             CT const b = CT(get_radius<2>(spheroid));
0234
0235             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 1> quantities;
0236             quantities::apply(dlon, sin_lat1, cos_lat1, sin_lat2, cos_lat2,
0237                               result.azimuth, result.reverse_azimuth,
0238                               b, f,
0239                               result.reduced_length, result.geodesic_scale);
0240         }
0241
0242         return result;
0243     }
0244
0245 private:
0246     static inline void normalize_azimuth(CT & azimuth, CT const& A, CT const& dA)
0247     {
0248         CT const c0 = 0;
0249
0250         if (A >= c0) // A indicates Eastern hemisphere
0251         {
0252             if (dA >= c0) // A altered towards 0
0253             {
0254                 if (azimuth < c0)
0255                 {
0256                     azimuth = c0;
0257                 }
0258             }
0259             else // dA < 0, A altered towards pi
0260             {
0261                 CT const pi = math::pi<CT>();
0262                 if (azimuth > pi)
0263                 {
0264                     azimuth = pi;
0265                 }
0266             }
0267         }
0268         else // A indicates Western hemisphere
0269         {
0270             if (dA <= c0) // A altered towards 0
0271             {
0272                 if (azimuth > c0)
0273                 {
0274                     azimuth = c0;
0275                 }
0276             }
0277             else // dA > 0, A altered towards -pi
0278             {
0279                 CT const minus_pi = -math::pi<CT>();
0280                 if (azimuth < minus_pi)
0281                 {
0282                     azimuth = minus_pi;
0283                 }
0284             }
0285         }
0286     }
0287 };
0288
0289 }}} // namespace boost::geometry::formula
0290
0291
0292 #endif // BOOST_GEOMETRY_FORMULAS_ANDOYER_INVERSE_HPP