1 // Boost.Geometry 2 3 // Copyright (c) 2016-2020 Oracle and/or its affiliates. 4 5 // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle 6 // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle 7 8 // Use, modification and distribution is subject to the Boost Software License, 9 // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at 10 // http://www.boost.org/LICENSE_1_0.txt) 11 12 #ifndef BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP 13 #define BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP 14 15 16 #include <boost/math/constants/constants.hpp> 17 18 #include <boost/geometry/core/assert.hpp> 19 #include <boost/geometry/core/radius.hpp> 20 21 #include <boost/geometry/util/condition.hpp> 22 #include <boost/geometry/util/math.hpp> 23 #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp> 24 25 #include <boost/geometry/formulas/differential_quantities.hpp> 26 #include <boost/geometry/formulas/flattening.hpp> 27 #include <boost/geometry/formulas/result_direct.hpp> 28 29 30 namespace boost { namespace geometry { namespace formula 31 { 32 33 34 /*! 35 \brief The solution of the direct problem of geodesics on latlong coordinates, 36 Forsyth-Andoyer-Lambert type approximation with first/second order terms. 37 \author See 38 - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965 39 http://www.dtic.mil/docs/citations/AD0627893 40 - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970 41 http://www.dtic.mil/docs/citations/AD0703541 42 */ 43 template < 44 typename CT, 45 bool SecondOrder = true, 46 bool EnableCoordinates = true, 47 bool EnableReverseAzimuth = false, 48 bool EnableReducedLength = false, 49 bool EnableGeodesicScale = false 50 > 51 class thomas_direct 52 { 53 static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale; 54 static const bool CalcCoordinates = EnableCoordinates || CalcQuantities; 55 static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcCoordinates || CalcQuantities; 56 57 public: 58 typedef result_direct<CT> result_type; 59 60 template <typename T, typename Dist, typename Azi, typename Spheroid> apply(T const & lo1,T const & la1,Dist const & distance,Azi const & azimuth12,Spheroid const & spheroid)61 static inline result_type apply(T const& lo1, 62 T const& la1, 63 Dist const& distance, 64 Azi const& azimuth12, 65 Spheroid const& spheroid) 66 { 67 result_type result; 68 69 CT const lon1 = lo1; 70 CT const lat1 = la1; 71 72 CT const c0 = 0; 73 CT const c1 = 1; 74 CT const c2 = 2; 75 CT const c4 = 4; 76 77 CT const a = CT(get_radius<0>(spheroid)); 78 CT const b = CT(get_radius<2>(spheroid)); 79 CT const f = formula::flattening<CT>(spheroid); 80 CT const one_minus_f = c1 - f; 81 82 CT const pi = math::pi<CT>(); 83 CT const pi_half = pi / c2; 84 85 BOOST_GEOMETRY_ASSERT(-pi <= azimuth12 && azimuth12 <= pi); 86 87 // keep azimuth small - experiments show low accuracy 88 // if the azimuth is closer to (+-)180 deg. 89 CT azi12_alt = azimuth12; 90 CT lat1_alt = lat1; 91 bool alter_result = vflip_if_south(lat1, azimuth12, lat1_alt, azi12_alt); 92 93 CT const theta1 = math::equals(lat1_alt, pi_half) ? lat1_alt : 94 math::equals(lat1_alt, -pi_half) ? lat1_alt : 95 atan(one_minus_f * tan(lat1_alt)); 96 CT const sin_theta1 = sin(theta1); 97 CT const cos_theta1 = cos(theta1); 98 99 CT const sin_a12 = sin(azi12_alt); 100 CT const cos_a12 = cos(azi12_alt); 101 102 CT const M = cos_theta1 * sin_a12; // cos_theta0 103 CT const theta0 = acos(M); 104 CT const sin_theta0 = sin(theta0); 105 106 CT const N = cos_theta1 * cos_a12; 107 CT const C1 = f * M; // lower-case c1 in the technical report 108 CT const C2 = f * (c1 - math::sqr(M)) / c4; // lower-case c2 in the technical report 109 CT D = 0; 110 CT P = 0; 111 if ( BOOST_GEOMETRY_CONDITION(SecondOrder) ) 112 { 113 D = (c1 - C2) * (c1 - C2 - C1 * M); 114 P = C2 * (c1 + C1 * M / c2) / D; 115 } 116 else 117 { 118 D = c1 - c2 * C2 - C1 * M; 119 P = C2 / D; 120 } 121 // special case for equator: 122 // sin_theta0 = 0 <=> lat1 = 0 ^ |azimuth12| = pi/2 123 // NOTE: in this case it doesn't matter what's the value of cos_sigma1 because 124 // theta1=0, theta0=0, M=1|-1, C2=0 so X=0 and Y=0 so d_sigma=d 125 // cos_a12=0 so N=0, therefore 126 // lat2=0, azi21=pi/2|-pi/2 127 // d_eta = atan2(sin_d_sigma, cos_d_sigma) 128 // H = C1 * d_sigma 129 CT const cos_sigma1 = math::equals(sin_theta0, c0) 130 ? c1 131 : normalized1_1(sin_theta1 / sin_theta0); 132 CT const sigma1 = acos(cos_sigma1); 133 CT const d = distance / (a * D); 134 CT const u = 2 * (sigma1 - d); 135 CT const cos_d = cos(d); 136 CT const sin_d = sin(d); 137 CT const cos_u = cos(u); 138 CT const sin_u = sin(u); 139 140 CT const W = c1 - c2 * P * cos_u; 141 CT const V = cos_u * cos_d - sin_u * sin_d; 142 CT const Y = c2 * P * V * W * sin_d; 143 CT X = 0; 144 CT d_sigma = d - Y; 145 if ( BOOST_GEOMETRY_CONDITION(SecondOrder) ) 146 { 147 X = math::sqr(C2) * sin_d * cos_d * (2 * math::sqr(V) - c1); 148 d_sigma += X; 149 } 150 CT const sin_d_sigma = sin(d_sigma); 151 CT const cos_d_sigma = cos(d_sigma); 152 153 if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth)) 154 { 155 result.reverse_azimuth = atan2(M, N * cos_d_sigma - sin_theta1 * sin_d_sigma); 156 157 if (alter_result) 158 { 159 vflip_rev_azi(result.reverse_azimuth, azimuth12); 160 } 161 } 162 163 if (BOOST_GEOMETRY_CONDITION(CalcCoordinates)) 164 { 165 CT const S_sigma = c2 * sigma1 - d_sigma; 166 CT cos_S_sigma = 0; 167 CT H = C1 * d_sigma; 168 if ( BOOST_GEOMETRY_CONDITION(SecondOrder) ) 169 { 170 cos_S_sigma = cos(S_sigma); 171 H = H * (c1 - C2) - C1 * C2 * sin_d_sigma * cos_S_sigma; 172 } 173 CT const d_eta = atan2(sin_d_sigma * sin_a12, cos_theta1 * cos_d_sigma - sin_theta1 * sin_d_sigma * cos_a12); 174 CT const d_lambda = d_eta - H; 175 176 result.lon2 = lon1 + d_lambda; 177 178 if (! math::equals(M, c0)) 179 { 180 CT const sin_a21 = sin(result.reverse_azimuth); 181 CT const tan_theta2 = (sin_theta1 * cos_d_sigma + N * sin_d_sigma) * sin_a21 / M; 182 result.lat2 = atan(tan_theta2 / one_minus_f); 183 } 184 else 185 { 186 CT const sigma2 = S_sigma - sigma1; 187 //theta2 = asin(cos(sigma2)) <=> sin_theta0 = 1 188 // NOTE: cos(sigma2) defines the sign of tan_theta2 189 CT const tan_theta2 = cos(sigma2) / math::abs(sin(sigma2)); 190 result.lat2 = atan(tan_theta2 / one_minus_f); 191 } 192 193 if (alter_result) 194 { 195 result.lat2 = -result.lat2; 196 } 197 } 198 199 if (BOOST_GEOMETRY_CONDITION(CalcQuantities)) 200 { 201 typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities; 202 quantities::apply(lon1, lat1, result.lon2, result.lat2, 203 azimuth12, result.reverse_azimuth, 204 b, f, 205 result.reduced_length, result.geodesic_scale); 206 } 207 208 if (BOOST_GEOMETRY_CONDITION(CalcCoordinates)) 209 { 210 // For longitudes close to the antimeridian the result can be out 211 // of range. Therefore normalize. 212 // It has to be done at the end because otherwise differential 213 // quantities are calculated incorrectly. 214 math::detail::normalize_angle_cond<radian>(result.lon2); 215 } 216 217 return result; 218 } 219 220 private: vflip_if_south(CT const & lat1,CT const & azi12,CT & lat1_alt,CT & azi12_alt)221 static inline bool vflip_if_south(CT const& lat1, CT const& azi12, CT & lat1_alt, CT & azi12_alt) 222 { 223 CT const c2 = 2; 224 CT const pi = math::pi<CT>(); 225 CT const pi_half = pi / c2; 226 227 if (azi12 > pi_half) 228 { 229 azi12_alt = pi - azi12; 230 lat1_alt = -lat1; 231 return true; 232 } 233 else if (azi12 < -pi_half) 234 { 235 azi12_alt = -pi - azi12; 236 lat1_alt = -lat1; 237 return true; 238 } 239 240 return false; 241 } 242 vflip_rev_azi(CT & rev_azi,CT const & azimuth12)243 static inline void vflip_rev_azi(CT & rev_azi, CT const& azimuth12) 244 { 245 CT const c0 = 0; 246 CT const pi = math::pi<CT>(); 247 248 if (rev_azi == c0) 249 { 250 rev_azi = azimuth12 >= 0 ? pi : -pi; 251 } 252 else if (rev_azi > c0) 253 { 254 rev_azi = pi - rev_azi; 255 } 256 else 257 { 258 rev_azi = -pi - rev_azi; 259 } 260 } 261 normalized1_1(CT const & value)262 static inline CT normalized1_1(CT const& value) 263 { 264 CT const c1 = 1; 265 return value > c1 ? c1 : 266 value < -c1 ? -c1 : 267 value; 268 } 269 }; 270 271 }}} // namespace boost::geometry::formula 272 273 274 #endif // BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP 275