lib/pud/nmealib/src/gmath.c

   1 /*
   2  * This file is part of nmealib.
   3  *
   4  * Copyright (c) 2008 Timur Sinitsyn
   5  * Copyright (c) 2011 Ferry Huberts
   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 2.1 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 General Public License
  18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*! \file gmath.h */
  22
  23 #include <nmea/gmath.h>
  24
  25 #include <math.h>
  26 #include <assert.h>
  27
  28 /**
  29  * \brief Convert degree to radian
  30  */
  31 double nmea_degree2radian(double val)
  32 { return (val * NMEA_PI180); }
  33
  34 /**
  35  * \brief Convert radian to degree
  36  */
  37 double nmea_radian2degree(double val)
  38 { return (val / NMEA_PI180); }
  39
  40 /**
  41  * \brief Convert NDEG (NMEA degree) to fractional degree
  42  */
  43 double nmea_ndeg2degree(double val)
  44 {
  45     double deg;
  46     double fra_part = modf(val / 100.0, &deg);
  47     return (deg + ((fra_part * 100.0) / 60.0));
  48 }
  49
  50 /**
  51  * \brief Convert fractional degree to NDEG (NMEA degree)
  52  */
  53 double nmea_degree2ndeg(double val)
  54 {
  55     double deg;
  56     double fra_part = modf(val, &deg);
  57     return ((deg * 100.0) + (fra_part * 60.0));
  58 }
  59
  60 /**
  61  * \brief Convert NDEG (NMEA degree) to radian
  62  */
  63 double nmea_ndeg2radian(double val)
  64 { return nmea_degree2radian(nmea_ndeg2degree(val)); }
  65
  66 /**
  67  * \brief Convert radian to NDEG (NMEA degree)
  68  */
  69 double nmea_radian2ndeg(double val)
  70 { return nmea_degree2ndeg(nmea_radian2degree(val)); }
  71
  72 /**
  73  * \brief Calculate PDOP (Position Dilution Of Precision) factor
  74  */
  75 double nmea_calc_pdop(double hdop, double vdop)
  76 {
  77     return sqrt(pow(hdop, 2) + pow(vdop, 2));
  78 }
  79
  80 double nmea_dop2meters(double dop)
  81 { return (dop * NMEA_DOP_FACTOR); }
  82
  83 double nmea_meters2dop(double meters)
  84 { return (meters / NMEA_DOP_FACTOR); }
  85
  86 /**
  87  * \brief Calculate distance between two points
  88  * \return Distance in meters
  89  */
  90 double nmea_distance(
  91         const nmeaPOS *from_pos,    /**< From position in radians */
  92         const nmeaPOS *to_pos       /**< To position in radians */
  93         )
  94 {
  95     double dist = ((double)NMEA_EARTHRADIUS_M) * acos(
  96         sin(to_pos->lat) * sin(from_pos->lat) +
  97         cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
  98         );
  99     return dist;
 100 }
 101
 102 /**
 103  * \brief Calculate distance between two points
 104  * This function uses an algorithm for an oblate spheroid earth model.
 105  * The algorithm is described here:
 106  * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
 107  * \return Distance in meters
 108  */
 109 double nmea_distance_ellipsoid(
 110         const nmeaPOS *from_pos,    /**< From position in radians */
 111         const nmeaPOS *to_pos,      /**< To position in radians */
 112         double *from_azimuth,       /**< (O) azimuth at "from" position in radians */
 113         double *to_azimuth          /**< (O) azimuth at "to" position in radians */
 114         )
 115 {
 116     /* All variables */
 117     double f, a, b, sqr_a, sqr_b;
 118     double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
 119     double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
 120     int remaining_steps;
 121     double sqr_u, A, B, delta_sigma, lambda_prev;
 122
 123     /* Check input */
 124     assert(from_pos != 0);
 125     assert(to_pos != 0);
 126
 127     if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
 128     { /* Identical points */
 129         if ( from_azimuth != 0 )
 130             *from_azimuth = 0;
 131         if ( to_azimuth != 0 )
 132             *to_azimuth = 0;
 133         return 0;
 134     } /* Identical points */
 135
 136     /* Earth geometry */
 137     f = NMEA_EARTH_FLATTENING;
 138     a = NMEA_EARTH_SEMIMAJORAXIS_M;
 139     b = (1 - f) * a;
 140     sqr_a = a * a;
 141     sqr_b = b * b;
 142
 143     /* Calculation */
 144     L = to_pos->lon - from_pos->lon;
 145     phi1 = from_pos->lat;
 146     phi2 = to_pos->lat;
 147     U1 = atan((1 - f) * tan(phi1));
 148     U2 = atan((1 - f) * tan(phi2));
 149     sin_U1 = sin(U1);
 150     sin_U2 = sin(U2);
 151     cos_U1 = cos(U1);
 152     cos_U2 = cos(U2);
 153
 154     /* Initialize iteration */
 155     sigma = 0;
 156     sin_sigma = sin(sigma);
 157     cos_sigma = cos(sigma);
 158     cos_2_sigmam = 0;
 159     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
 160     sqr_cos_alpha = 0;
 161     lambda = L;
 162     sin_lambda = sin(lambda);
 163     cos_lambda = cos(lambda);
 164     lambda_prev = (double)2.0 * (double)NMEA_PI;
 165     delta_lambda = lambda_prev - lambda;
 166     if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
 167     remaining_steps = 20;
 168
 169     while ((delta_lambda > 1e-12) && (remaining_steps > 0))
 170     { /* Iterate */
 171         /* Variables */
 172         double tmp1, tmp2, sin_alpha, cos_alpha, C;
 173
 174         /* Calculation */
 175         tmp1 = cos_U2 * sin_lambda;
 176         tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
 177         sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
 178         cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
 179         sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
 180         cos_alpha = cos(asin(sin_alpha));
 181         sqr_cos_alpha = cos_alpha * cos_alpha;
 182         cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
 183         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
 184         C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
 185         lambda_prev = lambda;
 186         sigma = asin(sin_sigma);
 187         lambda = L +
 188             (1 - C) * f * sin_alpha
 189             * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
 190         delta_lambda = lambda_prev - lambda;
 191         if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
 192         sin_lambda = sin(lambda);
 193         cos_lambda = cos(lambda);
 194         remaining_steps--;
 195     }  /* Iterate */
 196
 197     /* More calculation  */
 198     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
 199     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
 200     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
 201     delta_sigma = B * sin_sigma * (
 202         cos_2_sigmam + B / 4 * (
 203         cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
 204         B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
 205         ));
 206
 207     /* Calculate result */
 208     if ( from_azimuth != 0 )
 209     {
 210         double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
 211         *from_azimuth = atan(tan_alpha_1);
 212     }
 213     if ( to_azimuth != 0 )
 214     {
 215         double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
 216         *to_azimuth = atan(tan_alpha_2);
 217     }
 218
 219     return b * A * (sigma - delta_sigma);
 220 }
 221
 222 /**
 223  * \brief Horizontal move of point position
 224  */
 225 int nmea_move_horz(
 226     const nmeaPOS *start_pos,   /**< Start position in radians */
 227     nmeaPOS *end_pos,           /**< Result position in radians */
 228     double azimuth,             /**< Azimuth (degree) [0, 359] */
 229     double distance             /**< Distance (km) */
 230     )
 231 {
 232     nmeaPOS p1 = *start_pos;
 233     int RetVal = 1;
 234
 235     distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
 236     azimuth = nmea_degree2radian(azimuth);
 237
 238     end_pos->lat = asin(
 239         sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
 240     end_pos->lon = p1.lon + atan2(
 241         sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
 242
 243     if(isnan(end_pos->lat) || isnan(end_pos->lon))
 244     {
 245         end_pos->lat = 0; end_pos->lon = 0;
 246         RetVal = 0;
 247     }
 248
 249     return RetVal;
 250 }
 251
 252 /**
 253  * \brief Horizontal move of point position
 254  * This function uses an algorithm for an oblate spheroid earth model.
 255  * The algorithm is described here:
 256  * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
 257  */
 258 int nmea_move_horz_ellipsoid(
 259     const nmeaPOS *start_pos,   /**< Start position in radians */
 260     nmeaPOS *end_pos,           /**< (O) Result position in radians */
 261     double azimuth,             /**< Azimuth in radians */
 262     double distance,            /**< Distance (km) */
 263     double *end_azimuth         /**< (O) Azimuth at end position in radians */
 264     )
 265 {
 266     /* Variables */
 267     double f, a, b, sqr_a, sqr_b;
 268     double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
 269     double sigma1, sin_alpha, sqr_cos_alpha, sqr_u, A, B;
 270     double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
 271     int remaining_steps;
 272     double tmp1, phi2, lambda, C, L;
 273
 274     /* Check input */
 275     assert(start_pos != 0);
 276     assert(end_pos != 0);
 277
 278     if (fabs(distance) < 1e-12)
 279     { /* No move */
 280         *end_pos = *start_pos;
 281         if ( end_azimuth != 0 ) *end_azimuth = azimuth;
 282         return ! (isnan(end_pos->lat) || isnan(end_pos->lon));
 283     } /* No move */
 284
 285     /* Earth geometry */
 286     f = NMEA_EARTH_FLATTENING;
 287     a = NMEA_EARTH_SEMIMAJORAXIS_M;
 288     b = (1 - f) * a;
 289     sqr_a = a * a;
 290     sqr_b = b * b;
 291
 292     /* Calculation */
 293     phi1 = start_pos->lat;
 294     tan_U1 = (1 - f) * tan(phi1);
 295     cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
 296     sin_U1 = tan_U1 * cos_U1;
 297     s = distance;
 298     alpha1 = azimuth;
 299     sin_alpha1 = sin(alpha1);
 300     cos_alpha1 = cos(alpha1);
 301     sigma1 = atan2(tan_U1, cos_alpha1);
 302     sin_alpha = cos_U1 * sin_alpha1;
 303     sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
 304     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
 305     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
 306     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
 307
 308     /* Initialize iteration */
 309     sigma_initial = s / (b * A);
 310     sigma = sigma_initial;
 311     sin_sigma = sin(sigma);
 312     cos_sigma = cos(sigma);
 313     cos_2_sigmam = cos(2 * sigma1 + sigma);
 314     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
 315     delta_sigma = 0;
 316     sigma_prev = 2 * NMEA_PI;
 317     remaining_steps = 20;
 318
 319     while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
 320     { /* Iterate */
 321         cos_2_sigmam = cos(2 * sigma1 + sigma);
 322         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
 323         sin_sigma = sin(sigma);
 324         cos_sigma = cos(sigma);
 325         delta_sigma = B * sin_sigma * (
 326              cos_2_sigmam + B / 4 * (
 327              cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
 328              B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
 329              ));
 330         sigma_prev = sigma;
 331         sigma = sigma_initial + delta_sigma;
 332         remaining_steps --;
 333     } /* Iterate */
 334
 335     /* Calculate result */
 336     tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
 337     phi2 = atan2(
 338             sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
 339             (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
 340             );
 341     lambda = atan2(
 342             sin_sigma * sin_alpha1,
 343             cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
 344             );
 345     C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
 346     L = lambda -
 347         (1 - C) * f * sin_alpha * (
 348         sigma + C * sin_sigma *
 349         (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
 350         );
 351
 352     /* Result */
 353     end_pos->lon = start_pos->lon + L;
 354     end_pos->lat = phi2;
 355     if ( end_azimuth != 0 )
 356     {
 357         *end_azimuth = atan2(
 358             sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
 359             );
 360     }
 361     return ! (isnan(end_pos->lat) || isnan(end_pos->lon));
 362 }
 363
 364 /**
 365  * \brief Convert position from INFO to radians position
 366  */
 367 void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
 368 {
 369     pos->lat = nmea_ndeg2radian(info->lat);
 370     pos->lon = nmea_ndeg2radian(info->lon);
 371 }
 372
 373 /**
 374  * \brief Convert radians position to INFOs position
 375  */
 376 void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
 377 {
 378     info->lat = nmea_radian2ndeg(pos->lat);
 379     info->lon = nmea_radian2ndeg(pos->lon);
 380 }