[algorithms] Add thomas_inverse formula - Andoyer-Lambert type approx. with second order.

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 <boost/math/constants/constants.hpp>
+
+#include <boost/geometry/core/radius.hpp>
+#include <boost/geometry/core/srs.hpp>
+
+#include <boost/geometry/util/math.hpp>
+
+#include <boost/geometry/algorithms/detail/flattening.hpp>
+
+
+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 <typename CT>
+class thomas_inverse
+{
+public:
+    template <typename T1, typename T2, typename Spheroid>
+    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<CT>(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>();
+        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