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