src/marnav/geo/geodesic.cpp

   1 #include <marnav/geo/geodesic.hpp>
   2 #include <cmath>
   3
   4 namespace marnav::geo
   5 {
   6
   7 /// This geodesic functions may not be the best possible, but they are
   8 /// sufficient for their purpose.
   9
  10 namespace
  11 {
  12 /// mean radius
  13 static constexpr const double earth_radius = 6378000.0; // [m]
  14
  15 /// semi-major axis according to WGS84
  16 static constexpr const double earth_semi_major_axis = 6378137.0; // [m]
  17
  18 /// flattening according to WGS84
  19 static constexpr const double earth_flattening = 1.0 / 298.257223563;
  20
  21 /// Computes the square of the specified value.
  22 template <typename T>
  23 static T sqr(const T & a)
  24 {
  25         return a * a;
  26 }
  27
  28 /// Returns the spherical angle between the two specified position in rad.
  29 ///
  30 /// @param[in] p0_lat Latitude in rad of start point.
  31 /// @param[in] p0_lon Longitude in rad of start point.
  32 /// @param[in] p1_lat Latitude in rad of destination point.
  33 /// @param[in] p1_lon Longitude in rad of destination point.
  34 /// @return Spherical angle inbetweenn in rad.
  35 static double central_spherical_angle_rad(
  36         double p0_lat, double p0_lon, double p1_lat, double p1_lon)
  37 {
  38         // not used because of potential numerical problems:
  39         // return acos(sin(p1_lat) * sin(p0_lat) + cos(p1_lat) * cos(p0_lat) * cos(p1_lon -
  40         // p0_lon));
  41
  42         return atan(
  43                 sqrt(sqr(cos(p1_lat) * sin(p1_lon - p0_lon))
  44                         + sqr(cos(p0_lat) * sin(p1_lat) - sin(p0_lat) * cos(p1_lat) * cos(p1_lon - p0_lon)))
  45                 / (sin(p0_lat) * sin(p1_lat) + cos(p0_lat) * cos(p1_lat) * cos(p1_lon - p0_lon)));
  46 }
  47
  48 static double central_spherical_angle_rad(const position & p0, const position & p1)
  49 {
  50         return central_spherical_angle_rad(p0.lat(), p0.lon(), p1.lat(), p1.lon());
  51 }
  52 }
  53
  54 /// Returns the spherical angle between the two specified position in rad.
  55 ///
  56 /// @param[in] start Start point.
  57 /// @param[in] destination Destination point.
  58 /// @return Spherical angle inbetweenn in rad.
  59 double central_spherical_angle(const position & start, const position & destination)
  60 {
  61         return central_spherical_angle_rad(deg2rad(start), deg2rad(destination));
  62 }
  63
  64 /// Calculates distance of two points on earth, approximated as sphere.
  65 ///
  66 /// @param[in] start Start point.
  67 /// @param[in] destination Destination point.
  68 /// @return Distance.
  69 distance_result distance_sphere(const position & start, const position & destination)
  70 {
  71         return distance_result{earth_radius * central_spherical_angle(start, destination)};
  72 }
  73
  74 /// Calculates the distance on an ellipsoid between start and destination points.
  75 ///
  76 /// (indirect problem)
  77 ///
  78 /// This uses the method of Vincenty (see inverse.pdf, inverse formulae,
  79 /// http://en.wikipedia.org/wiki/Vincenty%27s_formulae)
  80 ///
  81 /// @param[in] start Start point.
  82 /// @param[in] destination Destination point.
  83 /// @return Distance.
  84 distance_result distance_ellipsoid_vincenty(
  85         const position & start, const position & destination)
  86 {
  87         if (start == destination)
  88                 return {};
  89
  90         const position p0 = deg2rad(start);
  91         const position p1 = deg2rad(destination);
  92
  93         const double f = earth_flattening;
  94         const double a = earth_semi_major_axis;
  95         const double b = (1.0 - f) * a;
  96
  97         const double L = p1.lon() - p0.lon();
  98         const double U1 = atan((1.0 - f) * tan(p0.lat()));
  99         const double U2 = atan((1.0 - f) * tan(p1.lat()));
 100
 101         double lambda = L; // first approximation
 102
 103         const double sin_U1 = sin(U1);
 104         const double sin_U2 = sin(U2);
 105         const double cos_U1 = cos(U1);
 106         const double cos_U2 = cos(U2);
 107
 108         double sigma = 0.0;
 109         double sin_sigma = sin(sigma);
 110         double cos_sigma = cos(sigma);
 111         double cos_sqr_alpha = 0.0;
 112         double cos_2_sigma_m = 0.0;
 113         double sin_lambda = sin(lambda);
 114         double cos_lambda = cos(lambda);
 115
 116         double d_lambda = 0.0;
 117
 118         int iteration = 200;
 119         do {
 120                 sin_lambda = sin(lambda);
 121                 cos_lambda = cos(lambda);
 122
 123                 sin_sigma = sqrt(sqr(cos_U2 * sin_lambda)
 124                         + sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // eq 14
 125
 126                 cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // eq 15
 127
 128                 sigma = atan2(sin_sigma, cos_sigma); // eq 16
 129
 130                 double sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // eq 17
 131
 132                 cos_sqr_alpha = 1.0 - sqr(sin_alpha); // cos^2(alpha) = 1-sin^2(alpha)
 133
 134                 cos_2_sigma_m = cos_sigma - 2.0 * sin_U1 * sin_U2 / cos_sqr_alpha; // eq 18
 135                 if (std::isnan(cos_2_sigma_m))
 136                         cos_2_sigma_m = 0.0; // equatorial line
 137
 138                 double C = f / 16.0 * cos_sqr_alpha * (4.0 + f * (4.0 - 3.0 * cos_sqr_alpha)); // eq 10
 139
 140                 double old_lambda = lambda; // save current lambda to calc delta lambda
 141                 lambda = L
 142                         + (1.0 - C) * f * sin_alpha
 143                                 * (sigma
 144                                         + C * sin_sigma
 145                                                 * (cos_2_sigma_m
 146                                                         + C * cos_sigma * (-1.0 + 2.0 * sqr(cos_2_sigma_m)))); // eq 11
 147
 148                 d_lambda = std::abs(old_lambda - lambda);
 149         } while ((--iteration > 0) && (d_lambda > 1.0e-12));
 150
 151         if (iteration <= 0)
 152                 return distance_result{NAN};
 153
 154         double u_sqr = cos_sqr_alpha * (sqr(a) - sqr(b)) / sqr(b);
 155         double A = 1.0
 156                 + u_sqr / 16384.0
 157                         * (4096.0 + u_sqr * (-768.0 + u_sqr * (320.0 - 175.0 * u_sqr))); // eq 3
 158         double B
 159                 = u_sqr / 1024.0 * (256.0 + u_sqr * (-128.0 + u_sqr * (74.0 - 47.0 * u_sqr))); // eq 4
 160         double d_sigma = B * sin_sigma
 161                 * (cos_2_sigma_m + B / 4.0 * (cos_sigma * (-1.0 + 2.0 * sqr(cos_2_sigma_m)))
 162                         - B / 6.0 * cos_2_sigma_m * (-3.0 + 4.0 * sqr(sin_sigma))
 163                                 * (-3.0 + 4.0 * sqr(cos_2_sigma_m))); // eq 6
 164         double s = A * b * (sigma - d_sigma); // eq 19
 165
 166         double alpha1 = atan2(cos_U2 * sin_lambda, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
 167         double alpha2 = atan2(cos_U1 * sin_lambda, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
 168
 169         return {s, alpha1, alpha2};
 170 }
 171
 172 /// Calculates a position from a starting point in a direction and of a certain distance.
 173 ///
 174 /// (direct problem)
 175 ///
 176 /// This uses the method of Vincenty (see inverse.pdf, direct formulae,
 177 /// http://en.wikipedia.org/wiki/Vincenty%27s_formulae)
 178 ///
 179 /// @param[in] start Starting point.
 180 /// @param[in] s Distance in meters.
 181 /// @param[in] alpha1 Azimuth in rad.
 182 /// @param[out] alpha2
 183 /// @return The point at the specified distance.
 184 position point_ellipsoid_vincenty(
 185         const position & start, double s, double alpha1, double & alpha2)
 186 {
 187         if (std::abs(s) < 1.0e-4)
 188                 return start;
 189
 190         const position p0 = deg2rad(start);
 191
 192         const double f = earth_flattening;
 193         const double a = earth_semi_major_axis;
 194         const double b = (1.0 - f) * a;
 195
 196         const double sin_alpha1 = sin(alpha1);
 197         const double cos_alpha1 = cos(alpha1);
 198
 199         const double tan_U1 = (1.0 - f) * tan(p0.lat());
 200         const double U1 = atan(tan_U1);
 201
 202         const double sin_U1 = sin(U1);
 203         const double cos_U1 = cos(U1);
 204
 205         const double sigma1 = atan2(tan_U1, cos_alpha1);
 206
 207         const double sin_alpha = cos(U1) * sin_alpha1;
 208         const double sin_sqr_alpha = sqr(sin_alpha);
 209
 210         const double cos_sqr_alpha = (1.0 - sin_alpha) * (1.0 + sin_alpha);
 211
 212         const double u_sqr = cos_sqr_alpha * (sqr(a) - sqr(b)) / sqr(b);
 213
 214         const double A = 1.0
 215                 + u_sqr / 16384.0
 216                         * (4096.0 + u_sqr * (-768.0 + u_sqr * (320.0 - 175.0 * u_sqr))); // eq 3
 217
 218         const double B
 219                 = u_sqr / 1024.0 * (256.0 + u_sqr * (-128.0 + u_sqr * (74.0 - 47.0 * u_sqr))); // eq 4
 220
 221         double sigma_0 = s / (b * A);
 222         double sigma = sigma_0;
 223
 224         double d_sigma = std::abs(sigma);
 225
 226         double sin_sigma = 0.0;
 227         double cos_sigma = 0.0;
 228         double cos_2_sigma_m = 0.0;
 229         double cos_sqr_2_sigma_m = 0.0;
 230         double lambda = 0.0;
 231         double C = 0.0;
 232         double L = 0.0;
 233
 234         do {
 235                 cos_2_sigma_m = cos(2.0 * sigma1 + sigma);
 236                 cos_sqr_2_sigma_m = sqr(cos_2_sigma_m);
 237                 cos_sigma = cos(sigma);
 238                 sin_sigma = sin(sigma);
 239                 double delta_sigma = B * sin_sigma
 240                         * (cos_2_sigma_m
 241                                 + B / 4.0
 242                                         * (cos_sigma * (-1.0 + 2.0 * cos_sqr_2_sigma_m)
 243                                                 - B / 6.0 * cos_2_sigma_m * (-3.0 + 4.0 * sqr(sin_sigma))
 244                                                         * (-3.0 + 4.0 * cos_sqr_2_sigma_m)));
 245
 246                 double old_sigma = sigma;
 247                 sigma = sigma_0 + delta_sigma;
 248                 d_sigma = std::abs(old_sigma - sigma);
 249         } while (d_sigma > 1.0e-12);
 250
 251         latitude lat = atan2(sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
 252                 (1.0 - f)
 253                         * sqrt(sin_sqr_alpha + sqr(sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1)));
 254         C = (f / 16.0) * cos_sqr_alpha * (4.0 + f * (4.0 - 3.0 * cos_sqr_alpha));
 255         lambda
 256                 = atan2(sin_sigma * sin_alpha1, cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1);
 257         L = lambda
 258                 - (1.0 - C) * f * sin_alpha
 259                         * (sigma
 260                                 + C * sin_sigma
 261                                         * (cos_2_sigma_m + C * cos_sigma * (-1.0 + 2.0 * cos_sqr_2_sigma_m)));
 262         longitude lon = p0.lon() + L;
 263         alpha2 = atan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1);
 264
 265         return rad2deg(position{lat, lon});
 266 }
 267
 268 /// Calculates the distance on an ellipsoid between start and destination points.
 269 ///
 270 /// This uses the method of Lambert.
 271 ///
 272 /// @param[in] start Start point.
 273 /// @param[in] destination Destination point.
 274 /// @return Distance.
 275 distance_result distance_ellipsoid_lambert(const position & start, const position & destination)
 276 {
 277         const position p0 = deg2rad(start);
 278         const position p1 = deg2rad(destination);
 279
 280         const double r = 1.0 / earth_flattening;
 281
 282         const double t1_lat = atan((r - 1.0) / r) * tan(p0.lat());
 283         const double t1_lon = p0.lon();
 284         const double t2_lat = atan((r - 1.0) / r) * tan(p1.lat());
 285         const double t2_lon = p1.lon();
 286
 287         const double sigma = central_spherical_angle_rad(t1_lat, t1_lon, t2_lat, t2_lon);
 288         const double P = (t1_lat + t2_lat) / 2.0;
 289         const double Q = (t2_lat - t1_lat) / 2.0;
 290         const double X = (sigma - sin(sigma)) * (sqr(sin(P)) * sqr(cos(Q))) / sqr(cos(sigma / 2.0));
 291         const double Y = (sigma + sin(sigma)) * (sqr(cos(P)) * sqr(sin(Q))) / sqr(sin(sigma / 2.0));
 292
 293         return distance_result{earth_radius * (sigma - (X + Y) / (2.0 * r))};
 294 }
 295 }