3 # include <simgear_config.h>
10 // These are hard numbers from the WGS84 standard. DON'T MODIFY
11 // unless you want to change the datum.
12 #define _EQURAD 6378137.0
13 #define _FLATTENING 298.257223563
15 // These are derived quantities more useful to the code:
17 #define _SQUASH (1 - 1/_FLATTENING)
18 #define _STRETCH (1/_SQUASH)
19 #define _POLRAD (EQURAD * _SQUASH)
21 // High-precision versions of the above produced with an arbitrary
22 // precision calculator (the compiler might lose a few bits in the FPU
23 // operations). These are specified to 81 bits of mantissa, which is
24 // higher than any FPU known to me:
25 #define _SQUASH 0.9966471893352525192801545
26 #define _STRETCH 1.0033640898209764189003079
27 #define _POLRAD 6356752.3142451794975639668
30 // The constants from the WGS84 standard
31 const double SGGeodesy::EQURAD = _EQURAD;
32 const double SGGeodesy::iFLATTENING = _FLATTENING;
33 const double SGGeodesy::SQUASH = _SQUASH;
34 const double SGGeodesy::STRETCH = _STRETCH;
35 const double SGGeodesy::POLRAD = _POLRAD;
37 // additional derived and precomputable ones
38 // for the geodetic conversion algorithm
40 #define E2 fabs(1 - _SQUASH*_SQUASH)
41 static double a = _EQURAD;
42 static double ra2 = 1/(_EQURAD*_EQURAD);
43 static double e = sqrt(E2);
44 static double e2 = E2;
45 static double e4 = E2*E2;
55 SGGeodesy::SGCartToGeod(const SGVec3<double>& cart, SGGeod& geod)
59 // Direct transformation from geocentric to geodetic ccordinates,
60 // Journal of Geodesy (2002) 76:451-454
64 double XXpYY = X*X+Y*Y;
65 double sqrtXXpYY = sqrt(XXpYY);
67 double q = Z*Z*(1-e2)*ra2;
68 double r = 1/6.0*(p+q-e4);
69 double s = e4*p*q/(4*r*r*r);
70 double t = pow(1+s+sqrt(s*(2+s)), 1/3.0);
71 double u = r*(1+t+1/t);
72 double v = sqrt(u*u+e4*q);
73 double w = e2*(u+v-q)/(2*v);
74 double k = sqrt(u+v+w*w)-w;
75 double D = k*sqrtXXpYY/(k+e2);
76 geod.setLongitudeRad(2*atan2(Y, X+sqrtXXpYY));
77 double sqrtDDpZZ = sqrt(D*D+Z*Z);
78 geod.setLatitudeRad(2*atan2(Z, D+sqrtDDpZZ));
79 geod.setElevationM((k+e2-1)*sqrtDDpZZ/k);
83 SGGeodesy::SGGeodToCart(const SGGeod& geod, SGVec3<double>& cart)
87 // Direct transformation from geocentric to geodetic ccordinates,
88 // Journal of Geodesy (2002) 76:451-454
89 double lambda = geod.getLongitudeRad();
90 double phi = geod.getLatitudeRad();
91 double h = geod.getElevationM();
92 double sphi = sin(phi);
93 double n = a/sqrt(1-e2*sphi*sphi);
94 double cphi = cos(phi);
95 double slambda = sin(lambda);
96 double clambda = cos(lambda);
97 cart(0) = (h+n)*cphi*clambda;
98 cart(1) = (h+n)*cphi*slambda;
99 cart(2) = (h+n-e2*n)*sphi;
103 SGGeodesy::SGGeodToSeaLevelRadius(const SGGeod& geod)
105 // this is just a simplified version of the SGGeodToCart function above,
106 // substitute h = 0, take the 2-norm of the cartesian vector and simplify
107 double phi = geod.getLatitudeRad();
108 double sphi = sin(phi);
109 double sphi2 = sphi*sphi;
110 return a*sqrt((1 + (e4 - 2*e2)*sphi2)/(1 - e2*sphi2));
114 SGGeodesy::SGCartToGeoc(const SGVec3<double>& cart, SGGeoc& geoc)
116 double minVal = SGLimits<double>::min();
117 if (fabs(cart(0)) < minVal && fabs(cart(1)) < minVal)
118 geoc.setLongitudeRad(0);
120 geoc.setLongitudeRad(atan2(cart(1), cart(0)));
122 double nxy = sqrt(cart(0)*cart(0) + cart(1)*cart(1));
123 if (fabs(nxy) < minVal && fabs(cart(2)) < minVal)
124 geoc.setLatitudeRad(0);
126 geoc.setLatitudeRad(atan2(cart(2), nxy));
128 geoc.setRadiusM(norm(cart));
132 SGGeodesy::SGGeocToCart(const SGGeoc& geoc, SGVec3<double>& cart)
134 double lat = geoc.getLatitudeRad();
135 double lon = geoc.getLongitudeRad();
136 double slat = sin(lat);
137 double clat = cos(lat);
138 double slon = sin(lon);
139 double clon = cos(lon);
140 cart = geoc.getRadiusM()*SGVec3<double>(clat*clon, clat*slon, slat);