+
+#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
}
-// given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2
+// given, lat1, lon1, az1 and distance (s), calculate lat2, lon2
// and az2. Lat, lon, and azimuth are in degrees. distance in meters
-int geo_direct_wgs_84 ( double alt, double lat1,
- double lon1, double az1,
+int geo_direct_wgs_84 ( double lat1, double lon1, double az1,
double s, double *lat2, double *lon2,
double *az2 )
{
double dM = a*M0(e2) - s;
double paz = ( phi1 < 0.0 ? 180.0 : 0.0 );
double zero = 0.0f;
- return geo_direct_wgs_84( alt, zero, lon1, paz, dM, lat2, lon2, az2 );
+ return geo_direct_wgs_84( zero, lon1, paz, dM, lat2, lon2, az2 );
}
}
-// given alt, lat1, lon1, lat2, lon2, calculate starting and ending
+// given lat1, lon1, lat2, lon2, calculate starting and ending
// az1, az2 and distance (s). Lat, lon, and azimuth are in degrees.
// distance in meters
-int geo_inverse_wgs_84( double alt, double lat1,
- double lon1, double lat2,
+int geo_inverse_wgs_84( double lat1, double lon1, double lat2,
double lon2, double *az1, double *az2,
double *s )
{
return 0;
} else if( fabs(cosphi1) < testv ) {
// initial point is polar
- int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
+ int k = geo_inverse_wgs_84( lat2,lon2,lat1,lon1, az1,az2,s );
k = k; // avoid compiler error since return result is unused
b = *az1; *az1 = *az2; *az2 = b;
return 0;
} else if( fabs(cosphi2) < testv ) {
// terminal point is polar
double _lon1 = lon1 + 180.0f;
- int k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1,
+ int k = geo_inverse_wgs_84( lat1, lon1, lat1, _lon1,
az1, az2, s );
k = k; // avoid compiler error since return result is unused
*s /= 2.0;
{
// Geodesic passes through the pole (antipodal)
double s1,s2;
- geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
- geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
+ geo_inverse_wgs_84( lat1,lon1, lat1,lon2, az1,az2, &s1 );
+ geo_inverse_wgs_84( lat2,lon2, lat1,lon2, az1,az2, &s2 );
*az2 = *az1;
*s = s1 + s2;
return 0;
projects / simgear.git / blobdiff