[ceph.git] / ceph / src / boost / libs / geometry / include / boost / geometry / algorithms / detail / andoyer_inverse.hpp

// Boost.Geometry

// Copyright (c) 2015-2016 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_ANDOYER_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP


#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/detail/flattening.hpp>
#include <boost/geometry/algorithms/detail/result_inverse.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 first 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, bool EnableDistance, bool EnableAzimuth>
struct andoyer_inverse
{
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        CT const c0 = CT(0);
        CT const c1 = CT(1);
        CT const pi = math::pi<CT>();

        result_type result;

        // coordinates in radians

        if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
        {
            result.set(c0, c0);
            return result;
        }

        CT const dlon = lon2 - lon1;
        CT const sin_dlon = sin(dlon);
        CT const cos_dlon = cos(dlon);
        CT const sin_lat1 = sin(lat1);
        CT const cos_lat1 = cos(lat1);
        CT const sin_lat2 = sin(lat2);
        CT const cos_lat2 = cos(lat2);

        // H,G,T = infinity if cos_d = 1 or cos_d = -1
        // lat1 == +-90 && lat2 == +-90
        // lat1 == lat2 && lon1 == lon2
        CT cos_d = sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon;
        // on some platforms cos_d may be outside valid range
        if (cos_d < -c1)
            cos_d = -c1;
        else if (cos_d > c1)
            cos_d = c1;

        CT const d = acos(cos_d); // [0, pi]
        CT const sin_d = sin(d);  // [-1, 1]
        
        CT const f = detail::flattening<CT>(spheroid);

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            CT const K = math::sqr(sin_lat1-sin_lat2);
            CT const L = math::sqr(sin_lat1+sin_lat2);
            CT const three_sin_d = CT(3) * sin_d;

            CT const one_minus_cos_d = c1 - cos_d;
            CT const one_plus_cos_d = c1 + cos_d;
            // cos_d = 1 or cos_d = -1 means that the points are antipodal

            CT const H = math::equals(one_minus_cos_d, c0) ?
                            c0 :
                            (d + three_sin_d) / one_minus_cos_d;
            CT const G = math::equals(one_plus_cos_d, c0) ?
                            c0 :
                            (d - three_sin_d) / one_plus_cos_d;

            CT const dd = -(f/CT(4))*(H*K+G*L);

            CT const a = get_radius<0>(spheroid);

            result.distance = a * (d + dd);
        }
        else
        {
            result.distance = c0;
        }

        if ( BOOST_GEOMETRY_CONDITION(EnableAzimuth) )
        {
            // sin_d = 0 <=> antipodal points
            if (math::equals(sin_d, c0))
            {
                // T = inf
                // dA = inf
                // azimuth = -inf
                if (lat1 <= lat2)
                    result.azimuth = c0;
                else
                    result.azimuth = pi;
            }
            else
            {
                CT const c2 = CT(2);

                CT A = c0;
                CT U = c0;
                if ( ! math::equals(cos_lat2, c0) )
                {
                    CT const tan_lat2 = sin_lat2/cos_lat2;
                    CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon;
                    A = atan2(sin_dlon, M);
                    CT const sin_2A = sin(c2*A);
                    U = (f/ c2)*math::sqr(cos_lat1)*sin_2A;
                }

                CT V = c0;
                if ( ! math::equals(cos_lat1, c0) )
                {
                    CT const tan_lat1 = sin_lat1/cos_lat1;
                    CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon;
                    CT const B = atan2(sin_dlon, N);
                    CT const sin_2B = sin(c2*B);
                    V = (f/ c2)*math::sqr(cos_lat2)*sin_2B;
                }

                CT const T = d / sin_d;
                CT const dA = V*T-U;

                result.azimuth = A - dA;

                // even with sin_d == 0 checked above if the second point
                // is somewhere in the antipodal area T may still be great
                // therefore dA may be great and the resulting azimuth
                // may be some more or less arbitrary angle
                if (A >= c0) // A indicates Eastern hemisphere
                {
                    if (dA >= c0) // A altered towards 0
                    {
                        if ((result.azimuth) < c0)
                            result.azimuth = c0;
                    }
                    else // dA < 0, A altered towards pi
                    {
                        if (result.azimuth > pi)
                            result.azimuth = pi;
                    }
                }
                else // A indicates Western hemisphere
                {
                    if (dA <= c0) // A altered towards 0
                    {
                        if (result.azimuth > c0)
                            result.azimuth = c0;
                    }
                    else // dA > 0, A altered towards -pi
                    {
                        CT const minus_pi = -pi;
                        if ((result.azimuth) < minus_pi)
                            result.azimuth = minus_pi;
                    }
                }
            }
        }
        else
        {
            result.azimuth = c0;
        }

        return result;
    }
};

}}} // namespace boost::geometry::detail


