distance_cross_track.hpp 25 KB

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