Geo Stars Library  0.9.3
Geodetic and Astrometry library
geoEfg2Llh_fast.c
Go to the documentation of this file.
1 /*! \file geoEfg2Llh_fast.c
2  \brief This file contains the geo library routines for
3  conversion between Geocentric and Geodetic coordinates. These routines are
4  optimised for quick execution at the expense of precision.
5 
6  Methods avalailable:
7  \li geoStarLib method
8  \li Hirvonen & Moritz method
9  \li Torge method
10  \li Astronomical Almanac 2002 method
11  \li Borkowski method
12  \li Bowring method
13  \li Vermeille method
14 
15 */
16 #include <stdio.h>
17 #include <math.h>
18 #include <string.h>
19 #include <stdlib.h>
20 #include <ctype.h>
21 #include "geoStars.h"
22 
23 //-----------------------------------------------------------------
24 /*! \brief This routine will convert earth centered Cartesian
25 coordinates (E,F,G), into geodetic coordinates (latitude
26 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
27 
28  \param int datum
29  \param double efg[] : EFG(xyz) in METERS
30  \param double *lat : Latitude in RADIANS
31  \param double *lon : Longitude in RADIANS
32  \param double *hgt : Height in METERS
33  \return nothing
34 
35 \note This routine will be exact only for WGS84 coordinates. All other
36 datums will be slightly off.
37 
38 */
39 
40 void DLL_CALLCONV geoEfg2Llh_fast (int datum, double efg[],
41 double *lat, double *lon, double *hgt)
42 {
43  double p, u, su,cu,bee2,ae2,aob,slat;
44 
45  bee2 = GEO_WGS84_ee2 * GEO_WGS84_b;
46  ae2 = GEO_WGS84_e2 * GEO_WGS84_a;
47  aob = GEO_WGS84_a / GEO_WGS84_b;
48 
49  // Computes EFG to lat, lon & height
50  p = sqrt(sqr(efg[GEO_E]) + sqr(efg[GEO_F]));
51  u = atan((efg[GEO_G]/p) * aob);
52 
53  su = sin(u);
54  cu = cos(u);
55 
56  *lat = atan((efg[GEO_G] + bee2 * cube(su) ) /
57  ( p - ae2 * cube(cu) ) );
58  slat = sin(*lat);
59 
60  *hgt = p / cos(*lat) - ( GEO_WGS84_a / (sqrt(1.0 - GEO_WGS84_e2 * sqr(slat)))); //same results
61  *lon = atan2(efg[GEO_F],efg[GEO_E]); // atan(f/e)
62 
63 }
64 
65 double DLL_CALLCONV geoEfg2Lat_fast (int datum, double e, double f, double g)
66 {
67  double efg[3];
68  double lat, lon, hgt;
69  efg[GEO_E] = e;
70  efg[GEO_F] = f;
71  efg[GEO_G] = g;
72 
73  geoEfg2Llh_fast (datum,efg,&lat,&lon,&hgt);
74  return(RAD_TO_DEG * lat);
75 }
76 
77 double DLL_CALLCONV geoEfg2Lon_fast (int datum, double e, double f, double g)
78 {
79  double efg[3];
80  double lat, lon, hgt;
81  efg[GEO_E] = e;
82  efg[GEO_F] = f;
83  efg[GEO_G] = g;
84 
85  geoEfg2Llh_fast (datum,efg,&lat,&lon,&hgt);
86  return(RAD_TO_DEG * lon);
87 }
88 double DLL_CALLCONV geoEfg2Hgt_fast (int datum, double e, double f, double g)
89 {
90  double efg[3];
91  double lat, lon, hgt;
92  efg[GEO_E] = e;
93  efg[GEO_F] = f;
94  efg[GEO_G] = g;
95 
96  geoEfg2Llh_fast (datum,efg,&lat,&lon,&hgt);
97  return(hgt);
98 }
99 
100 
101 /*! \brief Hirvonen & Moritz Method - This routine will convert earth centered Cartesian
102 coordinates (E,F,G), into geodetic coordinates (latitude
103 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
104 
105  \param int datum
106  \param double efg[] : EFG(xyz) in METERS
107  \param double *lat : Latitude in RADIANS
108  \param double *lon : Longitude in RADIANS
109  \param double *hgt : Height in METERS
110  \return nothing
111 
112 \note This method is a successive approximation
113 
114 */
115 
116 void DLL_CALLCONV geoEfg2Llh_hm_fast (int datum, double efg[],
117 double *lat, double *lon, double *hgt)
118 {
119  // double a, flat, b, e2, ee2;
120  double tmp,n,p,slat;
121 
122  p = sqrt(sqr(efg[GEO_E])+ sqr(efg[GEO_F]));
123  *lon = atan2(efg[GEO_F],efg[GEO_E]); // atan(f/e)
124 
125  // Prepare the initial value of lat
126  tmp = efg[GEO_G] / p;
127  *lat = atan((1.0 / (1.0 - GEO_WGS84_e2))*tmp);
128 
129  slat = sin(*lat);
130  n= GEO_WGS84_a/sqrt(1.0 - GEO_WGS84_e2 * sqr(slat));
131  // *lat=atan(tmp*(1.0 + (e2*n*slat)/efg[GEO_G]));
132  // lat1=*lat;
133  *hgt = (p / cos(*lat)) - n;
134 
135 }
136 
137 double DLL_CALLCONV geoEfg2Lat_hm_fast (int datum, double e, double f, double g)
138 {
139  double efg[3];
140  double lat, lon, hgt;
141  efg[GEO_E] = e;
142  efg[GEO_F] = f;
143  efg[GEO_G] = g;
144 
145  geoEfg2Llh_hm_fast (datum,efg,&lat,&lon,&hgt);
146  return(RAD_TO_DEG * lat);
147 }
148 
149 double DLL_CALLCONV geoEfg2Lon_hm_fast (int datum, double e, double f, double g)
150 {
151  double efg[3];
152  double lat, lon, hgt;
153  efg[GEO_E] = e;
154  efg[GEO_F] = f;
155  efg[GEO_G] = g;
156 
157  geoEfg2Llh_hm_fast (datum,efg,&lat,&lon,&hgt);
158  return(RAD_TO_DEG * lon);
159 }
160 double DLL_CALLCONV geoEfg2Hgt_hm_fast (int datum, double e, double f, double g)
161 {
162  double efg[3];
163  double lat, lon, hgt;
164  efg[GEO_E] = e;
165  efg[GEO_F] = f;
166  efg[GEO_G] = g;
167 
168  geoEfg2Llh_hm_fast (datum,efg,&lat,&lon,&hgt);
169  return(hgt);
170 }
171 
172 
173 /*! \brief Torge Method - This routine will convert earth centered Cartesian
174 coordinates (E,F,G), into geodetic coordinates (latitude
175 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
176 
177  \param int datum
178  \param double efg[] : EFG(xyz) in METERS
179  \param double *lat : Latitude in RADIANS
180  \param double *lon : Longitude in RADIANS
181  \param double *hgt : Height in METERS
182  \return nothing
183 
184 \note This method is a successive approximation
185 
186 */
187 
188 void DLL_CALLCONV geoEfg2Llh_torge_fast (int datum, double efg[],
189 double *lat, double *lon, double *hgt)
190 {
191  double tmp,n,p,slat;
192 
193  p = sqrt(sqr(efg[GEO_E])+ sqr(efg[GEO_F]));
194  *lon = atan2(efg[GEO_F],efg[GEO_E]); // atan(f/e)
195 
196  // Prepare the initial value of lat
197  tmp = efg[GEO_G] / p;
198  *lat = atan(tmp/(1.0 - GEO_WGS84_e2)); //assume hgt = 0 to start
199 
200 
201  slat = sin(*lat);
202  //clat = cos(*lat);
203 
204  // radius of curvature estimate
205  n= GEO_WGS84_a/sqrt(1.0 - GEO_WGS84_e2 * sqr(slat));
206 
207  // Next estimates of lat/hgt
208  *hgt = (p / cos(*lat)) - n;
209  *lat=atan(tmp / (1.0 - (GEO_WGS84_e2 * ( n / (n + *hgt)) ) ) );
210 
211 }
212 
213 double DLL_CALLCONV geoEfg2Lat_torge_fast (int datum, double e, double f, double g)
214 {
215  double efg[3];
216  double lat, lon, hgt;
217  efg[GEO_E] = e;
218  efg[GEO_F] = f;
219  efg[GEO_G] = g;
220 
221  geoEfg2Llh_torge_fast (datum,efg,&lat,&lon,&hgt);
222  return(RAD_TO_DEG * lat);
223 }
224 
225 double DLL_CALLCONV geoEfg2Lon_torge_fast (int datum, double e, double f, double g)
226 {
227  double efg[3];
228  double lat, lon, hgt;
229  efg[GEO_E] = e;
230  efg[GEO_F] = f;
231  efg[GEO_G] = g;
232 
233  geoEfg2Llh_torge_fast (datum,efg,&lat,&lon,&hgt);
234  return(RAD_TO_DEG * lon);
235 }
236 double DLL_CALLCONV geoEfg2Hgt_torge_fast (int datum, double e, double f, double g)
237 {
238  double efg[3];
239  double lat, lon, hgt;
240  efg[GEO_E] = e;
241  efg[GEO_F] = f;
242  efg[GEO_G] = g;
243 
244  geoEfg2Llh_torge_fast (datum,efg,&lat,&lon,&hgt);
245  return(hgt);
246 }
247 
248 
249 //-----------------------------------------------------------------
250 /*! \brief Bowring Method - This routine will convert earth centered Cartesian
251 coordinates (E,F,G), into geodetic coordinates (latitude
252 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
253 
254  \param int datum
255  \param double efg[] : EFG(xyz) in METERS
256  \param double *lat : Latitude in RADIANS
257  \param double *lon : Longitude in RADIANS
258  \param double *hgt : Height in METERS
259  \return nothing
260 
261 \note This method is a successive approximation
262 
263 */
264 void DLL_CALLCONV geoEfg2Llh_bowring_fast (int datum, double efg[],
265 double *lat, double *lon, double *hgt)
266 {
267  double tmp,n,b0,p,slat,clat,ee2b,ae2;
268  //double boa;
269 
270  *lon = atan2(efg[GEO_F],efg[GEO_E]); // atan(f/e)
271  p = sqrt(sqr(efg[GEO_E])+ sqr(efg[GEO_F]));
272  tmp = efg[GEO_G] / p;
273 
274  // temps pulled out of the loop
275  ae2 = GEO_WGS84_a * GEO_WGS84_e2;
276  ee2b = GEO_WGS84_ee2 * GEO_WGS84_b;
277  //boa = GEO_WGS84_b/GEO_WGS84_a;
278 
279  // Prepare the initial value of lat
280  b0 = atan((GEO_WGS84_a/GEO_WGS84_b)*tmp);
281 
282  // iterate until delta lat is tiny
283  slat = sin(b0);
284  clat = cos(b0);
285  *lat = atan((efg[GEO_G]+ ee2b * cube(slat) ) /
286  (p - ae2 * cube(clat) ) );
287  // b0 = atan((boa) * tan(*lat));
288  slat = sin(*lat);
289  n= GEO_WGS84_a / sqrt(1.0-GEO_WGS84_e2*sqr(slat));
290 
291  *hgt = (p / cos(*lat)) - n;
292 
293 }
294 
295 double DLL_CALLCONV geoEfg2Lat_bowring_fast (int datum, double e, double f, double g)
296 {
297  double efg[3];
298  double lat, lon, hgt;
299  efg[GEO_E] = e;
300  efg[GEO_F] = f;
301  efg[GEO_G] = g;
302 
303  geoEfg2Llh_bowring_fast (datum,efg,&lat,&lon,&hgt);
304  return(RAD_TO_DEG * lat);
305 }
306 
307 double DLL_CALLCONV geoEfg2Lon_bowring_fast (int datum, double e, double f, double g)
308 {
309  double efg[3];
310  double lat, lon, hgt;
311  efg[GEO_E] = e;
312  efg[GEO_F] = f;
313  efg[GEO_G] = g;
314 
315  geoEfg2Llh_bowring_fast (datum,efg,&lat,&lon,&hgt);
316  return(RAD_TO_DEG * lon);
317 }
318 double DLL_CALLCONV geoEfg2Hgt_bowring_fast (int datum, double e, double f, double g)
319 {
320  double efg[3];
321  double lat, lon, hgt;
322  efg[GEO_E] = e;
323  efg[GEO_F] = f;
324  efg[GEO_G] = g;
325 
326  geoEfg2Llh_bowring_fast (datum,efg,&lat,&lon,&hgt);
327  return(hgt);
328 }
329 
330 
331 
332 //-----------------------------------------------------------------
333 /*! \brief Astronomical Almanac 2002 Method - This routine will convert earth centered Cartesian
334 coordinates (E,F,G), into geodetic coordinates (latitude
335 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
336 
337  \param int datum
338  \param double efg[] : EFG(xyz) in METERS
339  \param double *lat : Latitude in RADIANS
340  \param double *lon : Longitude in RADIANS
341  \param double *hgt : Height in METERS
342  \return nothing
343 
344 \note This method is a successive approximation from Astronomical Almanac 2002 K11-12
345 
346 */
347 void DLL_CALLCONV geoEfg2Llh_aa_fast (int datum, double efg[],
348 double *lat, double *lon, double *hgt)
349 {
350  double phi,p,n;
351  double slat;
352 
353  // Computes EFG to lat, lon & height
354  // from the Astronomical Almanac 2002 - K11-12
355  *lon = atan2(efg[GEO_F],efg[GEO_E]); // atan(f/e)
356  p = sqrt(sqr(efg[GEO_E]) + sqr(efg[GEO_F]));
357 
358  // phi = atan(efg[GEO_G]/p); // first approximation
359  phi = atan(efg[GEO_G]/((1.0-GEO_WGS84_e2)*p)); // faster
360 
361  slat = sin(phi);
362  n = GEO_WGS84_a/sqrt(1.0 - GEO_WGS84_e2 * sqr(slat));
363  *lat = atan((efg[GEO_G] + n * GEO_WGS84_e2 * slat)/p);
364 
365  *hgt = p / cos(*lat) - n;
366 
367 }
368 
369 double DLL_CALLCONV geoEfg2Lat_aa_fast (int datum, double e, double f, double g)
370 {
371  double efg[3];
372  double lat, lon, hgt;
373  efg[GEO_E] = e;
374  efg[GEO_F] = f;
375  efg[GEO_G] = g;
376 
377  geoEfg2Llh_aa_fast (datum,efg,&lat,&lon,&hgt);
378  return(RAD_TO_DEG * lat);
379 }
380 
381 double DLL_CALLCONV geoEfg2Lon_aa_fast (int datum, double e, double f, double g)
382 {
383  double efg[3];
384  double lat, lon, hgt;
385  efg[GEO_E] = e;
386  efg[GEO_F] = f;
387  efg[GEO_G] = g;
388 
389  geoEfg2Llh_aa_fast (datum,efg,&lat,&lon,&hgt);
390  return(RAD_TO_DEG * lon);
391 }
392 double DLL_CALLCONV geoEfg2Hgt_aa_fast (int datum, double e, double f, double g)
393 {
394  double efg[3];
395  double lat, lon, hgt;
396  efg[GEO_E] = e;
397  efg[GEO_F] = f;
398  efg[GEO_G] = g;
399 
400  geoEfg2Llh_aa_fast (datum,efg,&lat,&lon,&hgt);
401  return(hgt);
402 }
403 
404 
405 //-----------------------------------------------------------------
406 /*! \brief Borkowski Method - This routine will convert earth centered Cartesian
407 coordinates (E,F,G), into geodetic coordinates (latitude
408 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
409 
410  \param int datum
411  \param double efg[] : EFG(xyz) in METERS
412  \param double *lat : Latitude in RADIANS
413  \param double *lon : Longitude in RADIANS
414  \param double *hgt : Height in METERS
415  \return nothing
416 
417 \note This method is closed exact method from Bull. Geod., 63 (1989), pp. 50-56
418 
419 */
420 void DLL_CALLCONV geoEfg2Llh_borkowski_fast (int datum, double efg[],
421 double *lat, double *lon, double *hgt)
422 {
423  double a,b,r,e,f,p,q,d,v,g,t,sign,bg,samb,sqrtd,sqre,ar;
424  //double e2;
425 
426  /* Get the ellipsoid parameters */
427  // geoGetEllipsoid (&a, &b, &e2, &ee2, &flat, datum);
428  a=GEO_WGS84_a;
429  // b=GEO_WGS84_b;
430  // e2=GEO_WGS84_e2;
431 
432  // Computes EFG to lat, lon & height
433  // from Bull. Geod., 63 (1989), pp. 50-56
434  // Accurate Algorithms to Transform Geocentric to Geodetic Coordinates
435  // By K.M. Borkowski
436 
437  *lon = atan2(efg[GEO_F],efg[GEO_E]); // atan(f/e)
438 
439  //use the sign for Z
440  if(efg[GEO_G] < 0.0) sign = -1.0;
441  else sign = 1.0;
442  b = GEO_WGS84_b * sign;
443  bg = b * efg[GEO_G]; //temp
444  samb = sqr(a)-sqr(b) ;//temp
445  r = efg[GEO_E] / cos(*lon);
446  ar = a*r;
447  e = (bg - samb) / ar;
448  sqre = sqr(e);
449  f = (bg + samb) / ar;
450  p = 4.0/3.0*(e*f+1.0);
451  q = 2.0 * (sqre-sqr(f));
452  d = cube(p) + sqr(q);
453  sqrtd = sqrt(d);
454  if(d < 0.0)
455  v = 2.0 * sqrt(-p) * cos(acos(q/pow(-p,3.0/2.0))/3.0);
456  else
457  v = pow((sqrtd - q),1.0/3.0) - pow((sqrtd + q),1.0/3.0);
458  g = (sqrt(sqre+v) + e ) / 2.0;
459  t = sqrt(sqr(g)+((f-v*g)/(2.0*g-e))) - g;
460  *lat = atan(a*(1.0-sqr(t))/(2.0*b*t));
461 
462  *hgt = (r - a*t) * cos(*lat) + (efg[GEO_G]-b)*sin(*lat);
463 
464 }
465 
466 double DLL_CALLCONV geoEfg2Lat_borkowski_fast (int datum, double e, double f, double g)
467 {
468  double efg[3];
469  double lat, lon, hgt;
470  efg[GEO_E] = e;
471  efg[GEO_F] = f;
472  efg[GEO_G] = g;
473 
474  geoEfg2Llh_borkowski_fast (datum,efg,&lat,&lon,&hgt);
475  return(RAD_TO_DEG * lat);
476 }
477 
478 double DLL_CALLCONV geoEfg2Lon_borkowski_fast (int datum, double e, double f, double g)
479 {
480  double efg[3];
481  double lat, lon, hgt;
482  efg[GEO_E] = e;
483  efg[GEO_F] = f;
484  efg[GEO_G] = g;
485 
486  geoEfg2Llh_borkowski_fast (datum,efg,&lat,&lon,&hgt);
487  return(RAD_TO_DEG * lon);
488 }
489 double DLL_CALLCONV geoEfg2Hgt_borkowski_fast (int datum, double e, double f, double g)
490 {
491  double efg[3];
492  double lat, lon, hgt;
493  efg[GEO_E] = e;
494  efg[GEO_F] = f;
495  efg[GEO_G] = g;
496 
497  geoEfg2Llh_borkowski_fast (datum,efg,&lat,&lon,&hgt);
498  return(hgt);
499 }
500 
501 
502 //-----------------------------------------------------------------
503 /*! \brief Vermeille Method - This routine will convert earth centered Cartesian
504 coordinates (E,F,G), into geodetic coordinates (latitude
505 \f$\phi\f$, longitude \f$\lambda\f$, and ellipsoid height \f$h\f$).
506 
507  \param int datum
508  \param double efg[] : EFG(xyz) in METERS
509  \param double *lat : Latitude in RADIANS
510  \param double *lon : Longitude in RADIANS
511  \param double *hgt : Height in METERS
512  \return nothing
513 
514 \note This method is closed exact method from ....
515 
516 */
517 void DLL_CALLCONV geoEfg2Llh_vermeille_fast (int datum, double efg[],
518 double *lat, double *lon, double *hgt)
519 {
520  double fr,a2,x2,y2,z2,d2,e4,p,q,r,s,t,u,v,w,k,d,sqrtx2py2,sqrtd2pz2;
521 
522 
523 
524  // precompute variables
525  x2 = sqr(efg[GEO_E]);
526  y2 = sqr(efg[GEO_F]);
527  z2 = sqr(efg[GEO_G]);
528  a2 = sqr(GEO_WGS84_a);
529  e4 = sqr(GEO_WGS84_e2);
530  sqrtx2py2 = sqrt(x2+y2);
531  fr = (1.0-GEO_WGS84_e2)/(sqr(GEO_WGS84_a));
532 
533  // main algorithm
534  p = (x2 + y2)/ a2;
535  q = fr*z2;
536  r = (p+q-e4)/6.0;
537  s = e4 * (p*q)/(4.0*cube(r));
538  t = pow(1.0+s+sqrt(s*(2.0+s)),1.0/3.0);
539  u = r*(1.0+t+(1.0/t));
540  v = sqrt(sqr(u)+e4*q);
541  w = GEO_WGS84_e2*(u+v-q)/(2.0*v);
542  k = sqrt(u+v+sqr(w)) - w;
543  d = k*sqrtx2py2/(k+GEO_WGS84_e2);
544  d2 = sqr(d);
545  sqrtd2pz2 = sqrt(d2+z2);
546 
547  *lon = 2.0 * atan(efg[GEO_F]/(efg[GEO_E] + sqrtx2py2));
548  *lat = 2.0 * atan(efg[GEO_G]/(d + sqrtd2pz2));
549  *hgt = ((k + GEO_WGS84_e2 - 1.0) / k) * sqrtd2pz2;
550 
551 }
552 
553 double DLL_CALLCONV geoEfg2Lat_vermeille_fast (int datum, double e, double f, double g)
554 {
555  double efg[3];
556  double lat, lon, hgt;
557  efg[GEO_E] = e;
558  efg[GEO_F] = f;
559  efg[GEO_G] = g;
560 
561  geoEfg2Llh_vermeille_fast (datum,efg,&lat,&lon,&hgt);
562  return(RAD_TO_DEG * lat);
563 }
564 
565 double DLL_CALLCONV geoEfg2Lon_vermeille_fast (int datum, double e, double f, double g)
566 {
567  double efg[3];
568  double lat, lon, hgt;
569  efg[GEO_E] = e;
570  efg[GEO_F] = f;
571  efg[GEO_G] = g;
572 
573  geoEfg2Llh_vermeille_fast (datum,efg,&lat,&lon,&hgt);
574  return(RAD_TO_DEG * lon);
575 }
576 double DLL_CALLCONV geoEfg2Hgt_vermeille_fast (int datum, double e, double f, double g)
577 {
578  double efg[3];
579  double lat, lon, hgt;
580  efg[GEO_E] = e;
581  efg[GEO_F] = f;
582  efg[GEO_G] = g;
583 
584  geoEfg2Llh_vermeille_fast (datum,efg,&lat,&lon,&hgt);
585  return(hgt);
586 }
  • Generated by doxygen 1.8.4