geometry/formulas/thomas_direct.hpp

0001 // Boost.Geometry
0002
0003 // Copyright (c) 2023 Adam Wulkiewicz, Lodz, Poland.
0004
0005 // Copyright (c) 2016-2020 Oracle and/or its affiliates.
0006 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
0007 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
0008
0009 // Use, modification and distribution is subject to the Boost Software License,
0010 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
0011 // http://www.boost.org/LICENSE_1_0.txt)
0012
0013 #ifndef BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP
0014 #define BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP
0015
0016
0017 #include <boost/math/constants/constants.hpp>
0018
0019 #include <boost/geometry/core/assert.hpp>
0020 #include <boost/geometry/core/radius.hpp>
0021
0022 #include <boost/geometry/util/constexpr.hpp>
0023 #include <boost/geometry/util/math.hpp>
0024 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
0025
0026 #include <boost/geometry/formulas/differential_quantities.hpp>
0027 #include <boost/geometry/formulas/flattening.hpp>
0028 #include <boost/geometry/formulas/result_direct.hpp>
0029
0030
0031 namespace boost { namespace geometry { namespace formula
0032 {
0033
0034
0035 /*!
0036 \brief The solution of the direct problem of geodesics on latlong coordinates,
0037        Forsyth-Andoyer-Lambert type approximation with first/second order terms.
0038 \author See
0039     - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
0040       http://www.dtic.mil/docs/citations/AD0627893
0041     - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
0042       http://www.dtic.mil/docs/citations/AD0703541
0043 */
0044 template <
0045     typename CT,
0046     bool SecondOrder = true,
0047     bool EnableCoordinates = true,
0048     bool EnableReverseAzimuth = false,
0049     bool EnableReducedLength = false,
0050     bool EnableGeodesicScale = false
0051 >
0052 class thomas_direct
0053 {
0054     static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
0055     static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
0056     static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcCoordinates || CalcQuantities;
0057
0058 public:
0059     typedef result_direct<CT> result_type;
0060
0061     template <typename T, typename Dist, typename Azi, typename Spheroid>
0062     static inline result_type apply(T const& lo1,
0063                                     T const& la1,
0064                                     Dist const& distance,
0065                                     Azi const& azimuth12,
0066                                     Spheroid const& spheroid)
0067     {
0068         result_type result;
0069
0070         CT const lon1 = lo1;
0071         CT const lat1 = la1;
0072
0073         CT const c0 = 0;
0074         CT const c1 = 1;
0075         CT const c2 = 2;
0076         CT const c4 = 4;
0077
0078         CT const a = CT(get_radius<0>(spheroid));
0079         CT const b = CT(get_radius<2>(spheroid));
0080         CT const f = formula::flattening<CT>(spheroid);
0081         CT const one_minus_f = c1 - f;
0082
0083         CT const pi = math::pi<CT>();
0084         CT const pi_half = pi / c2;
0085
0086         BOOST_GEOMETRY_ASSERT(-pi <= azimuth12 && azimuth12 <= pi);
0087
0088         // keep azimuth small - experiments show low accuracy
0089         // if the azimuth is closer to (+-)180 deg.
0090         CT azi12_alt = azimuth12;
0091         CT lat1_alt = lat1;
0092         bool alter_result = vflip_if_south(lat1, azimuth12, lat1_alt, azi12_alt);
0093
0094         CT const theta1 = math::equals(lat1_alt, pi_half) ? lat1_alt :
0095                           math::equals(lat1_alt, -pi_half) ? lat1_alt :
0096                           atan(one_minus_f * tan(lat1_alt));
0097         CT const sin_theta1 = sin(theta1);
0098         CT const cos_theta1 = cos(theta1);
0099
0100         CT const sin_a12 = sin(azi12_alt);
0101         CT const cos_a12 = cos(azi12_alt);
0102
0103         CT const M = cos_theta1 * sin_a12; // cos_theta0
0104         CT const theta0 = acos(M);
0105         CT const sin_theta0 = sin(theta0);
0106
0107         CT const N = cos_theta1 * cos_a12;
0108         CT const C1 = f * M; // lower-case c1 in the technical report
0109         CT const C2 = f * (c1 - math::sqr(M)) / c4; // lower-case c2 in the technical report
0110         CT D = 0;
0111         CT P = 0;
0112         if BOOST_GEOMETRY_CONSTEXPR (SecondOrder)
0113         {
0114             D = (c1 - C2) * (c1 - C2 - C1 * M);
0115             P = C2 * (c1 + C1 * M / c2) / D;
0116         }
0117         else
0118         {
0119             D = c1 - c2 * C2 - C1 * M;
0120             P = C2 / D;
0121         }
0122         // special case for equator:
0123         // sin_theta0 = 0 <=> lat1 = 0 ^ |azimuth12| = pi/2
0124         // NOTE: in this case it doesn't matter what's the value of cos_sigma1 because
0125         //       theta1=0, theta0=0, M=1|-1, C2=0 so X=0 and Y=0 so d_sigma=d
0126         //       cos_a12=0 so N=0, therefore
0127         //       lat2=0, azi21=pi/2|-pi/2
0128         //       d_eta = atan2(sin_d_sigma, cos_d_sigma)
0129         //       H = C1 * d_sigma
0130         CT const cos_sigma1 = math::equals(sin_theta0, c0)
0131                                 ? c1
0132                                 : normalized1_1(sin_theta1 / sin_theta0);
0133         CT const sigma1 = acos(cos_sigma1);
0134         CT const d = distance / (a * D);
0135         CT const u = 2 * (sigma1 - d);
0136         CT const cos_d = cos(d);
0137         CT const sin_d = sin(d);
0138         CT const cos_u = cos(u);
0139         CT const sin_u = sin(u);
0140
0141         CT const W = c1 - c2 * P * cos_u;
0142         CT const V = cos_u * cos_d - sin_u * sin_d;
0143         CT const Y = c2 * P * V * W * sin_d;
0144         CT X = 0;
0145         CT d_sigma = d - Y;
0146         if BOOST_GEOMETRY_CONSTEXPR (SecondOrder)
0147         {
0148             X = math::sqr(C2) * sin_d * cos_d * (2 * math::sqr(V) - c1);
0149             d_sigma += X;
0150         }
0151         CT const sin_d_sigma = sin(d_sigma);
0152         CT const cos_d_sigma = cos(d_sigma);
0153
0154         if BOOST_GEOMETRY_CONSTEXPR (CalcRevAzimuth)
0155         {
0156             result.reverse_azimuth = atan2(M, N * cos_d_sigma - sin_theta1 * sin_d_sigma);
0157
0158             if (alter_result)
0159             {
0160                 vflip_rev_azi(result.reverse_azimuth, azimuth12);
0161             }
0162         }
0163
0164         if BOOST_GEOMETRY_CONSTEXPR (CalcCoordinates)
0165         {
0166             CT const S_sigma = c2 * sigma1 - d_sigma;
0167             CT cos_S_sigma = 0;
0168             CT H = C1 * d_sigma;
0169             if BOOST_GEOMETRY_CONSTEXPR (SecondOrder)
0170             {
0171                 cos_S_sigma = cos(S_sigma);
0172                 H = H * (c1 - C2) - C1 * C2 * sin_d_sigma * cos_S_sigma;
0173             }
0174             CT const d_eta = atan2(sin_d_sigma * sin_a12, cos_theta1 * cos_d_sigma - sin_theta1 * sin_d_sigma * cos_a12);
0175             CT const d_lambda = d_eta - H;
0176
0177             result.lon2 = lon1 + d_lambda;
0178
0179             if (! math::equals(M, c0))
0180             {
0181                 CT const sin_a21 = sin(result.reverse_azimuth);
0182                 CT const tan_theta2 = (sin_theta1 * cos_d_sigma + N * sin_d_sigma) * sin_a21 / M;
0183                 result.lat2 = atan(tan_theta2 / one_minus_f);
0184             }
0185             else
0186             {
0187                 CT const sigma2 = S_sigma - sigma1;
0188                 //theta2 = asin(cos(sigma2)) <=> sin_theta0 = 1
0189                 // NOTE: cos(sigma2) defines the sign of tan_theta2
0190                 CT const tan_theta2 = cos(sigma2) / math::abs(sin(sigma2));
0191                 result.lat2 = atan(tan_theta2 / one_minus_f);
0192             }
0193
0194             if (alter_result)
0195             {
0196                 result.lat2 = -result.lat2;
0197             }
0198         }
0199
0200         if BOOST_GEOMETRY_CONSTEXPR (CalcQuantities)
0201         {
0202             typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
0203             quantities::apply(lon1, lat1, result.lon2, result.lat2,
0204                               azimuth12, result.reverse_azimuth,
0205                               b, f,
0206                               result.reduced_length, result.geodesic_scale);
0207         }
0208
0209         if BOOST_GEOMETRY_CONSTEXPR (CalcCoordinates)
0210         {
0211             // For longitudes close to the antimeridian the result can be out
0212             // of range. Therefore normalize.
0213             // It has to be done at the end because otherwise differential
0214             // quantities are calculated incorrectly.
0215             math::detail::normalize_angle_cond<radian>(result.lon2);
0216         }
0217
0218         return result;
0219     }
0220
0221 private:
0222     static inline bool vflip_if_south(CT const& lat1, CT const& azi12, CT & lat1_alt, CT & azi12_alt)
0223     {
0224         CT const c2 = 2;
0225         CT const pi = math::pi<CT>();
0226         CT const pi_half = pi / c2;
0227
0228         if (azi12 > pi_half)
0229         {
0230             azi12_alt = pi - azi12;
0231             lat1_alt = -lat1;
0232             return true;
0233         }
0234         else if (azi12 < -pi_half)
0235         {
0236             azi12_alt = -pi - azi12;
0237             lat1_alt = -lat1;
0238             return true;
0239         }
0240
0241         return false;
0242     }
0243
0244     static inline void vflip_rev_azi(CT & rev_azi, CT const& azimuth12)
0245     {
0246         CT const c0 = 0;
0247         CT const pi = math::pi<CT>();
0248
0249         if (rev_azi == c0)
0250         {
0251             rev_azi = azimuth12 >= 0 ? pi : -pi;
0252         }
0253         else if (rev_azi > c0)
0254         {
0255             rev_azi = pi - rev_azi;
0256         }
0257         else
0258         {
0259             rev_azi = -pi - rev_azi;
0260         }
0261     }
0262
0263     static inline CT normalized1_1(CT const& value)
0264     {
0265         CT const c1 = 1;
0266         return value > c1 ? c1 :
0267                value < -c1 ? -c1 :
0268                value;
0269     }
0270 };
0271
0272 }}} // namespace boost::geometry::formula
0273
0274
0275 #endif // BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP