2 * This file is part of nmealib.
4 * Copyright (c) 2008 Timur Sinitsyn
5 * Copyright (c) 2011 Ferry Huberts
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.
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.
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/>.
23 #include <nmea/gmath.h>
29 * \brief Convert degree to radian
31 double nmea_degree2radian(double val)
32 { return (val * NMEA_PI180); }
35 * \brief Convert radian to degree
37 double nmea_radian2degree(double val)
38 { return (val / NMEA_PI180); }
41 * \brief Convert NDEG (NMEA degree) to fractional degree
43 double nmea_ndeg2degree(double val)
46 double fra_part = modf(val / 100.0, °);
47 return (deg + ((fra_part * 100.0) / 60.0));
51 * \brief Convert fractional degree to NDEG (NMEA degree)
53 double nmea_degree2ndeg(double val)
56 double fra_part = modf(val, °);
57 return ((deg * 100.0) + (fra_part * 60.0));
61 * \brief Convert NDEG (NMEA degree) to radian
63 double nmea_ndeg2radian(double val)
64 { return nmea_degree2radian(nmea_ndeg2degree(val)); }
67 * \brief Convert radian to NDEG (NMEA degree)
69 double nmea_radian2ndeg(double val)
70 { return nmea_degree2ndeg(nmea_radian2degree(val)); }
73 * \brief Calculate PDOP (Position Dilution Of Precision) factor
75 double nmea_calc_pdop(double hdop, double vdop)
77 return sqrt(pow(hdop, 2) + pow(vdop, 2));
80 double nmea_dop2meters(double dop)
81 { return (dop * NMEA_DOP_FACTOR); }
83 double nmea_meters2dop(double meters)
84 { return (meters / NMEA_DOP_FACTOR); }
87 * \brief Calculate distance between two points
88 * \return Distance in meters
91 const nmeaPOS *from_pos, /**< From position in radians */
92 const nmeaPOS *to_pos /**< To position in radians */
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)
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
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 */
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;
121 double sqr_u, A, B, delta_sigma, lambda_prev;
124 assert(from_pos != 0);
127 if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
128 { /* Identical points */
129 if ( from_azimuth != 0 )
131 if ( to_azimuth != 0 )
134 } /* Identical points */
137 f = NMEA_EARTH_FLATTENING;
138 a = NMEA_EARTH_SEMIMAJORAXIS_M;
144 L = to_pos->lon - from_pos->lon;
145 phi1 = from_pos->lat;
147 U1 = atan((1 - f) * tan(phi1));
148 U2 = atan((1 - f) * tan(phi2));
154 /* Initialize iteration */
156 sin_sigma = sin(sigma);
157 cos_sigma = cos(sigma);
159 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
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;
169 while ((delta_lambda > 1e-12) && (remaining_steps > 0))
172 double tmp1, tmp2, sin_alpha, cos_alpha, C;
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);
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);
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)
207 /* Calculate result */
208 if ( from_azimuth != 0 )
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);
213 if ( to_azimuth != 0 )
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);
219 return b * A * (sigma - delta_sigma);
223 * \brief Horizontal move of point position
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) */
232 nmeaPOS p1 = *start_pos;
235 distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
236 azimuth = nmea_degree2radian(azimuth);
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));
243 if(isnan(end_pos->lat) || isnan(end_pos->lon))
245 end_pos->lat = 0; end_pos->lon = 0;
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
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 */
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;
272 double tmp1, phi2, lambda, C, L;
275 assert(start_pos != 0);
276 assert(end_pos != 0);
278 if (fabs(distance) < 1e-12)
280 *end_pos = *start_pos;
281 if ( end_azimuth != 0 ) *end_azimuth = azimuth;
282 return ! (isnan(end_pos->lat) || isnan(end_pos->lon));
286 f = NMEA_EARTH_FLATTENING;
287 a = NMEA_EARTH_SEMIMAJORAXIS_M;
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;
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)));
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;
316 sigma_prev = 2 * NMEA_PI;
317 remaining_steps = 20;
319 while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
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)
331 sigma = sigma_initial + delta_sigma;
335 /* Calculate result */
336 tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
338 sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
339 (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
342 sin_sigma * sin_alpha1,
343 cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
345 C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
347 (1 - C) * f * sin_alpha * (
348 sigma + C * sin_sigma *
349 (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
353 end_pos->lon = start_pos->lon + L;
355 if ( end_azimuth != 0 )
357 *end_azimuth = atan2(
358 sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
361 return ! (isnan(end_pos->lat) || isnan(end_pos->lon));
365 * \brief Convert position from INFO to radians position
367 void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
369 pos->lat = nmea_ndeg2radian(info->lat);
370 pos->lon = nmea_ndeg2radian(info->lon);
374 * \brief Convert radians position to INFOs position
376 void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
378 info->lat = nmea_radian2ndeg(pos->lat);
379 info->lon = nmea_radian2ndeg(pos->lon);