X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2Fsg_geodesy.cxx;h=adba76bc3f75f2ffa8d35bec4b2ee8f333f8c7dd;hb=de020ee69524393daf11200aa0a46bfd5aa2409a;hp=f26c0e934c8a3aceb484ca486eb83f2f018de673;hpb=0e8c0106451afb543b9c4d4a74abdc90f09141e3;p=simgear.git diff --git a/simgear/math/sg_geodesy.cxx b/simgear/math/sg_geodesy.cxx index f26c0e93..adba76bc 100644 --- a/simgear/math/sg_geodesy.cxx +++ b/simgear/math/sg_geodesy.cxx @@ -1,172 +1,53 @@ -// sg_geodesy.cxx -- routines to convert between geodetic and geocentric -// coordinate systems. -// -// Copied and adapted directly from LaRCsim/ls_geodesy.c -// -// See below for the complete original LaRCsim comments. -// -// $Id$ -#include - -#ifdef SG_HAVE_STD_INCLUDES -# include -# include -# include -#else -# include -# include -# include +#ifdef HAVE_CONFIG_H +# include #endif #include -#include - -#include "point3d.hxx" +#include "SGMath.hxx" #include "sg_geodesy.hxx" -#include "localconsts.hxx" - - -#ifndef SG_HAVE_NATIVE_SGI_COMPILERS -SG_USING_STD(cout); -#endif - -// ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator -#define ONE_SECOND 4.848136811E-6 - -#define DOMAIN_ERR_DEBUG 1 - - -// sgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r) -// INPUTS: -// lat_geoc Geocentric latitude, radians, + = North -// radius C.G. radius to earth center (meters) +// Notes: // -// OUTPUTS: -// lat_geod Geodetic latitude, radians, + = North -// alt C.G. altitude above mean sea level (meters) -// sea_level_r radius from earth center to sea level at -// local vertical (surface normal) of C.G. (meters) - - -void sgGeocToGeod( double lat_geoc, double radius, double - *lat_geod, double *alt, double *sea_level_r ) -{ -#ifdef DOMAIN_ERR_DEBUG - errno = 0; // start with error zero'd -#endif - - double t_lat, x_alpha, mu_alpha, delt_mu, r_alpha, l_point, rho_alpha; - double sin_mu_a, denom,delt_lambda, lambda_sl, sin_lambda_sl; - - if( ( (SGD_PI_2 - lat_geoc) < ONE_SECOND ) // near North pole - || ( (SGD_PI_2 + lat_geoc) < ONE_SECOND ) ) // near South pole - { - *lat_geod = lat_geoc; - *sea_level_r = EQUATORIAL_RADIUS_M*E; - *alt = radius - *sea_level_r; - } else { - // cout << " lat_geoc = " << lat_geoc << endl; - t_lat = tan(lat_geoc); - // cout << " tan(t_lat) = " << t_lat << endl; - x_alpha = E*EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E); -#ifdef DOMAIN_ERR_DEBUG - if ( errno ) { - perror("fgGeocToGeod()"); - FG_LOG( FG_GENERAL, FG_ALERT, "sqrt(" << t_lat*t_lat + E*E << ")" ); - } -#endif - // cout << " x_alpha = " << x_alpha << endl; - double tmp = sqrt(RESQ_M - x_alpha * x_alpha); - if ( tmp < 0.0 ) { tmp = 0.0; } -#ifdef DOMAIN_ERR_DEBUG - if ( errno ) { - perror("fgGeocToGeod()"); - FG_LOG( FG_GENERAL, FG_ALERT, "sqrt(" << RESQ_M - x_alpha * x_alpha - << ")" ); - } -#endif - mu_alpha = atan2(tmp,E*x_alpha); - if (lat_geoc < 0) mu_alpha = - mu_alpha; - sin_mu_a = sin(mu_alpha); - delt_lambda = mu_alpha - lat_geoc; - r_alpha = x_alpha/cos(lat_geoc); - l_point = radius - r_alpha; - *alt = l_point*cos(delt_lambda); - - denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a); -#ifdef DOMAIN_ERR_DEBUG - if ( errno ) { - perror("fgGeocToGeod()"); - FG_LOG( FG_GENERAL, FG_ALERT, "sqrt(" << - 1-EPS*EPS*sin_mu_a*sin_mu_a << ")" ); - } -#endif - rho_alpha = EQUATORIAL_RADIUS_M*(1-EPS)/ - (denom*denom*denom); - delt_mu = atan2(l_point*sin(delt_lambda),rho_alpha + *alt); - *lat_geod = mu_alpha - delt_mu; - lambda_sl = atan( E*E * tan(*lat_geod) ); // SL geoc. latitude - sin_lambda_sl = sin( lambda_sl ); - *sea_level_r = - sqrt(RESQ_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); -#ifdef DOMAIN_ERR_DEBUG - if ( errno ) { - perror("fgGeocToGeod()"); - FG_LOG( FG_GENERAL, FG_ALERT, "sqrt(" << - RESQ_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl) - << ")" ); - } -#endif - - } - -} - - -// sgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) -// INPUTS: -// lat_geod Geodetic latitude, radians, + = North -// alt C.G. altitude above mean sea level (meters) +// The XYZ/cartesian coordinate system in use puts the X axis through +// zero lat/lon (off west Africa), the Z axis through the north pole, +// and the Y axis through 90 degrees longitude (in the Indian Ocean). // -// OUTPUTS: -// sl_radius SEA LEVEL radius to earth center (meters) -// (add Altitude to get true distance from earth center. -// lat_geoc Geocentric latitude, radians, + = North +// All latitude and longitude values are in radians. Altitude is in +// meters, with zero on the WGS84 ellipsoid. // - - -void sgGeodToGeoc( double lat_geod, double alt, double *sl_radius, - double *lat_geoc ) -{ - double lambda_sl, sin_lambda_sl, cos_lambda_sl, sin_mu, cos_mu, px, py; - -#ifdef DOMAIN_ERR_DEBUG - errno = 0; -#endif - - lambda_sl = atan( E*E * tan(lat_geod) ); // sea level geocentric latitude - sin_lambda_sl = sin( lambda_sl ); - cos_lambda_sl = cos( lambda_sl ); - sin_mu = sin(lat_geod); // Geodetic (map makers') latitude - cos_mu = cos(lat_geod); - *sl_radius = - sqrt(RESQ_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)); -#ifdef DOMAIN_ERR_DEBUG - if ( errno ) { - perror("fgGeodToGeoc()"); - FG_LOG( FG_GENERAL, FG_ALERT, "sqrt(" << - RESQ_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl) - << ")" ); - } +// The code below makes use of the notion of "squashed" space. This +// is a 2D cylindrical coordinate system where the radius from the Z +// axis is multiplied by SQUASH; the earth in this space is a perfect +// circle with a radius of POLRAD. +// +// Performance: with full optimization, a transformation from +// lat/lon/alt to XYZ and back takes 5263 CPU cycles on my 2.2GHz +// Pentium 4. About 83% of this is spent in the iterative sgCartToGeod() +// algorithm. + +// These are hard numbers from the WGS84 standard. DON'T MODIFY +// unless you want to change the datum. +static const double EQURAD = 6378137; +static const double iFLATTENING = 298.257223563; + +// These are derived quantities more useful to the code: +#if 0 +static const double SQUASH = 1 - 1/iFLATTENING; +static const double STRETCH = 1/SQUASH; +static const double POLRAD = EQURAD * SQUASH; +#else +// High-precision versions of the above produced with an arbitrary +// precision calculator (the compiler might lose a few bits in the FPU +// operations). These are specified to 81 bits of mantissa, which is +// higher than any FPU known to me: +static const double SQUASH = 0.9966471893352525192801545; +static const double STRETCH = 1.0033640898209764189003079; +static const double POLRAD = 6356752.3142451794975639668; #endif - py = *sl_radius*sin_lambda_sl + alt*sin_mu; - px = *sl_radius*cos_lambda_sl + alt*cos_mu; - *lat_geoc = atan2( py, px ); -} - +//////////////////////////////////////////////////////////////////////// +// // Direct and inverse distance functions // // Proceedings of the 7th International Symposium on Geodetic @@ -180,26 +61,25 @@ void sgGeodToGeoc( double lat_geod, double alt, double *sl_radius, // // modified for FlightGear to use WGS84 only -- Norman Vine -#define GEOD_INV_PI SGD_PI +static const double GEOD_INV_PI = SGD_PI; // s == distance // az = azimuth -// for WGS_84 a = 6378137.000, rf = 298.257223563; - -static double M0( double e2 ) { +static inline double M0( double e2 ) { //double e4 = e2*e2; return GEOD_INV_PI*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + e2*(5.0/256.0) )))/2.0; } -// 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, - double s, double *lat2, double *lon2, double *az2 ) +int geo_direct_wgs_84 ( double lat1, double lon1, double az1, + double s, double *lat2, double *lon2, + double *az2 ) { - double a = 6378137.000, rf = 298.257223563; + double a = EQURAD, rf = iFLATTENING; double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10; double f = ( rf > 0.0 ? 1.0/rf : 0.0 ); double b = a*(1.0-f); @@ -278,18 +158,20 @@ int geo_direct_wgs_84 ( double alt, double lat1, double lon1, double az1, } else { // phi1 == 90 degrees, polar origin double dM = a*M0(e2) - s; double paz = ( phi1 < 0.0 ? 180.0 : 0.0 ); - return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM,lat2,lon2,az2 ); + double zero = 0.0f; + 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, - double lon2, double *az1, double *az2, double *s ) +int geo_inverse_wgs_84( double lat1, double lon1, double lat2, + double lon2, double *az1, double *az2, + double *s ) { - double a = 6378137.000, rf = 298.257223563; + double a = EQURAD, rf = iFLATTENING; int iter=0; double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10; double f = ( rf > 0.0 ? 1.0/rf : 0.0 ); @@ -308,14 +190,15 @@ int geo_inverse_wgs_84( double alt, double lat1, double lon1, double lat2, 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 - int k = geo_inverse_wgs_84( alt, lat1,lon1,lat1,lon1+180.0, - az1,az2,s ); + double _lon1 = lon1 + 180.0f; + 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; *az2 = *az1 + 180.0; @@ -326,8 +209,8 @@ int geo_inverse_wgs_84( double alt, double lat1, double lon1, double lat2, { // 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; @@ -398,92 +281,3 @@ int geo_inverse_wgs_84( double alt, double lat1, double lon1, double lat2, return 0; } } - - -/*************************************************************************** - - TITLE: ls_geodesy - ----------------------------------------------------------------------------- - - FUNCTION: Converts geocentric coordinates to geodetic positions - ----------------------------------------------------------------------------- - - MODULE STATUS: developmental - ----------------------------------------------------------------------------- - - GENEALOGY: Written as part of LaRCSim project by E. B. Jackson - ----------------------------------------------------------------------------- - - DESIGNED BY: E. B. Jackson - - CODED BY: E. B. Jackson - - MAINTAINED BY: E. B. Jackson - ----------------------------------------------------------------------------- - - MODIFICATION HISTORY: - - DATE PURPOSE BY - - 930208 Modified to avoid singularity near polar region. EBJ - 930602 Moved backwards calcs here from ls_step. EBJ - 931214 Changed erroneous Latitude and Altitude variables to - *lat_geod and *alt in routine ls_geoc_to_geod. EBJ - 940111 Changed header files from old ls_eom.h style to ls_types, - and ls_constants. Also replaced old DATA type with new - SCALAR type. EBJ - - CURRENT RCS HEADER: - -$Header$ - * Revision 1.5 1994/01/11 18:47:05 bjax - * Changed include files to use types and constants, not ls_eom.h - * Also changed DATA type to SCALAR type. - * - * Revision 1.4 1993/12/14 21:06:47 bjax - * Removed global variable references Altitude and Latitude. EBJ - * - * Revision 1.3 1993/06/02 15:03:40 bjax - * Made new subroutine for calculating geodetic to geocentric; changed name - * of forward conversion routine from ls_geodesy to ls_geoc_to_geod. - * - ----------------------------------------------------------------------------- - - REFERENCES: - - [ 1] Stevens, Brian L.; and Lewis, Frank L.: "Aircraft - Control and Simulation", Wiley and Sons, 1992. - ISBN 0-471-61397-5 - - ----------------------------------------------------------------------------- - - CALLED BY: ls_aux - ----------------------------------------------------------------------------- - - CALLS TO: - ----------------------------------------------------------------------------- - - INPUTS: - lat_geoc Geocentric latitude, radians, + = North - radius C.G. radius to earth center, ft - ----------------------------------------------------------------------------- - - OUTPUTS: - lat_geod Geodetic latitude, radians, + = North - alt C.G. altitude above mean sea level, ft - sea_level_r radius from earth center to sea level at - local vertical (surface normal) of C.G. - ---------------------------------------------------------------------------*/ - -