Chris@102: // Boost.Geometry
Chris@102: 
Chris@102: // Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
Chris@102: 
Chris@102: // This file was modified by Oracle on 2014.
Chris@102: // Modifications copyright (c) 2014 Oracle and/or its affiliates.
Chris@102: 
Chris@102: // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
Chris@102: 
Chris@102: // Use, modification and distribution is subject to the Boost Software License,
Chris@102: // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
Chris@102: // http://www.boost.org/LICENSE_1_0.txt)
Chris@102: 
Chris@102: #ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_DIRECT_HPP
Chris@102: #define BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_DIRECT_HPP
Chris@102: 
Chris@102: 
Chris@102: #include <boost/math/constants/constants.hpp>
Chris@102: 
Chris@102: #include <boost/geometry/core/radius.hpp>
Chris@102: #include <boost/geometry/core/srs.hpp>
Chris@102: 
Chris@102: #include <boost/geometry/util/math.hpp>
Chris@102: 
Chris@102: #include <boost/geometry/algorithms/detail/flattening.hpp>
Chris@102: 
Chris@102: 
Chris@102: #ifndef BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS
Chris@102: #define BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS 1000
Chris@102: #endif
Chris@102: 
Chris@102: 
Chris@102: namespace boost { namespace geometry { namespace detail
Chris@102: {
Chris@102: 
Chris@102: /*!
Chris@102: \brief The solution of the direct problem of geodesics on latlong coordinates, after Vincenty, 1975
Chris@102: \author See
Chris@102:     - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
Chris@102:     - http://www.icsm.gov.au/gda/gdav2.3.pdf
Chris@102: \author Adapted from various implementations to get it close to the original document
Chris@102:     - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
Chris@102:     - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
Chris@102:     - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink
Chris@102: 
Chris@102: */
Chris@102: template <typename CT>
Chris@102: class vincenty_direct
Chris@102: {
Chris@102: public:
Chris@102:     template <typename T, typename Dist, typename Azi, typename Spheroid>
Chris@102:     vincenty_direct(T const& lo1,
Chris@102:                     T const& la1,
Chris@102:                     Dist const& distance,
Chris@102:                     Azi const& azimuth12,
Chris@102:                     Spheroid const& spheroid)
Chris@102:         : lon1(lo1)
Chris@102:         , lat1(la1)
Chris@102:         , is_distance_zero(false)
Chris@102:     {
Chris@102:         if ( math::equals(distance, Dist(0)) || distance < Dist(0) )
Chris@102:         {
Chris@102:             is_distance_zero = true;
Chris@102:             return;
Chris@102:         }
Chris@102: 
Chris@102:         CT const radius_a = CT(get_radius<0>(spheroid));
Chris@102:         CT const radius_b = CT(get_radius<2>(spheroid));
Chris@102:         flattening = geometry::detail::flattening<CT>(spheroid);
Chris@102: 
Chris@102:         sin_azimuth12 = sin(azimuth12);
Chris@102:         cos_azimuth12 = cos(azimuth12);
Chris@102: 
Chris@102:         // U: reduced latitude, defined by tan U = (1-f) tan phi
Chris@102:         one_min_f = CT(1) - flattening;
Chris@102:         CT const tan_U1 = one_min_f * tan(lat1);
Chris@102:         CT const sigma1 = atan2(tan_U1, cos_azimuth12); // (1)
Chris@102: 
Chris@102:         // may be calculated from tan using 1 sqrt()
Chris@102:         CT const U1 = atan(tan_U1);
Chris@102:         sin_U1 = sin(U1);
Chris@102:         cos_U1 = cos(U1);
Chris@102: 
Chris@102:         sin_alpha = cos_U1 * sin_azimuth12; // (2)
Chris@102:         sin_alpha_sqr = math::sqr(sin_alpha);
Chris@102:         cos_alpha_sqr = CT(1) - sin_alpha_sqr;
Chris@102: 
Chris@102:         CT const b_sqr = radius_b * radius_b;
Chris@102:         CT const u_sqr = cos_alpha_sqr * (radius_a * radius_a - b_sqr) / b_sqr;
Chris@102:         CT const A = CT(1) + (u_sqr/CT(16384)) * (CT(4096) + u_sqr*(CT(-768) + u_sqr*(CT(320) - u_sqr*CT(175)))); // (3)
Chris@102:         CT const B = (u_sqr/CT(1024))*(CT(256) + u_sqr*(CT(-128) + u_sqr*(CT(74) - u_sqr*CT(47)))); // (4)
Chris@102: 
Chris@102:         CT s_div_bA = distance / (radius_b * A);
Chris@102:         sigma = s_div_bA; // (7)
Chris@102: 
Chris@102:         CT previous_sigma;
Chris@102: 
Chris@102:         int counter = 0; // robustness
Chris@102: 
Chris@102:         do
Chris@102:         {
Chris@102:             previous_sigma = sigma;
Chris@102: 
Chris@102:             CT const two_sigma_m = CT(2) * sigma1 + sigma; // (5)
Chris@102: 
Chris@102:             sin_sigma = sin(sigma);
Chris@102:             cos_sigma = cos(sigma);
Chris@102:             CT const sin_sigma_sqr = math::sqr(sin_sigma);
Chris@102:             cos_2sigma_m = cos(two_sigma_m);
Chris@102:             cos_2sigma_m_sqr = math::sqr(cos_2sigma_m);
Chris@102: 
Chris@102:             CT const delta_sigma = B * sin_sigma * (cos_2sigma_m
Chris@102:                                         + (B/CT(4)) * ( cos_sigma * (CT(-1) + CT(2)*cos_2sigma_m_sqr)
Chris@102:                                             - (B/CT(6) * cos_2sigma_m * (CT(-3)+CT(4)*sin_sigma_sqr) * (CT(-3)+CT(4)*cos_2sigma_m_sqr)) )); // (6)
Chris@102: 
Chris@102:             sigma = s_div_bA + delta_sigma; // (7)
Chris@102: 
Chris@102:             ++counter; // robustness
Chris@102: 
Chris@102:         } while ( geometry::math::abs(previous_sigma - sigma) > CT(1e-12)
Chris@102:                //&& geometry::math::abs(sigma) < pi
Chris@102:                && counter < BOOST_GEOMETRY_DETAIL_VINCENTY_MAX_STEPS ); // robustness
Chris@102:     }
Chris@102: 
Chris@102:     inline CT lat2() const
Chris@102:     {
Chris@102:         if ( is_distance_zero )
Chris@102:         {
Chris@102:             return lat1;
Chris@102:         }
Chris@102: 
Chris@102:         return atan2( sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_azimuth12,
Chris@102:                       one_min_f * math::sqrt(sin_alpha_sqr + math::sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12))); // (8)
Chris@102:     }
Chris@102: 
Chris@102:     inline CT lon2() const
Chris@102:     {
Chris@102:         if ( is_distance_zero )
Chris@102:         {
Chris@102:             return lon1;
Chris@102:         }
Chris@102: 
Chris@102:         CT const lambda = atan2( sin_sigma * sin_azimuth12,
Chris@102:                                  cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_azimuth12); // (9)
Chris@102:         CT const C = (flattening/CT(16)) * cos_alpha_sqr * ( CT(4) + flattening * ( CT(4) - CT(3) * cos_alpha_sqr ) ); // (10)
Chris@102:         CT const L = lambda - (CT(1) - C) * flattening * sin_alpha
Chris@102:                         * ( sigma + C * sin_sigma * ( cos_2sigma_m + C * cos_sigma * ( CT(-1) + CT(2) * cos_2sigma_m_sqr ) ) ); // (11)
Chris@102: 
Chris@102:         return lon1 + L;
Chris@102:     }
Chris@102: 
Chris@102:     inline CT azimuth21() const
Chris@102:     {
Chris@102:         // NOTE: signs of X and Y are different than in the original paper
Chris@102:         return is_distance_zero ?
Chris@102:                CT(0) :
Chris@102:                atan2(-sin_alpha, sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_azimuth12); // (12)
Chris@102:     }
Chris@102: 
Chris@102: private:
Chris@102:     CT sigma;
Chris@102:     CT sin_sigma;
Chris@102:     CT cos_sigma;
Chris@102: 
Chris@102:     CT cos_2sigma_m;
Chris@102:     CT cos_2sigma_m_sqr;
Chris@102: 
Chris@102:     CT sin_alpha;
Chris@102:     CT sin_alpha_sqr;
Chris@102:     CT cos_alpha_sqr;
Chris@102: 
Chris@102:     CT sin_azimuth12;
Chris@102:     CT cos_azimuth12;
Chris@102: 
Chris@102:     CT sin_U1;
Chris@102:     CT cos_U1;
Chris@102: 
Chris@102:     CT flattening;
Chris@102:     CT one_min_f;
Chris@102: 
Chris@102:     CT const lon1;
Chris@102:     CT const lat1;
Chris@102: 
Chris@102:     bool is_distance_zero;
Chris@102: };
Chris@102: 
Chris@102: }}} // namespace boost::geometry::detail
Chris@102: 
Chris@102: 
Chris@102: #endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_VINCENTY_DIRECT_HPP