Files
webcamservice/include/boost/geometry/strategies/geographic/distance_cross_track.hpp
2024-12-15 16:18:16 +08:00

642 lines
21 KiB
C++

// Boost.Geometry (aka GGL, Generic Geometry Library)
// Copyright (c) 2016-2017, Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fysikopoulos, 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_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
#include <algorithm>
#include <boost/config.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
#include <boost/geometry/formulas/result_direct.hpp>
#include <boost/geometry/formulas/mean_radius.hpp>
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
# include <boost/geometry/io/dsv/write.hpp>
#endif
#ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
#endif
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <iostream>
#endif
namespace boost { namespace geometry
{
namespace strategy { namespace distance
{
/*!
\brief Strategy functor for distance point to segment calculation on ellipsoid
Algorithm uses direct and inverse geodesic problems as subroutines.
The algorithm approximates the distance by an iterative Newton method.
\ingroup strategies
\details Class which calculates the distance of a point to a segment, for points
on the ellipsoid
\see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
https://arxiv.org/abs/1102.1215
\tparam FormulaPolicy underlying point-point distance strategy
\tparam Spheroid is the spheroidal model used
\tparam CalculationType \tparam_calculation
\tparam EnableClosestPoint computes the closest point on segment if true
*/
template
<
typename FormulaPolicy = strategy::andoyer,
typename Spheroid = srs::spheroid<double>,
typename CalculationType = void,
bool EnableClosestPoint = false
>
class geographic_cross_track
{
public :
template <typename Point, typename PointOfSegment>
struct return_type
: promote_floating_point
<
typename select_calculation_type
<
Point,
PointOfSegment,
CalculationType
>::type
>
{};
struct distance_strategy
{
typedef geographic<FormulaPolicy, Spheroid, CalculationType> type;
};
inline typename distance_strategy::type get_distance_strategy() const
{
typedef typename distance_strategy::type distance_type;
return distance_type(m_spheroid);
}
explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
: m_spheroid(spheroid)
{}
template <typename Point, typename PointOfSegment>
inline typename return_type<Point, PointOfSegment>::type
apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
{
typedef typename coordinate_system<Point>::type::units units_type;
return (apply<units_type>(get<0>(sp1), get<1>(sp1),
get<0>(sp2), get<1>(sp2),
get<0>(p), get<1>(p),
m_spheroid)).distance;
}
private :
template <typename CT>
struct result_distance_point_segment
{
result_distance_point_segment()
: distance(0)
, closest_point_lon(0)
, closest_point_lat(0)
{}
CT distance;
CT closest_point_lon;
CT closest_point_lat;
};
template <typename CT>
result_distance_point_segment<CT>
static inline non_iterative_case(CT lon, CT lat, CT distance)
{
result_distance_point_segment<CT> result;
result.distance = distance;
if (EnableClosestPoint)
{
result.closest_point_lon = lon;
result.closest_point_lat = lat;
}
return result;
}
template <typename CT>
result_distance_point_segment<CT>
static inline non_iterative_case(CT lon1, CT lat1, //p1
CT lon2, CT lat2, //p2
Spheroid const& spheroid)
{
CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
::apply(lon1, lat1, lon2, lat2, spheroid);
return non_iterative_case(lon1, lat1, distance);
}
template <typename CT>
CT static inline normalize(CT g4)
{
CT const pi = math::pi<CT>();
if (g4 < 0 && g4 < -pi)//close to -270
{
return g4 + 1.5 * pi;
}
else if (g4 > 0 && g4 > pi)//close to 270
{
return - g4 + 1.5 * pi;
}
else if (g4 < 0 && g4 > -pi)//close to -90
{
return -g4 - pi/2;
}
return g4 - pi/2;
}
template <typename Units, typename CT>
result_distance_point_segment<CT>
static inline apply(CT lon1, CT lat1, //p1
CT lon2, CT lat2, //p2
CT lon3, CT lat3, //query point p3
Spheroid const& spheroid)
{
typedef typename FormulaPolicy::template inverse<CT, true, false, false, true, true>
inverse_distance_quantities_type;
typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
inverse_azimuth_type;
typedef typename FormulaPolicy::template inverse<CT, false, true, true, false, false>
inverse_azimuth_reverse_type;
typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
direct_distance_type;
CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
result_distance_point_segment<CT> result;
// Constants
CT const f = geometry::formula::flattening<CT>(spheroid);
CT const pi = math::pi<CT>();
CT const half_pi = pi / CT(2);
CT const c0 = CT(0);
// Convert to radians
lon1 = math::as_radian<Units>(lon1);
lat1 = math::as_radian<Units>(lat1);
lon2 = math::as_radian<Units>(lon2);
lat2 = math::as_radian<Units>(lat2);
lon3 = math::as_radian<Units>(lon3);
lat3 = math::as_radian<Units>(lat3);
if (lon1 > lon2)
{
std::swap(lon1, lon2);
std::swap(lat1, lat2);
}
//segment on equator
//Note: antipodal points on equator does not define segment on equator
//but pass by the pole
CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
if (math::equals(lat1, c0) && math::equals(lat2, c0)
&& !math::equals(math::abs(diff), pi))
{
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "Equatorial segment" << std::endl;
#endif
if (lon3 <= lon1)
{
return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
}
if (lon3 >= lon2)
{
return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
}
return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
}
if (math::equals(math::abs(diff), pi))
{
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "Meridian segment" << std::endl;
#endif
result_distance_point_segment<CT> d1 = apply<geometry::radian>(lon1, lat1, lon1, half_pi, lon3, lat3, spheroid);
result_distance_point_segment<CT> d2 = apply<geometry::radian>(lon2, lat2, lon2, half_pi, lon3, lat3, spheroid);
if (d1.distance < d2.distance)
{
return d1;
}
else
{
return d2;
}
}
CT d1 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
::apply(lon1, lat1, lon3, lat3, spheroid);
CT d3 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
::apply(lon1, lat1, lon2, lat2, spheroid);
if (geometry::math::equals(d3, c0))
{
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "Degenerate segment" << std::endl;
std::cout << "distance between points=" << d1 << std::endl;
#endif
return non_iterative_case(lon1, lat2, d1);
}
CT d2 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
::apply(lon2, lat2, lon3, lat3, spheroid);
// Compute a12 (GEO)
geometry::formula::result_inverse<CT> res12 =
inverse_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
CT a12 = res12.azimuth;
CT a13 = inverse_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid).azimuth;
CT a312 = a13 - a12;
if (geometry::math::equals(a312, c0))
{
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "point on segment" << std::endl;
#endif
return non_iterative_case(lon3, lat3, c0);
}
CT projection1 = cos( a312 ) * d1 / d3;
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "segment=(" << lon1 * math::r2d<CT>();
std::cout << "," << lat1 * math::r2d<CT>();
std::cout << "),(" << lon2 * math::r2d<CT>();
std::cout << "," << lat2 * math::r2d<CT>();
std::cout << ")\np=(" << lon3 * math::r2d<CT>();
std::cout << "," << lat3 * math::r2d<CT>();
std::cout << ")\na1=" << a12 * math::r2d<CT>() << std::endl;
std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl;
std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
std::cout << "cos(a312)=" << cos(a312) << std::endl;
#endif
if (projection1 < 0.0)
{
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "projection closer to p1" << std::endl;
#endif
// projection of p3 on geodesic spanned by segment (p1,p2) fall
// outside of segment on the side of p1
return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
}
CT a21 = res12.reverse_azimuth - pi;
CT a23 = inverse_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid).azimuth;
CT a321 = a23 - a21;
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl;
std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl;
std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
std::cout << "cos(a321)=" << cos(a321) << std::endl;
#endif
CT projection2 = cos( a321 ) * d2 / d3;
if (projection2 < 0.0)
{
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "projection closer to p2" << std::endl;
#endif
// projection of p3 on geodesic spanned by segment (p1,p2) fall
// outside of segment on the side of p2
return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
}
// Guess s14 (SPHERICAL)
typedef geometry::model::point
<
CT, 2,
geometry::cs::spherical_equatorial<geometry::radian>
> point;
CT bet1 = atan((1 - f) * tan(lon1));
CT bet2 = atan((1 - f) * tan(lon2));
CT bet3 = atan((1 - f) * tan(lon3));
point p1 = point(bet1, lat1);
point p2 = point(bet2, lat2);
point p3 = point(bet3, lat3);
geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
CT s34 = cross_track.apply(p3, p1, p2);
geometry::strategy::distance::haversine<CT> str(earth_radius);
CT s13 = str.apply(p1, p3);
CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "s34=" << s34 << std::endl;
std::cout << "s13=" << s13 << std::endl;
std::cout << "s14=" << s14 << std::endl;
std::cout << "===============" << std::endl;
#endif
// Update s14 (using Newton method)
CT prev_distance = 0;
geometry::formula::result_direct<CT> res14;
geometry::formula::result_inverse<CT> res34;
int counter = 0; // robustness
CT g4;
CT delta_g4;
do{
prev_distance = res34.distance;
// Solve the direct problem to find p4 (GEO)
res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
// Solve an inverse problem to find g4
// g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)
CT a4 = inverse_azimuth_type::apply(res14.lon2, res14.lat2,
lon2, lat2, spheroid).azimuth;
res34 = inverse_distance_quantities_type::apply(res14.lon2, res14.lat2,
lon3, lat3, spheroid);
g4 = res34.azimuth - a4;
delta_g4 = normalize(g4);
CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
CT m34 = res34.reduced_length;
CT der = (M43 / m34) * sin(g4);
s14 = s14 - delta_g4 / der;
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
"," << res14.lat2 * math::r2d<CT>() << std::endl;
std::cout << "delta_g4=" << delta_g4 << std::endl;
std::cout << "g4=" << g4 * math::r2d<CT>() << std::endl;
std::cout << "der=" << der << std::endl;
std::cout << "M43=" << M43 << std::endl;
std::cout << "spherical limit=" << cos(s14/earth_radius) << std::endl;
std::cout << "m34=" << m34 << std::endl;
std::cout << "new_s14=" << s14 << std::endl;
std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl;
std::cout << "---------end of step " << counter << std::endl<< std::endl;
#endif
result.distance = prev_distance;
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
if (g4 == half_pi)
{
std::cout << "Stop msg: g4 == half_pi" << std::endl;
}
if (res34.distance >= prev_distance && prev_distance != 0)
{
std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
}
if (delta_g4 == 0)
{
std::cout << "Stop msg: delta_g4 == 0" << std::endl;
}
if (counter == 19)
{
std::cout << "Stop msg: counter" << std::endl;
}
#endif
} while (g4 != half_pi
&& (prev_distance > res34.distance || prev_distance == 0)
&& delta_g4 != 0
&& ++counter < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS ) ;
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
std::cout << "distance=" << res34.distance << std::endl;
point p4(res14.lon2, res14.lat2);
CT s34_sph = str.apply(p4, p3);
std::cout << "s34(sph) =" << s34_sph << std::endl;
std::cout << "s34(geo) ="
<< inverse_distance_quantities_type::apply(get<0>(p4), get<1>(p4), lon3, lat3, spheroid).distance
<< ", p4=(" << get<0>(p4) * math::r2d<double>() << ","
<< get<1>(p4) * math::r2d<double>() << ")"
<< std::endl;
CT s31 = inverse_distance_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
CT s32 = inverse_distance_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;
CT a4 = inverse_azimuth_type::apply(get<0>(p4), get<1>(p4), lon2, lat2, spheroid).azimuth;
geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(get<0>(p4), get<1>(p4), .04, a4, spheroid);
CT p4_plus = inverse_distance_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;
geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
CT p4_minus = inverse_distance_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;
std::cout << "s31=" << s31 << "\ns32=" << s32
<< "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
<< "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
<< std::endl;
if (res34.distance <= p4_plus && res34.distance <= p4_minus)
{
std::cout << "Closest point computed" << std::endl;
}
else
{
std::cout << "There is a closer point nearby" << std::endl;
}
#endif
return result;
}
Spheroid m_spheroid;
};
#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{
//tags
template <typename FormulaPolicy>
struct tag<geographic_cross_track<FormulaPolicy> >
{
typedef strategy_tag_distance_point_segment type;
};
template
<
typename FormulaPolicy,
typename Spheroid
>
struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
{
typedef strategy_tag_distance_point_segment type;
};
template
<
typename FormulaPolicy,
typename Spheroid,
typename CalculationType
>
struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
typedef strategy_tag_distance_point_segment type;
};
//return types
template <typename FormulaPolicy, typename P, typename PS>
struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
: geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
{};
template
<
typename FormulaPolicy,
typename Spheroid,
typename P,
typename PS
>
struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
: geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
{};
template
<
typename FormulaPolicy,
typename Spheroid,
typename CalculationType,
typename P,
typename PS
>
struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
: geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
{};
//comparable types
template
<
typename FormulaPolicy,
typename Spheroid,
typename CalculationType
>
struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
typedef geographic_cross_track
<
FormulaPolicy, Spheroid, CalculationType
> type;
};
template
<
typename FormulaPolicy,
typename Spheroid,
typename CalculationType
>
struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
typedef typename comparable_type
<
geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
>::type comparable_type;
public :
static inline comparable_type
apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& )
{
return comparable_type();
}
};
template
<
typename FormulaPolicy,
typename P,
typename PS
>
struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
{
private :
typedef typename geographic_cross_track
<
FormulaPolicy
>::template return_type<P, PS>::type return_type;
public :
template <typename T>
static inline return_type
apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
{
return distance;
}
};
template <typename Point, typename PointOfSegment>
struct default_strategy
<
point_tag, segment_tag, Point, PointOfSegment,
geographic_tag, geographic_tag
>
{
typedef geographic_cross_track<> type;
};
template <typename PointOfSegment, typename Point>
struct default_strategy
<
segment_tag, point_tag, PointOfSegment, Point,
geographic_tag, geographic_tag
>
{
typedef typename default_strategy
<
point_tag, segment_tag, Point, PointOfSegment,
geographic_tag, geographic_tag
>::type type;
};
} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
}} // namespace strategy::distance
}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP