diff --git a/include/boost/geometry/algorithms/detail/thomas_inverse.hpp b/include/boost/geometry/algorithms/detail/thomas_inverse.hpp new file mode 100644 index 000000000..d427e7697 --- /dev/null +++ b/include/boost/geometry/algorithms/detail/thomas_inverse.hpp @@ -0,0 +1,193 @@ +// Boost.Geometry + +// Copyright (c) 2015 Oracle and/or its affiliates. + +// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle + +// Use, modification and distribution is subject to the Boost Software License, +// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at +// http://www.boost.org/LICENSE_1_0.txt) + +#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP +#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP + + +#include + +#include +#include + +#include + +#include + + +namespace boost { namespace geometry { namespace detail +{ + +/*! +\brief The solution of the inverse problem of geodesics on latlong coordinates, + Forsyth-Andoyer-Lambert type approximation with second order terms. +\author See + - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965 + http://www.dtic.mil/docs/citations/AD0627893 + - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970 + http://www.dtic.mil/docs/citations/AD703541 +*/ +template +class thomas_inverse +{ +public: + template + thomas_inverse(T1 const& lon1, + T1 const& lat1, + T2 const& lon2, + T2 const& lat2, + Spheroid const& spheroid) + : a(get_radius<0>(spheroid)) + , f(detail::flattening(spheroid)) + , is_result_zero(false) + { + // coordinates in radians + + if ( math::equals(lon1, lon2) + && math::equals(lat1, lat2) ) + { + is_result_zero = true; + return; + } + + CT const one_minus_f = CT(1) - f; + + CT const tan_theta1 = one_minus_f * tan(lat1); + CT const tan_theta2 = one_minus_f * tan(lat2); + CT const theta1 = atan(tan_theta1); + CT const theta2 = atan(tan_theta2); + + CT const theta_m = (theta1 + theta2) / CT(2); + CT const d_theta_m = (theta2 - theta1) / CT(2); + d_lambda = lon2 - lon1; + CT const d_lambda_m = d_lambda / CT(2); + + sin_theta_m = sin(theta_m); + cos_theta_m = cos(theta_m); + sin_d_theta_m = sin(d_theta_m); + cos_d_theta_m = cos(d_theta_m); + CT const sin2_theta_m = math::sqr(sin_theta_m); + CT const cos2_theta_m = math::sqr(cos_theta_m); + CT const sin2_d_theta_m = math::sqr(sin_d_theta_m); + CT const cos2_d_theta_m = math::sqr(cos_d_theta_m); + CT const sin_d_lambda_m = sin(d_lambda_m); + CT const sin2_d_lambda_m = math::sqr(sin_d_lambda_m); + + CT const H = cos2_theta_m - sin2_d_theta_m; + CT const L = sin2_d_theta_m + H * sin2_d_lambda_m; + cos_d = CT(1) - CT(2) * L; + CT const d = acos(cos_d); + sin_d = sin(d); + + CT const one_minus_L = CT(1) - L; + + if ( math::equals(sin_d, CT(0)) + || math::equals(L, CT(0)) + || math::equals(one_minus_L, CT(0)) ) + { + is_result_zero = true; + return; + } + + CT const U = CT(2) * sin2_theta_m * cos2_d_theta_m / one_minus_L; + CT const V = CT(2) * sin2_d_theta_m * cos2_theta_m / L; + X = U + V; + Y = U - V; + T = d / sin_d; + //CT const D = CT(4) * math::sqr(T); + //CT const E = CT(2) * cos_d; + //CT const A = D * E; + //CT const B = CT(2) * D; + //CT const C = T - (A - E) / CT(2); + } + + inline CT distance() const + { + if ( is_result_zero ) + { + // TODO return some approximated value + return CT(0); + } + + //CT const n1 = X * (A + C*X); + //CT const n2 = Y * (B + E*Y); + //CT const n3 = D*X*Y; + + //CT const f_sqr = math::sqr(f); + //CT const f_sqr_per_64 = f_sqr / CT(64); + + CT const delta1d = f * (T*X-Y) / CT(4); + //CT const delta2d = f_sqr_per_64 * (n1 - n2 + n3); + + return a * sin_d * (T - delta1d); + //double S2 = a * sin_d * (T - delta1d + delta2d); + } + + inline CT azimuth() const + { + // NOTE: if both cos_latX == 0 then below we'd have 0 * INF + // it's a situation when the endpoints are on the poles +-90 deg + // in this case the azimuth could either be 0 or +-pi + if ( is_result_zero ) + { + return CT(0); + } + + // may also be used to calculate distance21 + //CT const D = CT(4) * math::sqr(T); + CT const E = CT(2) * cos_d; + //CT const A = D * E; + //CT const B = CT(2) * D; + // may also be used to calculate distance21 + CT const f_sqr = math::sqr(f); + CT const f_sqr_per_64 = f_sqr / CT(64); + + CT const F = CT(2)*Y-E*(CT(4)-X); + //CT const M = CT(32)*T-(CT(20)*T-A)*X-(B+CT(4))*Y; + CT const G = f*T/CT(2) + f_sqr_per_64; + CT const tan_d_lambda = tan(d_lambda); + CT const Q = -(F*G*tan_d_lambda) / CT(4); + + CT const d_lambda_p = (d_lambda + Q) / CT(2); + CT const tan_d_lambda_p = tan(d_lambda_p); + + CT const v = atan2(cos_d_theta_m, sin_theta_m * tan_d_lambda_p); + CT const u = atan2(-sin_d_theta_m, cos_theta_m * tan_d_lambda_p); + + CT const pi = math::pi(); + CT alpha1 = v + u; + if ( alpha1 > pi ) + alpha1 -= CT(2) * pi; + + return alpha1; + } + +private: + CT const a; + CT const f; + + CT d_lambda; + CT cos_d; + CT sin_d; + CT X; + CT Y; + CT T; + CT sin_theta_m; + CT cos_theta_m; + CT sin_d_theta_m; + CT cos_d_theta_m; + + bool is_result_zero; +}; + +}}} // namespace boost::geometry::detail + + +#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_THOMAS_INVERSE_HPP