+
+#ifdef HAVE_CONFIG_H
+# include <simgear_config.h>
+#endif
+
#include <simgear/constants.h>
+#include "SGMath.hxx"
#include "sg_geodesy.hxx"
// Notes:
static const double POLRAD = 6356752.3142451794975639668;
#endif
-// Returns a "local" geodetic latitude: an approximation that will be
-// correct only at zero altitude.
-static double localLat(double r, double z)
-{
- // Squash to a spherical earth, compute a tangent vector to the
- // surface circle (in squashed space, the surface is a perfect
- // sphere) by swapping the components and negating one, stretch to
- // real coordinates, and take an inverse-tangent/perpedicular
- // vector to get a local geodetic "up" vector. (Those steps all
- // cook down to just a few multiplies). Then just turn it into an
- // angle.
- double upr = r * SQUASH;
- double upz = z * STRETCH;
- return atan2(upz, upr);
-}
-
-// This is the inverse of the algorithm in localLat(). It returns the
-// (cylindrical) coordinates of a surface latitude expressed as an
-// "up" unit vector.
-static void surfRZ(double upr, double upz, double* r, double* z)
-{
- // We are
- // converting a (2D, cylindrical) "up" vector defined by the
- // geodetic latitude into unitless R and Z coordinates in
- // cartesian space.
- double R = upr * STRETCH;
- double Z = upz * SQUASH;
-
- // Now we need to turn R and Z into a surface point. That is,
- // pick a coefficient C for them such that the point is on the
- // surface when converted to "squashed" space:
- // (C*R*SQUASH)^2 + (C*Z)^2 = POLRAD^2
- // C^2 = POLRAD^2 / ((R*SQUASH)^2 + Z^2)
- double sr = R * SQUASH;
- double c = POLRAD / sqrt(sr*sr + Z*Z);
- R *= c;
- Z *= c;
-
- *r = R; *z = Z;
-}
-
-// Returns the insersection of the line joining the center of the
-// earth and the specified cylindrical point with the surface of the
-// WGS84 ellipsoid. Works by finding a normalization constant (in
-// squashed space) that places the squashed point on the surface of
-// the sphere.
-static double seaLevelRadius(double r, double z)
-{
- double sr = r * SQUASH;
- double norm = POLRAD/sqrt(sr*sr + z*z);
- r *= norm;
- z *= norm;
- return sqrt(r*r + z*z);
-}
-
-// Convert a cartexian XYZ coordinate to a geodetic lat/lon/alt. This
-// is a "recursion relation". In essence, it iterates over the 2D
-// part of sgGeodToCart refining its approximation at each step. The
-// MAX_LAT_ERROR threshold is picked carefully to allow us to reach
-// the full precision of an IEEE double. While this algorithm might
-// look slow, it's not. It actually converges very fast indeed --
-// I've never seen it take more than six iterations under normal
-// conditions. Three or four is more typical. (It gets slower as the
-// altitude/error gets larger; at 50000m altitude, it starts to need
-// seven loops.) One caveat is that at *very* large altitudes, it
-// starts making very poor guesses as to latitude. As altitude
-// approaches infinity, it should be guessing with geocentric
-// coordinates, not "local geodetic up" ones.
-void sgCartToGeod(double* xyz, double* lat, double* lon, double* alt)
-{
- // The error is expressed as a radian angle, and we want accuracy
- // to 1 part in 2^50 (an IEEE double has between 51 and 52
- // significant bits of magnitude due to the "hidden" digit; leave
- // at least one bit free for potential slop). In real units, this
- // works out to about 6 nanometers.
- static const double MAX_LAT_ERROR = 8.881784197001252e-16;
- double x = xyz[0], y = xyz[1], z = xyz[2];
-
- // Longitude is trivial. Convert to cylindrical "(r, z)"
- // coordinates while we're at it.
- *lon = atan2(y, x);
- double r = sqrt(x*x + y*y);
-
- double lat1, lat2 = localLat(r, z);
- double r2, z2, dot;
- do {
- lat1 = lat2;
-
- // Compute an "up" vector
- double upr = cos(lat1);
- double upz = sin(lat1);
-
- // Find the surface point with that latitude
- surfRZ(upr, upz, &r2, &z2);
-
- // Convert r2z2 to the vector pointing from the surface to rz
- r2 = r - r2;
- z2 = z - z2;
-
- // Dot it with "up" to get an approximate altitude
- dot = r2*upr + z2*upz;
-
- // And compute an approximate geodetic surface coordinate
- // using that altitude, so now: R2Z2 = RZ - ((RZ - SURF) dot
- // UP)
- r2 = r - dot * upr;
- z2 = z - dot * upz;
-
- // Find the latitude of *that* point, and iterate
- lat2 = localLat(r2, z2);
- } while(fabs(lat2 - lat1) > MAX_LAT_ERROR);
-
- // All done! We have an accurate geodetic lattitude, now
- // calculate the altitude as a cartesian distance between the
- // final geodetic surface point and the initial r/z coordinate.
- *lat = lat1;
- double dr = r - r2;
- double dz = z - z2;
- double altsign = (dot > 0) ? 1 : -1;
- *alt = altsign * sqrt(dr*dr + dz*dz);
-}
-
-void sgGeodToCart(double lat, double lon, double alt, double* xyz)
-{
- // This is the inverse of the algorithm in localLat(). We are
- // converting a (2D, cylindrical) "up" vector defined by the
- // geodetic latitude into unitless R and Z coordinates in
- // cartesian space.
- double upr = cos(lat);
- double upz = sin(lat);
- double r, z;
- surfRZ(upr, upz, &r, &z);
-
- // Add the altitude using the "up" unit vector we calculated
- // initially.
- r += upr * alt;
- z += upz * alt;
-
- // Finally, convert from cylindrical to cartesian
- xyz[0] = r * cos(lon);
- xyz[1] = r * sin(lon);
- xyz[2] = z;
-}
-
-void sgGeocToGeod(double lat_geoc, double radius,
- double *lat_geod, double *alt, double *sea_level_r)
-{
- // Build a fake cartesian point, and run it through CartToGeod
- double lon_dummy, xyz[3];
- xyz[0] = cos(lat_geoc) * radius;
- xyz[1] = 0;
- xyz[2] = sin(lat_geoc) * radius;
- sgCartToGeod(xyz, lat_geod, &lon_dummy, alt);
- *sea_level_r = seaLevelRadius(xyz[0], xyz[2]);
-}
-
-void sgGeodToGeoc(double lat_geod, double alt,
- double *sl_radius, double *lat_geoc)
-{
- double xyz[3];
- sgGeodToCart(lat_geod, 0, alt, xyz);
- *lat_geoc = atan2(xyz[2], xyz[0]);
- *sl_radius = seaLevelRadius(xyz[0], xyz[2]);
-}
-
////////////////////////////////////////////////////////////////////////
//
// Direct and inverse distance functions
projects / simgear.git / blobdiff