gmath.c

00001 /*
00002  *
00003  * NMEA library
00004  * URL: http://nmea.sourceforge.net
00005  * Author: Tim (xtimor@gmail.com)
00006  * Licence: http://www.gnu.org/licenses/lgpl.html
00007  * $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $
00008  *
00009  The original gmath.c has been modified to fix a bug in:
00010   nmea_distance_ellipsoid() function
00011   according to bug report ID: 2945855
00012  
00013  http://sourceforge.net/tracker/?func=detail&aid=2945855&group_id=192054&atid=939854  
00014       
00015  // while ((delta_lambda > 1e-12) && (remaining_steps > 0))  original by xtimor
00016 while (( remaining_steps == 20 ) || ((fabs(delta_lambda) > 1e-12) && (remaining_steps > 0)))
00017     
00018     the original code always returns a zero distance if the arrival point longitude
00019     is equal or smaller than the starting point one.
00020 
00021  */
00022 
00023 /*! \file gmath.h */
00024 
00025 #include "nmea/gmath.h "
00026 
00027 #include <math.h>
00028 #include <float.h>
00029 
00030 /**
00031  * \fn nmea_degree2radian
00032  * \brief Convert degree to radian
00033  */
00034 double nmea_degree2radian(double val)
00035 { return (val * NMEA_PI180); }
00036 
00037 /**
00038  * \fn nmea_radian2degree
00039  * \brief Convert radian to degree
00040  */
00041 double nmea_radian2degree(double val)
00042 { return (val / NMEA_PI180); }
00043 
00044 /**
00045  * \brief Convert NDEG (NMEA degree) to fractional degree
00046  */
00047 double nmea_ndeg2degree(double val)
00048 {
00049     double deg = ((int)(val / 100));
00050     val = deg + (val - deg * 100) / 60;
00051     return val;
00052 }
00053 
00054 /**
00055  * \brief Convert fractional degree to NDEG (NMEA degree)
00056  */
00057 double nmea_degree2ndeg(double val)
00058 {
00059     double int_part;
00060     double fra_part;
00061     fra_part = modf(val, &int_part);
00062     val = int_part * 100 + fra_part * 60;
00063     return val;
00064 }
00065 
00066 /**
00067  * \fn nmea_ndeg2radian
00068  * \brief Convert NDEG (NMEA degree) to radian
00069  */
00070 double nmea_ndeg2radian(double val)
00071 { return nmea_degree2radian(nmea_ndeg2degree(val)); }
00072 
00073 /**
00074  * \fn nmea_radian2ndeg
00075  * \brief Convert radian to NDEG (NMEA degree)
00076  */
00077 double nmea_radian2ndeg(double val)
00078 { return nmea_degree2ndeg(nmea_radian2degree(val)); }
00079 
00080 /**
00081  * \brief Calculate PDOP (Position Dilution Of Precision) factor
00082  */
00083 double nmea_calc_pdop(double hdop, double vdop)
00084 {
00085     return sqrt(pow(hdop, 2) + pow(vdop, 2));
00086 }
00087 
00088 double nmea_dop2meters(double dop)
00089 { return (dop * NMEA_DOP_FACTOR); }
00090 
00091 double nmea_meters2dop(double meters)
00092 { return (meters / NMEA_DOP_FACTOR); }
00093 
00094 /**
00095  * \brief Calculate distance between two points
00096  * \return Distance in meters
00097  */
00098 double nmea_distance(
00099         const nmeaPOS *from_pos,    /**< From position in radians */
00100         const nmeaPOS *to_pos       /**< To position in radians */
00101         )
00102 {
00103     double dist = ((double)NMEA_EARTHRADIUS_M) * acos(
00104         sin(to_pos->lat) * sin(from_pos->lat) +
00105         cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon)
00106         );
00107     return dist;
00108 }
00109 
00110 /**
00111  * \brief Calculate distance between two points
00112  * This function uses an algorithm for an oblate spheroid earth model.
00113  * The algorithm is described here: 
00114  * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
00115  * \return Distance in meters
00116  */
00117 double nmea_distance_ellipsoid(
00118         const nmeaPOS *from_pos,    /**< From position in radians */
00119         const nmeaPOS *to_pos,      /**< To position in radians */
00120         double *from_azimuth,       /**< (O) azimuth at "from" position in radians */
00121         double *to_azimuth          /**< (O) azimuth at "to" position in radians */
00122         )
00123 {
00124     /* All variables */
00125     double f, a, b, sqr_a, sqr_b;
00126     double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
00127     double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
00128     int remaining_steps; 
00129     double sqr_u, A, B, delta_sigma;
00130 
00131     /* Check input */
00132     NMEA_ASSERT(from_pos != 0);
00133     NMEA_ASSERT(to_pos != 0);
00134 
00135     if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
00136     { /* Identical points */
00137         if ( from_azimuth != 0 )
00138             *from_azimuth = 0;
00139         if ( to_azimuth != 0 )
00140             *to_azimuth = 0;
00141         return 0;    
00142     } /* Identical points */
00143 
00144     /* Earth geometry */
00145     f = NMEA_EARTH_FLATTENING;
00146     a = NMEA_EARTH_SEMIMAJORAXIS_M;
00147     b = (1 - f) * a;
00148     sqr_a = a * a;
00149     sqr_b = b * b;
00150 
00151     /* Calculation */
00152     L = to_pos->lon - from_pos->lon;
00153     phi1 = from_pos->lat;
00154     phi2 = to_pos->lat;
00155     U1 = atan((1 - f) * tan(phi1));
00156     U2 = atan((1 - f) * tan(phi2));
00157     sin_U1 = sin(U1);
00158     sin_U2 = sin(U2);
00159     cos_U1 = cos(U1);
00160     cos_U2 = cos(U2);
00161 
00162     /* Initialize iteration */
00163     sigma = 0;
00164     sin_sigma = sin(sigma);
00165     cos_sigma = cos(sigma);
00166     cos_2_sigmam = 0;
00167     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
00168     sqr_cos_alpha = 0;
00169     lambda = L;
00170     sin_lambda = sin(lambda);                            
00171     cos_lambda = cos(lambda);                       
00172     delta_lambda = lambda;
00173     remaining_steps = 20; 
00174 
00175   // while ((delta_lambda > 1e-12) && (remaining_steps > 0))  original by xtimor
00176     while (( remaining_steps == 20 ) || ((fabs(delta_lambda) > 1e-12) && (remaining_steps > 0)))
00177     { /* Iterate */
00178         /* Variables */
00179         double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; 
00180 
00181         /* Calculation */
00182         tmp1 = cos_U2 * sin_lambda;
00183         tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;  
00184         sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);                
00185         cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;   
00186         tan_sigma = sin_sigma / cos_sigma;                  
00187         sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;  
00188         cos_alpha = cos(asin(sin_alpha));                 
00189         sqr_cos_alpha = cos_alpha * cos_alpha;                     
00190         cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
00191         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; 
00192         C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
00193         lambda_prev = lambda; 
00194         sigma = asin(sin_sigma); 
00195         lambda = L + 
00196             (1 - C) * f * sin_alpha
00197             * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));                                                
00198         delta_lambda = lambda_prev - lambda; 
00199         if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; 
00200         sin_lambda = sin(lambda);
00201         cos_lambda = cos(lambda);
00202         remaining_steps--; 
00203     }  /* Iterate */
00204 
00205     /* More calculation  */
00206     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; 
00207     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
00208     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
00209     delta_sigma = B * sin_sigma * ( 
00210         cos_2_sigmam + B / 4 * ( 
00211         cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
00212         B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
00213         ));
00214 
00215     /* Calculate result */
00216     if ( from_azimuth != 0 )
00217     {
00218         double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
00219         *from_azimuth = atan(tan_alpha_1);
00220     }
00221     if ( to_azimuth != 0 )
00222     {
00223         double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
00224         *to_azimuth = atan(tan_alpha_2);
00225     }
00226 
00227     return b * A * (sigma - delta_sigma);
00228 }
00229 
00230 /**
00231  * \brief Horizontal move of point position
00232  */
00233 int nmea_move_horz(
00234     const nmeaPOS *start_pos,   /**< Start position in radians */
00235     nmeaPOS *end_pos,           /**< Result position in radians */
00236     double azimuth,             /**< Azimuth (degree) [0, 359] */
00237     double distance             /**< Distance (km) */
00238     )
00239 {
00240     nmeaPOS p1 = *start_pos;
00241     int RetVal = 1;
00242 
00243     distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
00244     azimuth = nmea_degree2radian(azimuth);
00245 
00246     end_pos->lat = asin(
00247         sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
00248     end_pos->lon = p1.lon + atan2(
00249         sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
00250 
00251     if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
00252     {
00253         end_pos->lat = 0; end_pos->lon = 0;
00254         RetVal = 0;
00255     }
00256 
00257     return RetVal;
00258 }
00259 
00260 /**
00261  * \brief Horizontal move of point position
00262  * This function uses an algorithm for an oblate spheroid earth model.
00263  * The algorithm is described here: 
00264  * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
00265  */
00266 int nmea_move_horz_ellipsoid(
00267     const nmeaPOS *start_pos,   /**< Start position in radians */
00268     nmeaPOS *end_pos,           /**< (O) Result position in radians */
00269     double azimuth,             /**< Azimuth in radians */
00270     double distance,            /**< Distance (km) */
00271     double *end_azimuth         /**< (O) Azimuth at end position in radians */
00272     )
00273 {
00274     /* Variables */
00275     double f, a, b, sqr_a, sqr_b;
00276     double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
00277     double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B;
00278     double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
00279     int remaining_steps;
00280     double tmp1, phi2, lambda, C, L;
00281     
00282     /* Check input */
00283     NMEA_ASSERT(start_pos != 0);
00284     NMEA_ASSERT(end_pos != 0);
00285     
00286     if (fabs(distance) < 1e-12)
00287     { /* No move */
00288         *end_pos = *start_pos;
00289         if ( end_azimuth != 0 ) *end_azimuth = azimuth;
00290         return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
00291     } /* No move */
00292 
00293     /* Earth geometry */
00294     f = NMEA_EARTH_FLATTENING;
00295     a = NMEA_EARTH_SEMIMAJORAXIS_M;
00296     b = (1 - f) * a;
00297     sqr_a = a * a;
00298     sqr_b = b * b;
00299     
00300     /* Calculation */
00301     phi1 = start_pos->lat;
00302     tan_U1 = (1 - f) * tan(phi1);
00303     cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
00304     sin_U1 = tan_U1 * cos_U1;
00305     s = distance;
00306     alpha1 = azimuth;
00307     sin_alpha1 = sin(alpha1);
00308     cos_alpha1 = cos(alpha1);
00309     tan_sigma1 = tan_U1 / cos_alpha1;
00310     sigma1 = atan2(tan_U1, cos_alpha1);
00311     sin_alpha = cos_U1 * sin_alpha1;
00312     sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
00313     cos_alpha = sqrt(sqr_cos_alpha);
00314     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; 
00315     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
00316     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
00317     
00318     /* Initialize iteration */
00319     sigma_initial = s / (b * A);
00320     sigma = sigma_initial;
00321     sin_sigma = sin(sigma);
00322     cos_sigma = cos(sigma);
00323     cos_2_sigmam = cos(2 * sigma1 + sigma);
00324     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
00325     delta_sigma = 0;
00326     sigma_prev = 2 * NMEA_PI;
00327     remaining_steps = 20;
00328 
00329     while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
00330     { /* Iterate */
00331         cos_2_sigmam = cos(2 * sigma1 + sigma);
00332         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
00333         sin_sigma = sin(sigma);
00334         cos_sigma = cos(sigma);
00335         delta_sigma = B * sin_sigma * ( 
00336              cos_2_sigmam + B / 4 * ( 
00337              cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - 
00338              B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
00339              ));
00340         sigma_prev = sigma;
00341         sigma = sigma_initial + delta_sigma;
00342         remaining_steps --;
00343     } /* Iterate */
00344     
00345     /* Calculate result */
00346     tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
00347     phi2 = atan2(
00348             sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
00349             (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
00350             );
00351     lambda = atan2(
00352             sin_sigma * sin_alpha1,
00353             cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
00354             );
00355     C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
00356     L = lambda -
00357         (1 - C) * f * sin_alpha * (
00358         sigma + C * sin_sigma *
00359         (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
00360         );
00361     
00362     /* Result */
00363     end_pos->lon = start_pos->lon + L;
00364     end_pos->lat = phi2;
00365     if ( end_azimuth != 0 )
00366     {
00367         *end_azimuth = atan2(
00368             sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
00369             );
00370     }
00371     return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
00372 }
00373 
00374 /**
00375  * \brief Convert position from INFO to radians position
00376  */
00377 void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
00378 {
00379     pos->lat = nmea_ndeg2radian(info->lat);
00380     pos->lon = nmea_ndeg2radian(info->lon);
00381 }
00382 
00383 /**
00384  * \brief Convert radians position to INFOs position
00385  */
00386 void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
00387 {
00388     info->lat = nmea_radian2ndeg(pos->lat);
00389     info->lon = nmea_radian2ndeg(pos->lon);
00390 }