diff options
Diffstat (limited to 'boost/geometry/strategies/spherical/distance_cross_track.hpp')
| -rw-r--r-- | boost/geometry/strategies/spherical/distance_cross_track.hpp | 47 |
1 files changed, 40 insertions, 7 deletions
diff --git a/boost/geometry/strategies/spherical/distance_cross_track.hpp b/boost/geometry/strategies/spherical/distance_cross_track.hpp index ba589223ec..7b353020eb 100644 --- a/boost/geometry/strategies/spherical/distance_cross_track.hpp +++ b/boost/geometry/strategies/spherical/distance_cross_track.hpp @@ -101,23 +101,56 @@ public : { // http://williams.best.vwh.net/avform.htm#XTE return_type d1 = m_strategy.apply(sp1, p); + return_type d3 = m_strategy.apply(sp1, sp2); + + if (geometry::math::equals(d3, 0.0)) + { + // "Degenerate" segment, return either d1 or d2 + return d1; + } - // Actually, calculation of d2 not necessary if we know that the projected point is on the great circle... return_type d2 = m_strategy.apply(sp2, p); return_type crs_AD = course(sp1, p); return_type crs_AB = course(sp1, sp2); - return_type XTD = m_radius * geometry::math::abs(asin(sin(d1 / m_radius) * sin(crs_AD - crs_AB))); + return_type crs_BA = crs_AB - geometry::math::pi<return_type>(); + return_type crs_BD = course(sp2, p); + return_type d_crs1 = crs_AD - crs_AB; + return_type d_crs2 = crs_BD - crs_BA; + + // d1, d2, d3 are in principle not needed, only the sign matters + return_type projection1 = cos( d_crs1 ) * d1 / d3; + return_type projection2 = cos( d_crs2 ) * d2 / d3; #ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK -std::cout << "Course " << dsv(sp1) << " to " << dsv(p) << " " << crs_AD * geometry::math::r2d << std::endl; -std::cout << "Course " << dsv(sp1) << " to " << dsv(sp2) << " " << crs_AB * geometry::math::r2d << std::endl; -std::cout << "XTD: " << XTD << " d1: " << d1 << " d2: " << d2 << std::endl; + std::cout << "Course " << dsv(sp1) << " to " << dsv(p) << " " << crs_AD * geometry::math::r2d << std::endl; + std::cout << "Course " << dsv(sp1) << " to " << dsv(sp2) << " " << crs_AB * geometry::math::r2d << std::endl; + std::cout << "Course " << dsv(sp2) << " to " << dsv(p) << " " << crs_BD * geometry::math::r2d << std::endl; + std::cout << "Projection AD-AB " << projection1 << " : " << d_crs1 * geometry::math::r2d << std::endl; + std::cout << "Projection BD-BA " << projection2 << " : " << d_crs2 * geometry::math::r2d << std::endl; #endif + if(projection1 > 0.0 && projection2 > 0.0) + { + return_type XTD = m_radius * geometry::math::abs( asin( sin( d1 / m_radius ) * sin( d_crs1 ) )); + + #ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK + std::cout << "Projection ON the segment" << std::endl; + std::cout << "XTD: " << XTD << " d1: " << d1 << " d2: " << d2 << std::endl; +#endif + + // Return shortest distance, projected point on segment sp1-sp2 + return return_type(XTD); + } + else + { +#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK + std::cout << "Projection OUTSIDE the segment" << std::endl; +#endif - // Return shortest distance, either to projected point on segment sp1-sp2, or to sp1, or to sp2 - return return_type((std::min)((std::min)(d1, d2), XTD)); + // Return shortest distance, project either on point sp1 or sp2 + return return_type( (std::min)( d1 , d2 ) ); + } } inline return_type radius() const { return m_radius; } |