#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP
Commit	Line	Data
7c673cae FG	1	// Boost.Geometry
	2
	3	// Copyright (c) 2015-2016 Oracle and/or its affiliates.
	4
	5	// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
	6
	7	// Use, modification and distribution is subject to the Boost Software License,
	8	// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
	9	// http://www.boost.org/LICENSE_1_0.txt)
	10
	11	#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP
	12	#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP
	13
	14
	15	#include <boost/math/constants/constants.hpp>
	16
	17	#include <boost/geometry/core/radius.hpp>
	18	#include <boost/geometry/core/srs.hpp>
	19
	20	#include <boost/geometry/util/condition.hpp>
	21	#include <boost/geometry/util/math.hpp>
	22
	23	#include <boost/geometry/algorithms/detail/flattening.hpp>
	24	#include <boost/geometry/algorithms/detail/result_inverse.hpp>
	25
	26
	27	namespace boost { namespace geometry { namespace detail
	28	{
	29
	30	/*!
	31	\brief The solution of the inverse problem of geodesics on latlong coordinates,
	32	Forsyth-Andoyer-Lambert type approximation with first order terms.
	33	\author See
	34	- Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
	35	http://www.dtic.mil/docs/citations/AD0627893
	36	- Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
	37	http://www.dtic.mil/docs/citations/AD703541
	38	*/
	39
	40	template <typename CT, bool EnableDistance, bool EnableAzimuth>
	41	struct andoyer_inverse
	42	{
	43	typedef result_inverse<CT> result_type;
	44
	45	template <typename T1, typename T2, typename Spheroid>
	46	static inline result_type apply(T1 const& lon1,
	47	T1 const& lat1,
	48	T2 const& lon2,
	49	T2 const& lat2,
	50	Spheroid const& spheroid)
	51	{
	52	CT const c0 = CT(0);
	53	CT const c1 = CT(1);
	54	CT const pi = math::pi<CT>();
	55
	56	result_type result;
	57
	58	// coordinates in radians
	59
	60	if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
	61	{
	62	result.set(c0, c0);
	63	return result;
	64	}
65
66	CT const dlon = lon2 - lon1;
67	CT const sin_dlon = sin(dlon);
68	CT const cos_dlon = cos(dlon);
69	CT const sin_lat1 = sin(lat1);
70	CT const cos_lat1 = cos(lat1);
71	CT const sin_lat2 = sin(lat2);
72	CT const cos_lat2 = cos(lat2);
73
74	// H,G,T = infinity if cos_d = 1 or cos_d = -1
75	// lat1 == +-90 && lat2 == +-90
76	// lat1 == lat2 && lon1 == lon2
77	CT cos_d = sin_lat1sin_lat2 + cos_lat1cos_lat2*cos_dlon;
78	// on some platforms cos_d may be outside valid range
79	if (cos_d < -c1)
80	cos_d = -c1;
81	else if (cos_d > c1)
82	cos_d = c1;
83
84	CT const d = acos(cos_d); // [0, pi]
85	CT const sin_d = sin(d); // [-1, 1]
86
87	CT const f = detail::flattening<CT>(spheroid);
88
89	if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
90	{
91	CT const K = math::sqr(sin_lat1-sin_lat2);
92	CT const L = math::sqr(sin_lat1+sin_lat2);
93	CT const three_sin_d = CT(3) * sin_d;
94
95	CT const one_minus_cos_d = c1 - cos_d;
96	CT const one_plus_cos_d = c1 + cos_d;
97	// cos_d = 1 or cos_d = -1 means that the points are antipodal
98
99	CT const H = math::equals(one_minus_cos_d, c0) ?
100	c0 :
101	(d + three_sin_d) / one_minus_cos_d;
102	CT const G = math::equals(one_plus_cos_d, c0) ?
103	c0 :
104	(d - three_sin_d) / one_plus_cos_d;
105
106	CT const dd = -(f/CT(4))(HK+G*L);
107
108	CT const a = get_radius<0>(spheroid);
109
110	result.distance = a * (d + dd);
111	}
112	else
113	{
114	result.distance = c0;
115	}
116
117	if ( BOOST_GEOMETRY_CONDITION(EnableAzimuth) )
118	{
119	// sin_d = 0 <=> antipodal points
120	if (math::equals(sin_d, c0))
121	{
122	// T = inf
123	// dA = inf
124	// azimuth = -inf
125	if (lat1 <= lat2)
126	result.azimuth = c0;
127	else
128	result.azimuth = pi;
129	}
130	else
131	{
132	CT const c2 = CT(2);
133
134	CT A = c0;
135	CT U = c0;
136	if ( ! math::equals(cos_lat2, c0) )
137	{
138	CT const tan_lat2 = sin_lat2/cos_lat2;
139	CT const M = cos_lat1tan_lat2-sin_lat1cos_dlon;
140	A = atan2(sin_dlon, M);
141	CT const sin_2A = sin(c2*A);
142	U = (f/ c2)math::sqr(cos_lat1)sin_2A;
143	}
144
145	CT V = c0;
146	if ( ! math::equals(cos_lat1, c0) )
147	{
148	CT const tan_lat1 = sin_lat1/cos_lat1;
149	CT const N = cos_lat2tan_lat1-sin_lat2cos_dlon;
150	CT const B = atan2(sin_dlon, N);
151	CT const sin_2B = sin(c2*B);
152	V = (f/ c2)math::sqr(cos_lat2)sin_2B;
153	}
154
155	CT const T = d / sin_d;
156	CT const dA = V*T-U;
157
158	result.azimuth = A - dA;
159
160	// even with sin_d == 0 checked above if the second point
161	// is somewhere in the antipodal area T may still be great
162	// therefore dA may be great and the resulting azimuth
163	// may be some more or less arbitrary angle
164	if (A >= c0) // A indicates Eastern hemisphere
165	{
166	if (dA >= c0) // A altered towards 0
167	{
168	if ((result.azimuth) < c0)
169	result.azimuth = c0;
170	}
171	else // dA < 0, A altered towards pi
172	{
173	if (result.azimuth > pi)
174	result.azimuth = pi;
175	}
176	}
177	else // A indicates Western hemisphere
178	{
179	if (dA <= c0) // A altered towards 0
180	{
181	if (result.azimuth > c0)
182	result.azimuth = c0;
183	}
184	else // dA > 0, A altered towards -pi
185	{
186	CT const minus_pi = -pi;
187	if ((result.azimuth) < minus_pi)
188	result.azimuth = minus_pi;
189	}
190	}
191	}
192	}
193	else
194	{
195	result.azimuth = c0;
196	}
197
198	return result;
199	}
200	};
201
202	}}} // namespace boost::geometry::detail
203
204
205	#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP