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