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 General Public License
14 // along with this program; if not, write to the Free Software
15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 # include <simgear_config.h>
26 // These are hard numbers from the WGS84 standard. DON'T MODIFY
27 // unless you want to change the datum.
28 #define _EQURAD 6378137.0
29 #define _FLATTENING 298.257223563
31 // These are derived quantities more useful to the code:
33 #define _SQUASH (1 - 1/_FLATTENING)
34 #define _STRETCH (1/_SQUASH)
35 #define _POLRAD (EQURAD * _SQUASH)
37 // High-precision versions of the above produced with an arbitrary
38 // precision calculator (the compiler might lose a few bits in the FPU
39 // operations). These are specified to 81 bits of mantissa, which is
40 // higher than any FPU known to me:
41 #define _SQUASH 0.9966471893352525192801545
42 #define _STRETCH 1.0033640898209764189003079
43 #define _POLRAD 6356752.3142451794975639668
46 // The constants from the WGS84 standard
47 const double SGGeodesy::EQURAD = _EQURAD;
48 const double SGGeodesy::iFLATTENING = _FLATTENING;
49 const double SGGeodesy::SQUASH = _SQUASH;
50 const double SGGeodesy::STRETCH = _STRETCH;
51 const double SGGeodesy::POLRAD = _POLRAD;
53 // additional derived and precomputable ones
54 // for the geodetic conversion algorithm
56 #define E2 fabs(1 - _SQUASH*_SQUASH)
57 static double a = _EQURAD;
58 static double ra2 = 1/(_EQURAD*_EQURAD);
59 static double e = sqrt(E2);
60 static double e2 = E2;
61 static double e4 = E2*E2;
71 SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
75 // Direct transformation from geocentric to geodetic ccordinates,
76 // Journal of Geodesy (2002) 76:451-454
80 double XXpYY = X*X+Y*Y;
81 double sqrtXXpYY = sqrt(XXpYY);
83 double q = Z*Z*(1-e2)*ra2;
84 double r = 1/6.0*(p+q-e4);
85 double s = e4*p*q/(4*r*r*r);
86 double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
87 double u = r*(1+t+1/t);
88 double v = sqrt(u*u+e4*q);
89 double w = e2*(u+v-q)/(2*v);
90 double k = sqrt(u+v+w*w)-w;
91 double D = k*sqrtXXpYY/(k+e2);
92 geod.setLongitudeRad(2*atan2(Y, X+sqrtXXpYY));
93 double sqrtDDpZZ = sqrt(D*D+Z*Z);
94 geod.setLatitudeRad(2*atan2(Z, D+sqrtDDpZZ));
95 geod.setElevationM((k+e2-1)*sqrtDDpZZ/k);
99 SGGeodesy::SGGeodToCart(const SGGeod& geod, SGVec3<double>& cart)
103 // Direct transformation from geocentric to geodetic ccordinates,
104 // Journal of Geodesy (2002) 76:451-454
105 double lambda = geod.getLongitudeRad();
106 double phi = geod.getLatitudeRad();
107 double h = geod.getElevationM();
108 double sphi = sin(phi);
109 double n = a/sqrt(1-e2*sphi*sphi);
110 double cphi = cos(phi);
111 double slambda = sin(lambda);
112 double clambda = cos(lambda);
113 cart(0) = (h+n)*cphi*clambda;
114 cart(1) = (h+n)*cphi*slambda;
115 cart(2) = (h+n-e2*n)*sphi;
119 SGGeodesy::SGGeodToSeaLevelRadius(const SGGeod& geod)
121 // this is just a simplified version of the SGGeodToCart function above,
122 // substitute h = 0, take the 2-norm of the cartesian vector and simplify
123 double phi = geod.getLatitudeRad();
124 double sphi = sin(phi);
125 double sphi2 = sphi*sphi;
126 return a*sqrt((1 + (e4 - 2*e2)*sphi2)/(1 - e2*sphi2));
130 SGGeodesy::SGCartToGeoc(const SGVec3<double>& cart, SGGeoc& geoc)
132 double minVal = SGLimits<double>::min();
133 if (fabs(cart(0)) < minVal && fabs(cart(1)) < minVal)
134 geoc.setLongitudeRad(0);
136 geoc.setLongitudeRad(atan2(cart(1), cart(0)));
138 double nxy = sqrt(cart(0)*cart(0) + cart(1)*cart(1));
139 if (fabs(nxy) < minVal && fabs(cart(2)) < minVal)
140 geoc.setLatitudeRad(0);
142 geoc.setLatitudeRad(atan2(cart(2), nxy));
144 geoc.setRadiusM(norm(cart));
148 SGGeodesy::SGGeocToCart(const SGGeoc& geoc, SGVec3<double>& cart)
150 double lat = geoc.getLatitudeRad();
151 double lon = geoc.getLongitudeRad();
152 double slat = sin(lat);
153 double clat = cos(lat);
154 double slon = sin(lon);
155 double clon = cos(lon);
156 cart = geoc.getRadiusM()*SGVec3<double>(clat*clon, clat*slon, slat);