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