Algorithm.cpp 3.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596
  1. //==============================================================================
  2. //
  3. // Algorithm - the algorithm module
  4. //
  5. // Copyright (C) 2018 Dick van Oudheusden
  6. //
  7. // This library is free software; you can redistribute it and/or
  8. // modify it under the terms of the GNU Lesser General Public
  9. // License as published by the Free Software Foundation; either
  10. // version 3 of the License, or (at your option) any later version.
  11. //
  12. // This library is distributed in the hope that it will be useful,
  13. // but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  15. // Lesser General Public License for more details.
  16. //
  17. // You should have received a copy of the GNU Lesser General Public
  18. // License along with this library; if not, write to the Free
  19. // Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20. //
  21. //==============================================================================
  22. #include <cmath>
  23. #include "Algorithm.h"
  24. namespace gpx
  25. {
  26. double deg2rad(double deg)
  27. {
  28. return (deg * M_PI) / 180.0;
  29. }
  30. double rad2deg(double rad)
  31. {
  32. return (rad * 180.0) / M_PI;
  33. }
  34. // http://www.movable-type.co.uk/scripts/latlong.html
  35. // distance in metres
  36. double calcDistance(double lat1deg, double lon1deg, double lat2deg, double lon2deg)
  37. {
  38. const double R = 6371E3; // m
  39. double lat1rad = deg2rad(lat1deg);
  40. double lon1rad = deg2rad(lon1deg);
  41. double lat2rad = deg2rad(lat2deg);
  42. double lon2rad = deg2rad(lon2deg);
  43. double dlat = lat2rad - lat1rad;
  44. double dlon = lon2rad - lon1rad;
  45. double a = sin(dlat / 2.0) * sin(dlat / 2.0) + cos(lat1rad) * cos(lat2rad) * sin(dlon / 2.0) * sin(dlon / 2.0);
  46. double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));
  47. return c * R;
  48. }
  49. // std::cout << "Distance: " << gpx::calcDistance(50.06639, -5.71472, 58.64389, -3.07000) << std::endl; // 968853.52
  50. // bearing in rads
  51. double calcBearing(double lat1deg, double lon1deg, double lat2deg, double lon2deg)
  52. {
  53. double lat1rad = deg2rad(lat1deg);
  54. double lon1rad = deg2rad(lon1deg);
  55. double lat2rad = deg2rad(lat2deg);
  56. double lon2rad = deg2rad(lon2deg);
  57. double dlon = lon2rad - lon1rad;
  58. double y = sin(dlon) * cos(lat2rad);
  59. double x = cos(lat1rad) * sin(lat2rad) - sin(lat1rad) * cos(lat2rad) * cos(dlon);
  60. return atan2(y, x);
  61. }
  62. // std::cout << "Bearing: " << gpx::calcBearing(50.06639, -5.71472, 58.64389, -3.07000) << std::endl; // 0.16
  63. double calcBearingInDeg(double lat1deg, double lon1deg, double lat2deg, double lon2deg)
  64. {
  65. return std::fmod((gpx::calcBearing(lat1deg, lon1deg, lat2deg, lon2deg) * 180.0) / M_PI + 360.0, 360.0);
  66. }
  67. // crosstrack distance in metres from point3 to the point1-point2 line
  68. double calcCrosstrack(double lat1deg, double lon1deg, double lat2deg, double lon2deg, double lat3deg, double lon3deg)
  69. {
  70. const double R = 6371E3; // m
  71. double distance13 = calcDistance(lat1deg, lon1deg, lat3deg, lon3deg) / R;
  72. double bearing13 = calcBearing(lat1deg, lon1deg, lat3deg, lon3deg);
  73. double bearing12 = calcBearing(lat1deg, lon1deg, lat2deg, lon2deg);
  74. return asin(sin(distance13) * sin(bearing13 - bearing12)) * R;
  75. }
  76. // std::cout << "Crosstrack:" << gpx::calcCrosstrack(53.3206, -1.7297, 53.1887, 0.1334, 53.2611, -0.7972) << std::endl; // -307.55
  77. }