distance_cross_track.hpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739
  1. // Boost.Geometry (aka GGL, Generic Geometry Library)
  2. // Copyright (c) 2016-2018, Oracle and/or its affiliates.
  3. // Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
  4. // Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
  5. // Use, modification and distribution is subject to the Boost Software License,
  6. // Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
  7. // http://www.boost.org/LICENSE_1_0.txt)
  8. #ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
  9. #define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
  10. #include <algorithm>
  11. #include <boost/tuple/tuple.hpp>
  12. #include <boost/algorithm/minmax.hpp>
  13. #include <boost/config.hpp>
  14. #include <boost/concept_check.hpp>
  15. #include <boost/mpl/if.hpp>
  16. #include <boost/type_traits/is_void.hpp>
  17. #include <boost/geometry/core/cs.hpp>
  18. #include <boost/geometry/core/access.hpp>
  19. #include <boost/geometry/core/radian_access.hpp>
  20. #include <boost/geometry/core/tags.hpp>
  21. #include <boost/geometry/strategies/distance.hpp>
  22. #include <boost/geometry/strategies/concepts/distance_concept.hpp>
  23. #include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
  24. #include <boost/geometry/strategies/spherical/distance_haversine.hpp>
  25. #include <boost/geometry/strategies/spherical/point_in_point.hpp>
  26. #include <boost/geometry/strategies/geographic/azimuth.hpp>
  27. #include <boost/geometry/strategies/geographic/distance.hpp>
  28. #include <boost/geometry/strategies/geographic/parameters.hpp>
  29. #include <boost/geometry/formulas/vincenty_direct.hpp>
  30. #include <boost/geometry/util/math.hpp>
  31. #include <boost/geometry/util/promote_floating_point.hpp>
  32. #include <boost/geometry/util/select_calculation_type.hpp>
  33. #include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
  34. #include <boost/geometry/formulas/result_direct.hpp>
  35. #include <boost/geometry/formulas/mean_radius.hpp>
  36. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  37. #include <boost/geometry/io/dsv/write.hpp>
  38. #endif
  39. #ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
  40. #define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
  41. #endif
  42. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  43. #include <iostream>
  44. #endif
  45. namespace boost { namespace geometry
  46. {
  47. namespace strategy { namespace distance
  48. {
  49. /*!
  50. \brief Strategy functor for distance point to segment calculation on ellipsoid
  51. Algorithm uses direct and inverse geodesic problems as subroutines.
  52. The algorithm approximates the distance by an iterative Newton method.
  53. \ingroup strategies
  54. \details Class which calculates the distance of a point to a segment, for points
  55. on the ellipsoid
  56. \see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
  57. https://arxiv.org/abs/1102.1215
  58. \tparam FormulaPolicy underlying point-point distance strategy
  59. \tparam Spheroid is the spheroidal model used
  60. \tparam CalculationType \tparam_calculation
  61. \tparam EnableClosestPoint computes the closest point on segment if true
  62. */
  63. template
  64. <
  65. typename FormulaPolicy = strategy::andoyer,
  66. typename Spheroid = srs::spheroid<double>,
  67. typename CalculationType = void,
  68. bool EnableClosestPoint = false
  69. >
  70. class geographic_cross_track
  71. {
  72. public :
  73. typedef within::spherical_point_point equals_point_point_strategy_type;
  74. template <typename Point, typename PointOfSegment>
  75. struct return_type
  76. : promote_floating_point
  77. <
  78. typename select_calculation_type
  79. <
  80. Point,
  81. PointOfSegment,
  82. CalculationType
  83. >::type
  84. >
  85. {};
  86. explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
  87. : m_spheroid(spheroid)
  88. {}
  89. template <typename Point, typename PointOfSegment>
  90. inline typename return_type<Point, PointOfSegment>::type
  91. apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
  92. {
  93. typedef typename geometry::detail::cs_angular_units<Point>::type units_type;
  94. return (apply<units_type>(get<0>(sp1), get<1>(sp1),
  95. get<0>(sp2), get<1>(sp2),
  96. get<0>(p), get<1>(p),
  97. m_spheroid)).distance;
  98. }
  99. // points on a meridian not crossing poles
  100. template <typename CT>
  101. inline CT vertical_or_meridian(CT lat1, CT lat2) const
  102. {
  103. typedef typename formula::meridian_inverse
  104. <
  105. CT,
  106. strategy::default_order<FormulaPolicy>::value
  107. > meridian_inverse;
  108. return meridian_inverse::meridian_not_crossing_pole_dist(lat1, lat2,
  109. m_spheroid);
  110. }
  111. private :
  112. template <typename CT>
  113. struct result_distance_point_segment
  114. {
  115. result_distance_point_segment()
  116. : distance(0)
  117. , closest_point_lon(0)
  118. , closest_point_lat(0)
  119. {}
  120. CT distance;
  121. CT closest_point_lon;
  122. CT closest_point_lat;
  123. };
  124. template <typename CT>
  125. result_distance_point_segment<CT>
  126. static inline non_iterative_case(CT lon, CT lat, CT distance)
  127. {
  128. result_distance_point_segment<CT> result;
  129. result.distance = distance;
  130. if (EnableClosestPoint)
  131. {
  132. result.closest_point_lon = lon;
  133. result.closest_point_lat = lat;
  134. }
  135. return result;
  136. }
  137. template <typename CT>
  138. result_distance_point_segment<CT>
  139. static inline non_iterative_case(CT lon1, CT lat1, //p1
  140. CT lon2, CT lat2, //p2
  141. Spheroid const& spheroid)
  142. {
  143. CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
  144. ::apply(lon1, lat1, lon2, lat2, spheroid);
  145. return non_iterative_case(lon1, lat1, distance);
  146. }
  147. template <typename CT>
  148. CT static inline normalize(CT g4, CT& der)
  149. {
  150. CT const pi = math::pi<CT>();
  151. if (g4 < -1.25*pi)//close to -270
  152. {
  153. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  154. std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -270" << std::endl;
  155. #endif
  156. return g4 + 1.5 * pi;
  157. }
  158. else if (g4 > 1.25*pi)//close to 270
  159. {
  160. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  161. std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to 270" << std::endl;
  162. #endif
  163. der = -der;
  164. return - g4 + 1.5 * pi;
  165. }
  166. else if (g4 < 0 && g4 > -0.75*pi)//close to -90
  167. {
  168. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  169. std::cout << "g4=" << g4 * math::r2d<CT>() << ", close to -90" << std::endl;
  170. #endif
  171. der = -der;
  172. return -g4 - pi/2;
  173. }
  174. return g4 - pi/2;
  175. }
  176. template <typename Units, typename CT>
  177. result_distance_point_segment<CT>
  178. static inline apply(CT lon1, CT lat1, //p1
  179. CT lon2, CT lat2, //p2
  180. CT lon3, CT lat3, //query point p3
  181. Spheroid const& spheroid)
  182. {
  183. typedef typename FormulaPolicy::template inverse<CT, true, true, false, true, true>
  184. inverse_distance_azimuth_quantities_type;
  185. typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
  186. inverse_azimuth_type;
  187. typedef typename FormulaPolicy::template inverse<CT, false, true, true, false, false>
  188. inverse_azimuth_reverse_type;
  189. typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
  190. direct_distance_type;
  191. CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);
  192. result_distance_point_segment<CT> result;
  193. // Constants
  194. //CT const f = geometry::formula::flattening<CT>(spheroid);
  195. CT const pi = math::pi<CT>();
  196. CT const half_pi = pi / CT(2);
  197. CT const c0 = CT(0);
  198. // Convert to radians
  199. lon1 = math::as_radian<Units>(lon1);
  200. lat1 = math::as_radian<Units>(lat1);
  201. lon2 = math::as_radian<Units>(lon2);
  202. lat2 = math::as_radian<Units>(lat2);
  203. lon3 = math::as_radian<Units>(lon3);
  204. lat3 = math::as_radian<Units>(lat3);
  205. if (lon1 > lon2)
  206. {
  207. std::swap(lon1, lon2);
  208. std::swap(lat1, lat2);
  209. }
  210. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  211. std::cout << ">>\nSegment=(" << lon1 * math::r2d<CT>();
  212. std::cout << "," << lat1 * math::r2d<CT>();
  213. std::cout << "),(" << lon2 * math::r2d<CT>();
  214. std::cout << "," << lat2 * math::r2d<CT>();
  215. std::cout << ")\np=(" << lon3 * math::r2d<CT>();
  216. std::cout << "," << lat3 * math::r2d<CT>();
  217. std::cout << ")" << std::endl;
  218. #endif
  219. //segment on equator
  220. //Note: antipodal points on equator does not define segment on equator
  221. //but pass by the pole
  222. CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);
  223. typedef typename formula::meridian_inverse<CT>
  224. meridian_inverse;
  225. bool meridian_not_crossing_pole =
  226. meridian_inverse::meridian_not_crossing_pole
  227. (lat1, lat2, diff);
  228. bool meridian_crossing_pole =
  229. meridian_inverse::meridian_crossing_pole(diff);
  230. if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole)
  231. {
  232. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  233. std::cout << "Equatorial segment" << std::endl;
  234. std::cout << "segment=(" << lon1 * math::r2d<CT>();
  235. std::cout << "," << lat1 * math::r2d<CT>();
  236. std::cout << "),(" << lon2 * math::r2d<CT>();
  237. std::cout << "," << lat2 * math::r2d<CT>();
  238. std::cout << ")\np=(" << lon3 * math::r2d<CT>();
  239. std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
  240. #endif
  241. if (lon3 <= lon1)
  242. {
  243. return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
  244. }
  245. if (lon3 >= lon2)
  246. {
  247. return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
  248. }
  249. return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
  250. }
  251. if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && std::abs(lat1) > std::abs(lat2))
  252. {
  253. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  254. std::cout << "Meridian segment not crossing pole" << std::endl;
  255. #endif
  256. std::swap(lat1,lat2);
  257. }
  258. if (meridian_crossing_pole)
  259. {
  260. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  261. std::cout << "Meridian segment crossing pole" << std::endl;
  262. #endif
  263. CT sign_non_zero = lat3 >= c0 ? 1 : -1;
  264. result_distance_point_segment<CT> d1 =
  265. apply<geometry::radian>(lon1, lat1,
  266. lon1, half_pi * sign_non_zero,
  267. lon3, lat3, spheroid);
  268. result_distance_point_segment<CT> d2 =
  269. apply<geometry::radian>(lon2, lat2,
  270. lon2, half_pi * sign_non_zero,
  271. lon3, lat3, spheroid);
  272. if (d1.distance < d2.distance)
  273. {
  274. return d1;
  275. }
  276. else
  277. {
  278. return d2;
  279. }
  280. }
  281. CT d1 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
  282. ::apply(lon1, lat1, lon3, lat3, spheroid);
  283. CT d3 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
  284. ::apply(lon1, lat1, lon2, lat2, spheroid);
  285. if (geometry::math::equals(d3, c0))
  286. {
  287. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  288. std::cout << "Degenerate segment" << std::endl;
  289. std::cout << "distance between points=" << d1 << std::endl;
  290. #endif
  291. return non_iterative_case(lon1, lat2, d1);
  292. }
  293. CT d2 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
  294. ::apply(lon2, lat2, lon3, lat3, spheroid);
  295. // Compute a12 (GEO)
  296. geometry::formula::result_inverse<CT> res12 =
  297. inverse_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
  298. CT a12 = res12.azimuth;
  299. CT a13 = inverse_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid).azimuth;
  300. CT a312 = a13 - a12;
  301. // TODO: meridian case optimization
  302. if (geometry::math::equals(a312, c0) && meridian_not_crossing_pole)
  303. {
  304. boost::tuple<CT,CT> minmax_elem = boost::minmax(lat1, lat2);
  305. if (lat3 >= minmax_elem.template get<0>() &&
  306. lat3 <= minmax_elem.template get<1>())
  307. {
  308. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  309. std::cout << "Point on meridian segment" << std::endl;
  310. #endif
  311. return non_iterative_case(lon3, lat3, c0);
  312. }
  313. }
  314. CT projection1 = cos( a312 ) * d1 / d3;
  315. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  316. std::cout << "a1=" << a12 * math::r2d<CT>() << std::endl;
  317. std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl;
  318. std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
  319. std::cout << "cos(a312)=" << cos(a312) << std::endl;
  320. std::cout << "projection 1=" << projection1 << std::endl;
  321. #endif
  322. if (projection1 < c0)
  323. {
  324. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  325. std::cout << "projection closer to p1" << std::endl;
  326. #endif
  327. // projection of p3 on geodesic spanned by segment (p1,p2) fall
  328. // outside of segment on the side of p1
  329. return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
  330. }
  331. CT a21 = res12.reverse_azimuth - pi;
  332. CT a23 = inverse_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid).azimuth;
  333. CT a321 = a23 - a21;
  334. CT projection2 = cos( a321 ) * d2 / d3;
  335. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  336. std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl;
  337. std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl;
  338. std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
  339. std::cout << "cos(a321)=" << cos(a321) << std::endl;
  340. std::cout << "projection 2=" << projection2 << std::endl;
  341. #endif
  342. if (projection2 < c0)
  343. {
  344. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  345. std::cout << "projection closer to p2" << std::endl;
  346. #endif
  347. // projection of p3 on geodesic spanned by segment (p1,p2) fall
  348. // outside of segment on the side of p2
  349. return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
  350. }
  351. // Guess s14 (SPHERICAL) aka along-track distance
  352. typedef geometry::model::point
  353. <
  354. CT, 2,
  355. geometry::cs::spherical_equatorial<geometry::radian>
  356. > point;
  357. point p1 = point(lon1, lat1);
  358. point p2 = point(lon2, lat2);
  359. point p3 = point(lon3, lat3);
  360. geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
  361. CT s34 = cross_track.apply(p3, p1, p2);
  362. geometry::strategy::distance::haversine<CT> str(earth_radius);
  363. CT s13 = str.apply(p1, p3);
  364. //CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;
  365. CT cos_frac = cos(s13/earth_radius) / cos(s34/earth_radius);
  366. CT s14 = cos_frac >= 1 ? CT(0)
  367. : cos_frac <= -1 ? pi * earth_radius
  368. : acos(cos_frac) * earth_radius;
  369. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  370. std::cout << "s34=" << s34 << std::endl;
  371. std::cout << "s13=" << s13 << std::endl;
  372. std::cout << "s14=" << s14 << std::endl;
  373. std::cout << "===============" << std::endl;
  374. #endif
  375. // Update s14 (using Newton method)
  376. CT prev_distance;
  377. geometry::formula::result_direct<CT> res14;
  378. geometry::formula::result_inverse<CT> res34;
  379. res34.distance = -1;
  380. int counter = 0; // robustness
  381. CT g4;
  382. CT delta_g4;
  383. bool dist_improve = true;
  384. do{
  385. prev_distance = res34.distance;
  386. // Solve the direct problem to find p4 (GEO)
  387. res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);
  388. // Solve an inverse problem to find g4
  389. // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)
  390. CT a4 = inverse_azimuth_type::apply(res14.lon2, res14.lat2,
  391. lon2, lat2, spheroid).azimuth;
  392. res34 = inverse_distance_azimuth_quantities_type::apply(res14.lon2, res14.lat2,
  393. lon3, lat3, spheroid);
  394. g4 = res34.azimuth - a4;
  395. CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
  396. CT m34 = res34.reduced_length;
  397. CT der = (M43 / m34) * sin(g4);
  398. //normalize
  399. delta_g4 = normalize(g4, der);
  400. s14 = s14 - delta_g4 / der;
  401. result.distance = res34.distance;
  402. dist_improve = prev_distance > res34.distance || prev_distance == -1;
  403. if (!dist_improve)
  404. {
  405. result.distance = prev_distance;
  406. }
  407. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  408. std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
  409. "," << res14.lat2 * math::r2d<CT>() << std::endl;
  410. std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
  411. std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
  412. std::cout << "g4(normalized)=" << g4 * math::r2d<CT>() << std::endl;
  413. std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>() << std::endl;
  414. std::cout << "der=" << der << std::endl;
  415. std::cout << "M43=" << M43 << std::endl;
  416. std::cout << "spherical limit=" << cos(s14/earth_radius) << std::endl;
  417. std::cout << "m34=" << m34 << std::endl;
  418. std::cout << "new_s14=" << s14 << std::endl;
  419. std::cout << std::setprecision(16) << "dist =" << res34.distance << std::endl;
  420. std::cout << "---------end of step " << counter << std::endl<< std::endl;
  421. if (g4 == half_pi)
  422. {
  423. std::cout << "Stop msg: g4 == half_pi" << std::endl;
  424. }
  425. if (!dist_improve)
  426. {
  427. std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
  428. }
  429. if (delta_g4 == 0)
  430. {
  431. std::cout << "Stop msg: delta_g4 == 0" << std::endl;
  432. }
  433. if (counter == BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS)
  434. {
  435. std::cout << "Stop msg: counter" << std::endl;
  436. }
  437. #endif
  438. } while (g4 != half_pi
  439. && dist_improve
  440. && delta_g4 != 0
  441. && counter++ < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS);
  442. #ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
  443. std::cout << "distance=" << res34.distance << std::endl;
  444. point p4(res14.lon2, res14.lat2);
  445. CT s34_sph = str.apply(p4, p3);
  446. std::cout << "s34(sph) =" << s34_sph << std::endl;
  447. std::cout << "s34(geo) ="
  448. << inverse_distance_azimuth_quantities_type::apply(get<0>(p4), get<1>(p4), lon3, lat3, spheroid).distance
  449. << ", p4=(" << get<0>(p4) * math::r2d<double>() << ","
  450. << get<1>(p4) * math::r2d<double>() << ")"
  451. << std::endl;
  452. CT s31 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
  453. CT s32 = inverse_distance_azimuth_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;
  454. CT a4 = inverse_azimuth_type::apply(get<0>(p4), get<1>(p4), lon2, lat2, spheroid).azimuth;
  455. geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(get<0>(p4), get<1>(p4), .04, a4, spheroid);
  456. CT p4_plus = inverse_distance_azimuth_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;
  457. geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
  458. CT p4_minus = inverse_distance_azimuth_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;
  459. std::cout << "s31=" << s31 << "\ns32=" << s32
  460. << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
  461. << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
  462. << std::endl;
  463. if (res34.distance <= p4_plus && res34.distance <= p4_minus)
  464. {
  465. std::cout << "Closest point computed" << std::endl;
  466. }
  467. else
  468. {
  469. std::cout << "There is a closer point nearby" << std::endl;
  470. }
  471. #endif
  472. return result;
  473. }
  474. Spheroid m_spheroid;
  475. };
  476. #ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
  477. namespace services
  478. {
  479. //tags
  480. template <typename FormulaPolicy>
  481. struct tag<geographic_cross_track<FormulaPolicy> >
  482. {
  483. typedef strategy_tag_distance_point_segment type;
  484. };
  485. template
  486. <
  487. typename FormulaPolicy,
  488. typename Spheroid
  489. >
  490. struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
  491. {
  492. typedef strategy_tag_distance_point_segment type;
  493. };
  494. template
  495. <
  496. typename FormulaPolicy,
  497. typename Spheroid,
  498. typename CalculationType
  499. >
  500. struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
  501. {
  502. typedef strategy_tag_distance_point_segment type;
  503. };
  504. //return types
  505. template <typename FormulaPolicy, typename P, typename PS>
  506. struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
  507. : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
  508. {};
  509. template
  510. <
  511. typename FormulaPolicy,
  512. typename Spheroid,
  513. typename P,
  514. typename PS
  515. >
  516. struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
  517. : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
  518. {};
  519. template
  520. <
  521. typename FormulaPolicy,
  522. typename Spheroid,
  523. typename CalculationType,
  524. typename P,
  525. typename PS
  526. >
  527. struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
  528. : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
  529. {};
  530. //comparable types
  531. template
  532. <
  533. typename FormulaPolicy,
  534. typename Spheroid,
  535. typename CalculationType
  536. >
  537. struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
  538. {
  539. typedef geographic_cross_track
  540. <
  541. FormulaPolicy, Spheroid, CalculationType
  542. > type;
  543. };
  544. template
  545. <
  546. typename FormulaPolicy,
  547. typename Spheroid,
  548. typename CalculationType
  549. >
  550. struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
  551. {
  552. public :
  553. static inline geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
  554. apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& strategy)
  555. {
  556. return strategy;
  557. }
  558. };
  559. template
  560. <
  561. typename FormulaPolicy,
  562. typename P,
  563. typename PS
  564. >
  565. struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
  566. {
  567. private :
  568. typedef typename geographic_cross_track
  569. <
  570. FormulaPolicy
  571. >::template return_type<P, PS>::type return_type;
  572. public :
  573. template <typename T>
  574. static inline return_type
  575. apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
  576. {
  577. return distance;
  578. }
  579. };
  580. template
  581. <
  582. typename FormulaPolicy,
  583. typename Spheroid,
  584. typename CalculationType,
  585. typename P,
  586. typename PS
  587. >
  588. struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
  589. {
  590. private :
  591. typedef typename geographic_cross_track
  592. <
  593. FormulaPolicy, Spheroid, CalculationType
  594. >::template return_type<P, PS>::type return_type;
  595. public :
  596. template <typename T>
  597. static inline return_type
  598. apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
  599. {
  600. return distance;
  601. }
  602. };
  603. template <typename Point, typename PointOfSegment>
  604. struct default_strategy
  605. <
  606. point_tag, segment_tag, Point, PointOfSegment,
  607. geographic_tag, geographic_tag
  608. >
  609. {
  610. typedef geographic_cross_track<> type;
  611. };
  612. template <typename PointOfSegment, typename Point>
  613. struct default_strategy
  614. <
  615. segment_tag, point_tag, PointOfSegment, Point,
  616. geographic_tag, geographic_tag
  617. >
  618. {
  619. typedef typename default_strategy
  620. <
  621. point_tag, segment_tag, Point, PointOfSegment,
  622. geographic_tag, geographic_tag
  623. >::type type;
  624. };
  625. } // namespace services
  626. #endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
  627. }} // namespace strategy::distance
  628. }} // namespace boost::geometry
  629. #endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP