PUD: nmealib: get rid of NMEA_ASSERT
[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 #include <assert.h>
27
28 /**
29  * \brief Convert degree to radian
30  */
31 double nmea_degree2radian(double val)
32 { return (val * NMEA_PI180); }
33
34 /**
35  * \brief Convert radian to degree
36  */
37 double nmea_radian2degree(double val)
38 { return (val / NMEA_PI180); }
39
40 /**
41  * \brief Convert NDEG (NMEA degree) to fractional degree
42  */
43 double nmea_ndeg2degree(double val)
44 {
45     double deg;
46     double fra_part = modf(val / 100.0, &deg);
47     return (deg + ((fra_part * 100.0) / 60.0));
48 }
49
50 /**
51  * \brief Convert fractional degree to NDEG (NMEA degree)
52  */
53 double nmea_degree2ndeg(double val)
54 {
55     double deg;
56     double fra_part = modf(val, &deg);
57     return ((deg * 100.0) + (fra_part * 60.0));
58 }
59
60 /**
61  * \brief Convert NDEG (NMEA degree) to radian
62  */
63 double nmea_ndeg2radian(double val)
64 { return nmea_degree2radian(nmea_ndeg2degree(val)); }
65
66 /**
67  * \brief Convert radian to NDEG (NMEA degree)
68  */
69 double nmea_radian2ndeg(double val)
70 { return nmea_degree2ndeg(nmea_radian2degree(val)); }
71
72 /**
73  * \brief Calculate PDOP (Position Dilution Of Precision) factor
74  */
75 double nmea_calc_pdop(double hdop, double vdop)
76 {
77     return sqrt(pow(hdop, 2) + pow(vdop, 2));
78 }
79
80 double nmea_dop2meters(double dop)
81 { return (dop * NMEA_DOP_FACTOR); }
82
83 double nmea_meters2dop(double meters)
84 { return (meters / NMEA_DOP_FACTOR); }
85
86 /**
87  * \brief Calculate distance between two points
88  * \return Distance in meters
89  */
90 double nmea_distance(
91         const nmeaPOS *from_pos,    /**< From position in radians */
92         const nmeaPOS *to_pos       /**< To position in radians */
93         )
94 {
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)
98         );
99     return dist;
100 }
101
102 /**
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
108  */
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 */
114         )
115 {
116     /* All variables */
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;
120     int remaining_steps; 
121     double sqr_u, A, B, delta_sigma, lambda_prev;
122
123     /* Check input */
124     assert(from_pos != 0);
125     assert(to_pos != 0);
126
127     if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon))
128     { /* Identical points */
129         if ( from_azimuth != 0 )
130             *from_azimuth = 0;
131         if ( to_azimuth != 0 )
132             *to_azimuth = 0;
133         return 0;    
134     } /* Identical points */
135
136     /* Earth geometry */
137     f = NMEA_EARTH_FLATTENING;
138     a = NMEA_EARTH_SEMIMAJORAXIS_M;
139     b = (1 - f) * a;
140     sqr_a = a * a;
141     sqr_b = b * b;
142
143     /* Calculation */
144     L = to_pos->lon - from_pos->lon;
145     phi1 = from_pos->lat;
146     phi2 = to_pos->lat;
147     U1 = atan((1 - f) * tan(phi1));
148     U2 = atan((1 - f) * tan(phi2));
149     sin_U1 = sin(U1);
150     sin_U2 = sin(U2);
151     cos_U1 = cos(U1);
152     cos_U2 = cos(U2);
153
154     /* Initialize iteration */
155     sigma = 0;
156     sin_sigma = sin(sigma);
157     cos_sigma = cos(sigma);
158     cos_2_sigmam = 0;
159     sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
160     sqr_cos_alpha = 0;
161     lambda = L;
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; 
168
169     while ((delta_lambda > 1e-12) && (remaining_steps > 0)) 
170     { /* Iterate */
171         /* Variables */
172         double tmp1, tmp2, sin_alpha, cos_alpha, C;
173
174         /* Calculation */
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); 
187         lambda = L + 
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);
194         remaining_steps--; 
195     }  /* Iterate */
196
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)
205         ));
206
207     /* Calculate result */
208     if ( from_azimuth != 0 )
209     {
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);
212     }
213     if ( to_azimuth != 0 )
214     {
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);
217     }
218
219     return b * A * (sigma - delta_sigma);
220 }
221
222 /**
223  * \brief Horizontal move of point position
224  */
225 int nmea_move_horz(
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) */
230     )
231 {
232     nmeaPOS p1 = *start_pos;
233     int RetVal = 1;
234
235     distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
236     azimuth = nmea_degree2radian(azimuth);
237
238     end_pos->lat = asin(
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));
242
243     if(isnan(end_pos->lat) || isnan(end_pos->lon))
244     {
245         end_pos->lat = 0; end_pos->lon = 0;
246         RetVal = 0;
247     }
248
249     return RetVal;
250 }
251
252 /**
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
257  */
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 */
264     )
265 {
266     /* Variables */
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;
271     int remaining_steps;
272     double tmp1, phi2, lambda, C, L;
273     
274     /* Check input */
275     assert(start_pos != 0);
276     assert(end_pos != 0);
277     
278     if (fabs(distance) < 1e-12)
279     { /* No move */
280         *end_pos = *start_pos;
281         if ( end_azimuth != 0 ) *end_azimuth = azimuth;
282         return ! (isnan(end_pos->lat) || isnan(end_pos->lon));
283     } /* No move */
284
285     /* Earth geometry */
286     f = NMEA_EARTH_FLATTENING;
287     a = NMEA_EARTH_SEMIMAJORAXIS_M;
288     b = (1 - f) * a;
289     sqr_a = a * a;
290     sqr_b = b * b;
291     
292     /* Calculation */
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;
297     s = distance;
298     alpha1 = azimuth;
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)));
307     
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;
315     delta_sigma = 0;
316     sigma_prev = 2 * NMEA_PI;
317     remaining_steps = 20;
318
319     while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0))
320     { /* Iterate */
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)
329              ));
330         sigma_prev = sigma;
331         sigma = sigma_initial + delta_sigma;
332         remaining_steps --;
333     } /* Iterate */
334     
335     /* Calculate result */
336     tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1);
337     phi2 = atan2(
338             sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
339             (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1)
340             );
341     lambda = atan2(
342             sin_sigma * sin_alpha1,
343             cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
344             );
345     C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
346     L = lambda -
347         (1 - C) * f * sin_alpha * (
348         sigma + C * sin_sigma *
349         (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))
350         );
351     
352     /* Result */
353     end_pos->lon = start_pos->lon + L;
354     end_pos->lat = phi2;
355     if ( end_azimuth != 0 )
356     {
357         *end_azimuth = atan2(
358             sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
359             );
360     }
361     return ! (isnan(end_pos->lat) || isnan(end_pos->lon));
362 }
363
364 /**
365  * \brief Convert position from INFO to radians position
366  */
367 void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos)
368 {
369     pos->lat = nmea_ndeg2radian(info->lat);
370     pos->lon = nmea_ndeg2radian(info->lon);
371 }
372
373 /**
374  * \brief Convert radians position to INFOs position
375  */
376 void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info)
377 {
378     info->lat = nmea_radian2ndeg(pos->lat);
379     info->lon = nmea_radian2ndeg(pos->lon);
380 }