Branch data Line data Source code
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 : : */
38 : 0 : double nmea_degree2radian( double val )
39 : 0 : { return ( val * NMEA_PI180 ); }
40 : :
41 : : /**
42 : : * \fn nmea_radian2degree
43 : : * \brief Convert radian to degree
44 : : */
45 : 0 : double nmea_radian2degree( double val )
46 : 0 : { return ( val / NMEA_PI180 ); }
47 : :
48 : : /**
49 : : * \brief Convert NDEG (NMEA degree) to fractional degree
50 : : */
51 : 0 : double nmea_ndeg2degree( double val )
52 : : {
53 : 0 : double deg = ( ( int )( val / 100 ) );
54 : 0 : val = deg + ( val - deg * 100 ) / 60;
55 : 0 : return val;
56 : : }
57 : :
58 : : /**
59 : : * \brief Convert fractional degree to NDEG (NMEA degree)
60 : : */
61 : 0 : double nmea_degree2ndeg( double val )
62 : : {
63 : : double int_part;
64 : : double fra_part;
65 : 0 : fra_part = modf( val, &int_part );
66 : 0 : val = int_part * 100 + fra_part * 60;
67 : 0 : return val;
68 : : }
69 : :
70 : : /**
71 : : * \fn nmea_ndeg2radian
72 : : * \brief Convert NDEG (NMEA degree) to radian
73 : : */
74 : 0 : double nmea_ndeg2radian( double val )
75 : 0 : { return nmea_degree2radian( nmea_ndeg2degree( val ) ); }
76 : :
77 : : /**
78 : : * \fn nmea_radian2ndeg
79 : : * \brief Convert radian to NDEG (NMEA degree)
80 : : */
81 : 0 : double nmea_radian2ndeg( double val )
82 : 0 : { return nmea_degree2ndeg( nmea_radian2degree( val ) ); }
83 : :
84 : : /**
85 : : * \brief Calculate PDOP (Position Dilution Of Precision) factor
86 : : */
87 : 0 : double nmea_calc_pdop( double hdop, double vdop )
88 : : {
89 : 0 : return sqrt( pow( hdop, 2 ) + pow( vdop, 2 ) );
90 : : }
91 : :
92 : 0 : double nmea_dop2meters( double dop )
93 : 0 : { return ( dop * NMEA_DOP_FACTOR ); }
94 : :
95 : 0 : double nmea_meters2dop( double meters )
96 : 0 : { return ( meters / NMEA_DOP_FACTOR ); }
97 : :
98 : : /**
99 : : * \brief Calculate distance between two points
100 : : * \return Distance in meters
101 : : */
102 : 0 : double nmea_distance(
103 : : const nmeaPOS *from_pos, //!< From position in radians
104 : : const nmeaPOS *to_pos //!< To position in radians
105 : : )
106 : : {
107 : 0 : double dist = ( ( double )NMEA_EARTHRADIUS_M ) * acos(
108 : 0 : sin( to_pos->lat ) * sin( from_pos->lat ) +
109 : 0 : cos( to_pos->lat ) * cos( from_pos->lat ) * cos( to_pos->lon - from_pos->lon )
110 : : );
111 : 0 : 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 : : */
121 : 0 : 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 : 0 : NMEA_ASSERT( from_pos != 0 );
137 : 0 : NMEA_ASSERT( to_pos != 0 );
138 : :
139 : 0 : if ( ( from_pos->lat == to_pos->lat ) && ( from_pos->lon == to_pos->lon ) )
140 : : {
141 : : /* Identical points */
142 : 0 : if ( from_azimuth != 0 )
143 : 0 : *from_azimuth = 0;
144 : 0 : if ( to_azimuth != 0 )
145 : 0 : *to_azimuth = 0;
146 : 0 : return 0;
147 : : } /* Identical points */
148 : :
149 : : /* Earth geometry */
150 : 0 : f = NMEA_EARTH_FLATTENING;
151 : 0 : a = NMEA_EARTH_SEMIMAJORAXIS_M;
152 : 0 : b = ( 1 - f ) * a;
153 : 0 : sqr_a = a * a;
154 : 0 : sqr_b = b * b;
155 : :
156 : : /* Calculation */
157 : 0 : L = to_pos->lon - from_pos->lon;
158 : 0 : phi1 = from_pos->lat;
159 : 0 : phi2 = to_pos->lat;
160 : 0 : U1 = atan( ( 1 - f ) * tan( phi1 ) );
161 : 0 : U2 = atan( ( 1 - f ) * tan( phi2 ) );
162 : 0 : sin_U1 = sin( U1 );
163 : 0 : sin_U2 = sin( U2 );
164 : 0 : cos_U1 = cos( U1 );
165 : 0 : cos_U2 = cos( U2 );
166 : :
167 : : /* Initialize iteration */
168 : 0 : sigma = 0;
169 : 0 : sin_sigma = sin( sigma );
170 : 0 : cos_sigma = cos( sigma );
171 : 0 : cos_2_sigmam = 0;
172 : 0 : sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
173 : 0 : sqr_cos_alpha = 0;
174 : 0 : lambda = L;
175 : 0 : sin_lambda = sin( lambda );
176 : 0 : cos_lambda = cos( lambda );
177 : 0 : delta_lambda = lambda;
178 : 0 : remaining_steps = 20;
179 : :
180 : 0 : 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 : 0 : tmp1 = cos_U2 * sin_lambda;
188 : 0 : tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
189 : 0 : sin_sigma = sqrt( tmp1 * tmp1 + tmp2 * tmp2 );
190 : 0 : cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
191 : 0 : sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
192 : 0 : cos_alpha = cos( asin( sin_alpha ) );
193 : 0 : sqr_cos_alpha = cos_alpha * cos_alpha;
194 : 0 : cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
195 : 0 : sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
196 : 0 : C = f / 16 * sqr_cos_alpha * ( 4 + f * ( 4 - 3 * sqr_cos_alpha ) );
197 : 0 : lambda_prev = lambda;
198 : 0 : sigma = asin( sin_sigma );
199 : 0 : lambda = L +
200 : 0 : ( 1 - C ) * f * sin_alpha
201 : 0 : * ( sigma + C * sin_sigma * ( cos_2_sigmam + C * cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) ) );
202 : 0 : delta_lambda = lambda_prev - lambda;
203 : 0 : if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
204 : 0 : sin_lambda = sin( lambda );
205 : 0 : cos_lambda = cos( lambda );
206 : 0 : remaining_steps--;
207 : : } /* Iterate */
208 : :
209 : : /* More calculation */
210 : 0 : sqr_u = sqr_cos_alpha * ( sqr_a - sqr_b ) / sqr_b;
211 : 0 : A = 1 + sqr_u / 16384 * ( 4096 + sqr_u * ( -768 + sqr_u * ( 320 - 175 * sqr_u ) ) );
212 : 0 : B = sqr_u / 1024 * ( 256 + sqr_u * ( -128 + sqr_u * ( 74 - 47 * sqr_u ) ) );
213 : 0 : delta_sigma = B * sin_sigma * (
214 : 0 : cos_2_sigmam + B / 4 * (
215 : 0 : cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) -
216 : 0 : B / 6 * cos_2_sigmam * ( -3 + 4 * sin_sigma * sin_sigma ) * ( -3 + 4 * sqr_cos_2_sigmam )
217 : : ) );
218 : :
219 : : /* Calculate result */
220 : 0 : if ( from_azimuth != 0 )
221 : : {
222 : 0 : double tan_alpha_1 = cos_U2 * sin_lambda / ( cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda );
223 : 0 : *from_azimuth = atan( tan_alpha_1 );
224 : 0 : }
225 : 0 : if ( to_azimuth != 0 )
226 : : {
227 : 0 : double tan_alpha_2 = cos_U1 * sin_lambda / ( -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda );
228 : 0 : *to_azimuth = atan( tan_alpha_2 );
229 : 0 : }
230 : :
231 : 0 : return b * A * ( sigma - delta_sigma );
232 : 0 : }
233 : :
234 : : /**
235 : : * \brief Horizontal move of point position
236 : : */
237 : 0 : 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 : 0 : nmeaPOS p1 = *start_pos;
245 : 0 : int RetVal = 1;
246 : :
247 : 0 : distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */
248 : 0 : azimuth = nmea_degree2radian( azimuth );
249 : :
250 : 0 : end_pos->lat = asin(
251 : 0 : sin( p1.lat ) * cos( distance ) + cos( p1.lat ) * sin( distance ) * cos( azimuth ) );
252 : 0 : end_pos->lon = p1.lon + atan2(
253 : 0 : sin( azimuth ) * sin( distance ) * cos( p1.lat ), cos( distance ) - sin( p1.lat ) * sin( end_pos->lat ) );
254 : :
255 : 0 : if ( NMEA_POSIX( isnan )( end_pos->lat ) || NMEA_POSIX( isnan )( end_pos->lon ) )
256 : : {
257 : 0 : end_pos->lat = 0;
258 : 0 : end_pos->lon = 0;
259 : 0 : RetVal = 0;
260 : 0 : }
261 : :
262 : 0 : 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 : : */
271 : 0 : 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 : 0 : NMEA_ASSERT( start_pos != 0 );
289 : 0 : NMEA_ASSERT( end_pos != 0 );
290 : :
291 : 0 : if ( fabs( distance ) < 1e-12 )
292 : : {
293 : : /* No move */
294 : 0 : *end_pos = *start_pos;
295 : 0 : if ( end_azimuth != 0 ) *end_azimuth = azimuth;
296 : 0 : return !( NMEA_POSIX( isnan )( end_pos->lat ) || NMEA_POSIX( isnan )( end_pos->lon ) );
297 : : } /* No move */
298 : :
299 : : /* Earth geometry */
300 : 0 : f = NMEA_EARTH_FLATTENING;
301 : 0 : a = NMEA_EARTH_SEMIMAJORAXIS_M;
302 : 0 : b = ( 1 - f ) * a;
303 : 0 : sqr_a = a * a;
304 : 0 : sqr_b = b * b;
305 : :
306 : : /* Calculation */
307 : 0 : phi1 = start_pos->lat;
308 : 0 : tan_U1 = ( 1 - f ) * tan( phi1 );
309 : 0 : cos_U1 = 1 / sqrt( 1 + tan_U1 * tan_U1 );
310 : 0 : sin_U1 = tan_U1 * cos_U1;
311 : 0 : s = distance;
312 : 0 : alpha1 = azimuth;
313 : 0 : sin_alpha1 = sin( alpha1 );
314 : 0 : cos_alpha1 = cos( alpha1 );
315 : 0 : sigma1 = atan2( tan_U1, cos_alpha1 );
316 : 0 : sin_alpha = cos_U1 * sin_alpha1;
317 : 0 : sqr_cos_alpha = 1 - sin_alpha * sin_alpha;
318 : 0 : sqr_u = sqr_cos_alpha * ( sqr_a - sqr_b ) / sqr_b;
319 : 0 : A = 1 + sqr_u / 16384 * ( 4096 + sqr_u * ( -768 + sqr_u * ( 320 - 175 * sqr_u ) ) );
320 : 0 : B = sqr_u / 1024 * ( 256 + sqr_u * ( -128 + sqr_u * ( 74 - 47 * sqr_u ) ) );
321 : :
322 : : /* Initialize iteration */
323 : 0 : sigma_initial = s / ( b * A );
324 : 0 : sigma = sigma_initial;
325 : 0 : sin_sigma = sin( sigma );
326 : 0 : cos_sigma = cos( sigma );
327 : 0 : cos_2_sigmam = cos( 2 * sigma1 + sigma );
328 : 0 : sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
329 : 0 : delta_sigma = 0;
330 : 0 : sigma_prev = 2 * NMEA_PI;
331 : 0 : remaining_steps = 20;
332 : :
333 : 0 : while ( ( fabs( sigma - sigma_prev ) > 1e-12 ) && ( remaining_steps > 0 ) )
334 : : {
335 : : /* Iterate */
336 : 0 : cos_2_sigmam = cos( 2 * sigma1 + sigma );
337 : 0 : sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
338 : 0 : sin_sigma = sin( sigma );
339 : 0 : cos_sigma = cos( sigma );
340 : 0 : delta_sigma = B * sin_sigma * (
341 : 0 : cos_2_sigmam + B / 4 * (
342 : 0 : cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) -
343 : 0 : B / 6 * cos_2_sigmam * ( -3 + 4 * sin_sigma * sin_sigma ) * ( -3 + 4 * sqr_cos_2_sigmam )
344 : : ) );
345 : 0 : sigma_prev = sigma;
346 : 0 : sigma = sigma_initial + delta_sigma;
347 : 0 : remaining_steps --;
348 : : } /* Iterate */
349 : :
350 : : /* Calculate result */
351 : 0 : tmp1 = ( sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1 );
352 : 0 : phi2 = atan2(
353 : 0 : sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1,
354 : 0 : ( 1 - f ) * sqrt( sin_alpha * sin_alpha + tmp1 * tmp1 )
355 : : );
356 : 0 : lambda = atan2(
357 : 0 : sin_sigma * sin_alpha1,
358 : 0 : cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
359 : : );
360 : 0 : C = f / 16 * sqr_cos_alpha * ( 4 + f * ( 4 - 3 * sqr_cos_alpha ) );
361 : 0 : L = lambda -
362 : 0 : ( 1 - C ) * f * sin_alpha * (
363 : 0 : sigma + C * sin_sigma *
364 : 0 : ( cos_2_sigmam + C * cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) )
365 : : );
366 : :
367 : : /* Result */
368 : 0 : end_pos->lon = start_pos->lon + L;
369 : 0 : end_pos->lat = phi2;
370 : 0 : if ( end_azimuth != 0 )
371 : : {
372 : 0 : *end_azimuth = atan2(
373 : 0 : sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
374 : : );
375 : 0 : }
376 : 0 : return !( NMEA_POSIX( isnan )( end_pos->lat ) || NMEA_POSIX( isnan )( end_pos->lon ) );
377 : 0 : }
378 : :
379 : : /**
380 : : * \brief Convert position from INFO to radians position
381 : : */
382 : 0 : void nmea_info2pos( const nmeaINFO *info, nmeaPOS *pos )
383 : : {
384 : 0 : pos->lat = nmea_ndeg2radian( info->lat );
385 : 0 : pos->lon = nmea_ndeg2radian( info->lon );
386 : 0 : }
387 : :
388 : : /**
389 : : * \brief Convert radians position to INFOs position
390 : : */
391 : 0 : void nmea_pos2info( const nmeaPOS *pos, nmeaINFO *info )
392 : : {
393 : 0 : info->lat = nmea_radian2ndeg( pos->lat );
394 : 0 : info->lon = nmea_radian2ndeg( pos->lon );
395 : 0 : }
|
