PUD: include nmealib v0.6.7
[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;
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     delta_lambda = lambda;
166     remaining_steps = 20; 
167
168     while ((delta_lambda > 1e-12) && (remaining_steps > 0)) 
169     { /* Iterate */
170         /* Variables */
171         double tmp1, tmp2, sin_alpha, cos_alpha, C, lambda_prev;
172
173         /* Calculation */
174         tmp1 = cos_U2 * sin_lambda;
175         tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;  
176         sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);                
177         cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;   
178         sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;  
179         cos_alpha = cos(asin(sin_alpha));                 
180         sqr_cos_alpha = cos_alpha * cos_alpha;                     
181         cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
182         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; 
183         C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
184         lambda_prev = lambda; 
185         sigma = asin(sin_sigma); 
186         lambda = L + 
187             (1 - C) * f * sin_alpha
188             * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));                                                
189         delta_lambda = lambda_prev - lambda; 
190         if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; 
191         sin_lambda = sin(lambda);
192         cos_lambda = cos(lambda);
193         remaining_steps--; 
194     }  /* Iterate */
195
196     /* More calculation  */
197     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; 
198     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
199     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
200     delta_sigma = B * sin_sigma * ( 
201         cos_2_sigmam + B / 4 * ( 
202         cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
203         B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
204         ));
205
206     /* Calculate result */
207     if ( from_azimuth != 0 )
208     {
209         double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
210         *from_azimuth = atan(tan_alpha_1);
211     }
212     if ( to_azimuth != 0 )
213     {
214         double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
215         *to_azimuth = atan(tan_alpha_2);
216     }
217
218     return b * A * (sigma - delta_sigma);
219 }
220
221 /**
222  * \brief Horizontal move of point position
223  */
224 int nmea_move_horz(
225     const nmeaPOS *start_pos,   /**< Start position in radians */
226     nmeaPOS *end_pos,           /**< Result position in radians */
227     double azimuth,             /**< Azimuth (degree) [0, 359] */
228     double distance             /**< Distance (km) */
229     )
230 {
231     nmeaPOS p1 = *start_pos;
232     int RetVal = 1;
233
234     distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
235     azimuth = nmea_degree2radian(azimuth);
236
237     end_pos->lat = asin(
238         sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth));
239     end_pos->lon = p1.lon + atan2(
240         sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat));
241
242     if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon))
243     {
244         end_pos->lat = 0; end_pos->lon = 0;
245         RetVal = 0;
246     }
247
248     return RetVal;
249 }
250
251 /**
252  * \brief Horizontal move of point position
253  * This function uses an algorithm for an oblate spheroid earth model.
254  * The algorithm is described here: 
255  * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
256  */
257 int nmea_move_horz_ellipsoid(
258     const nmeaPOS *start_pos,   /**< Start position in radians */
259     nmeaPOS *end_pos,           /**< (O) Result position in radians */
260     double azimuth,             /**< Azimuth in radians */
261     double distance,            /**< Distance (km) */
262     double *end_azimuth         /**< (O) Azimuth at end position in radians */
263     )
264 {
265     /* Variables */
266     double f, a, b, sqr_a, sqr_b;
267     double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1;
268     double sigma1, sin_alpha, sqr_cos_alpha, sqr_u, A, B;
269     double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma;
270     int remaining_steps;
271     double tmp1, phi2, lambda, C, L;
272     
273     /* Check input */
274     NMEA_ASSERT(start_pos != 0);
275     NMEA_ASSERT(end_pos != 0);
276     
277     if (fabs(distance) < 1e-12)
278     { /* No move */
279         *end_pos = *start_pos;
280         if ( end_azimuth != 0 ) *end_azimuth = azimuth;
281         return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
282     } /* No move */
283
284     /* Earth geometry */
285     f = NMEA_EARTH_FLATTENING;
286     a = NMEA_EARTH_SEMIMAJORAXIS_M;
287     b = (1 - f) * a;
288     sqr_a = a * a;
289     sqr_b = b * b;
290     
291     /* Calculation */
292     phi1 = start_pos->lat;
293     tan_U1 = (1 - f) * tan(phi1);
294     cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1);
295     sin_U1 = tan_U1 * cos_U1;
296     s = distance;
297     alpha1 = azimuth;
298     sin_alpha1 = sin(alpha1);
299     cos_alpha1 = cos(alpha1);
300     sigma1 = atan2(tan_U1, cos_alpha1);
301     sin_alpha = cos_U1 * sin_alpha1;
302     sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
303     sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; 
304     A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
305     B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
306     
307     /* Initialize iteration */
308     sigma_initial = s / (b * A);
309     sigma = sigma_initial;
310     sin_sigma = sin(sigma);
311     cos_sigma = cos(sigma);
312     cos_2_sigmam = cos(2 * sigma1 + sigma);
313     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
314     delta_sigma = 0;
315     sigma_prev = 2 * NMEA_PI;
316     remaining_steps = 20;
317
318     while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
319     { /* Iterate */
320         cos_2_sigmam = cos(2 * sigma1 + sigma);
321         sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
322         sin_sigma = sin(sigma);
323         cos_sigma = cos(sigma);
324         delta_sigma = B * sin_sigma * ( 
325              cos_2_sigmam + B / 4 * ( 
326              cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - 
327              B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
328              ));
329         sigma_prev = sigma;
330         sigma = sigma_initial + delta_sigma;
331         remaining_steps --;
332     } /* Iterate */
333     
334     /* Calculate result */
335     tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
336     phi2 = atan2(
337             sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
338             (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
339             );
340     lambda = atan2(
341             sin_sigma * sin_alpha1,
342             cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
343             );
344     C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
345     L = lambda -
346         (1 - C) * f * sin_alpha * (
347         sigma + C * sin_sigma *
348         (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
349         );
350     
351     /* Result */
352     end_pos->lon = start_pos->lon + L;
353     end_pos->lat = phi2;
354     if ( end_azimuth != 0 )
355     {
356         *end_azimuth = atan2(
357             sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
358             );
359     }
360     return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon));
361 }
362
363 /**
364  * \brief Convert position from INFO to radians position
365  */
366 void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
367 {
368     pos->lat = nmea_ndeg2radian(info->lat);
369     pos->lon = nmea_ndeg2radian(info->lon);
370 }
371
372 /**
373  * \brief Convert radians position to INFOs position
374  */
375 void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
376 {
377     info->lat = nmea_radian2ndeg(pos->lat);
378     info->lon = nmea_radian2ndeg(pos->lon);
379 }