1 // Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de
3 // This library is free software; you can redistribute it and/or
4 // modify it under the terms of the GNU Library General Public
5 // License as published by the Free Software Foundation; either
6 // version 2 of the License, or (at your option) any later version.
8 // This library is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 // Library General Public License for more details.
13 // You should have received a copy of the GNU Library General Public
14 // License along with this library; if not, write to the
15 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
16 // Boston, MA 02111-1307, USA.
20 # include <simgear_config.h>
27 // These are hard numbers from the WGS84 standard. DON'T MODIFY
28 // unless you want to change the datum.
29 #define _EQURAD 6378137.0
30 #define _FLATTENING 298.257223563
32 // These are derived quantities more useful to the code:
34 #define _SQUASH (1 - 1/_FLATTENING)
35 #define _STRETCH (1/_SQUASH)
36 #define _POLRAD (EQURAD * _SQUASH)
38 // High-precision versions of the above produced with an arbitrary
39 // precision calculator (the compiler might lose a few bits in the FPU
40 // operations). These are specified to 81 bits of mantissa, which is
41 // higher than any FPU known to me:
42 #define _SQUASH 0.9966471893352525192801545
43 #define _STRETCH 1.0033640898209764189003079
44 #define _POLRAD 6356752.3142451794975639668
47 // The constants from the WGS84 standard
48 const double SGGeodesy::EQURAD = _EQURAD;
49 const double SGGeodesy::iFLATTENING = _FLATTENING;
50 const double SGGeodesy::SQUASH = _SQUASH;
51 const double SGGeodesy::STRETCH = _STRETCH;
52 const double SGGeodesy::POLRAD = _POLRAD;
54 // additional derived and precomputable ones
55 // for the geodetic conversion algorithm
57 #define E2 fabs(1 - _SQUASH*_SQUASH)
58 static double a = _EQURAD;
59 static double ra2 = 1/(_EQURAD*_EQURAD);
60 static double e = sqrt(E2);
61 static double e2 = E2;
62 static double e4 = E2*E2;
72 SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
76 // Direct transformation from geocentric to geodetic ccordinates,
77 // Journal of Geodesy (2002) 76:451-454
81 double XXpYY = X*X+Y*Y;
82 double sqrtXXpYY = sqrt(XXpYY);
84 double q = Z*Z*(1-e2)*ra2;
85 double r = 1/6.0*(p+q-e4);
86 double s = e4*p*q/(4*r*r*r);
87 double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
88 double u = r*(1+t+1/t);
89 double v = sqrt(u*u+e4*q);
90 double w = e2*(u+v-q)/(2*v);
91 double k = sqrt(u+v+w*w)-w;
92 double D = k*sqrtXXpYY/(k+e2);
93 geod.setLongitudeRad(2*atan2(Y, X+sqrtXXpYY));
94 double sqrtDDpZZ = sqrt(D*D+Z*Z);
95 geod.setLatitudeRad(2*atan2(Z, D+sqrtDDpZZ));
96 geod.setElevationM((k+e2-1)*sqrtDDpZZ/k);
100 SGGeodesy::SGGeodToCart(const SGGeod& geod, SGVec3<double>& cart)
104 // Direct transformation from geocentric to geodetic ccordinates,
105 // Journal of Geodesy (2002) 76:451-454
106 double lambda = geod.getLongitudeRad();
107 double phi = geod.getLatitudeRad();
108 double h = geod.getElevationM();
109 double sphi = sin(phi);
110 double n = a/sqrt(1-e2*sphi*sphi);
111 double cphi = cos(phi);
112 double slambda = sin(lambda);
113 double clambda = cos(lambda);
114 cart(0) = (h+n)*cphi*clambda;
115 cart(1) = (h+n)*cphi*slambda;
116 cart(2) = (h+n-e2*n)*sphi;
120 SGGeodesy::SGGeodToSeaLevelRadius(const SGGeod& geod)
122 // this is just a simplified version of the SGGeodToCart function above,
123 // substitute h = 0, take the 2-norm of the cartesian vector and simplify
124 double phi = geod.getLatitudeRad();
125 double sphi = sin(phi);
126 double sphi2 = sphi*sphi;
127 return a*sqrt((1 + (e4 - 2*e2)*sphi2)/(1 - e2*sphi2));
131 SGGeodesy::SGCartToGeoc(const SGVec3<double>& cart, SGGeoc& geoc)
133 double minVal = SGLimits<double>::min();
134 if (fabs(cart(0)) < minVal && fabs(cart(1)) < minVal)
135 geoc.setLongitudeRad(0);
137 geoc.setLongitudeRad(atan2(cart(1), cart(0)));
139 double nxy = sqrt(cart(0)*cart(0) + cart(1)*cart(1));
140 if (fabs(nxy) < minVal && fabs(cart(2)) < minVal)
141 geoc.setLatitudeRad(0);
143 geoc.setLatitudeRad(atan2(cart(2), nxy));
145 geoc.setRadiusM(norm(cart));
149 SGGeodesy::SGGeocToCart(const SGGeoc& geoc, SGVec3<double>& cart)
151 double lat = geoc.getLatitudeRad();
152 double lon = geoc.getLongitudeRad();
153 double slat = sin(lat);
154 double clat = cos(lat);
155 double slon = sin(lon);
156 double clon = cos(lon);
157 cart = geoc.getRadiusM()*SGVec3<double>(clat*clon, clat*slon, slat);