From: curt Date: Wed, 27 Sep 2000 18:00:04 +0000 (+0000) Subject: Cleaned up a few poluting #defines. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=7d7e41dacc78c3e997b77a8b0be6b4fed8c1adf2;p=simgear.git Cleaned up a few poluting #defines. Did some fg -> sg name space updating (lots more to do but we'll get there eventually) --- diff --git a/simgear/constants.h b/simgear/constants.h index 63f6ad94..4347bd70 100644 --- a/simgear/constants.h +++ b/simgear/constants.h @@ -86,13 +86,14 @@ #define RESQ_FT 437882827922500. // ft #define RESQ_M 40680645877797.1344 // meter +#if 0 // Value of earth flattening parameter from ref [8] // // Note: FP = f // E = 1-f // EPS = sqrt(1-(1-f)^2) // - + #define FP 0.003352813178 #define E 0.996647186 #define EPS 0.081819221 @@ -103,7 +104,7 @@ #define MJD0 2415020.0 #define J2000 (2451545.0 - MJD0) #define SIDRATE .9972695677 - +#endif // Conversions diff --git a/simgear/io/sg_socket.cxx b/simgear/io/sg_socket.cxx index 43401357..6336e76b 100644 --- a/simgear/io/sg_socket.cxx +++ b/simgear/io/sg_socket.cxx @@ -215,15 +215,29 @@ bool SGSocket::open( SGProtocolDir dir ) { // this means client for now sock = make_client_socket(); - // TODO: check for error. + // TODO: check for error. if ( sock_style == SOCK_DGRAM ) { // Non-blocking UDP nonblock(); } + } else if ( dir == SG_IO_BI && sock_style == SOCK_STREAM ) { + // this means server for TCP sockets + + // Setup socket to listen on. Set "port" before making this + // call. A port of "0" indicates that we want to let the os + // pick any available port. + sock = make_server_socket(); + // TODO: check for error. + + FG_LOG( FG_IO, FG_INFO, "socket is connected to port = " << port ); + + // Blocking TCP + // Specify the maximum length of the connection queue + listen( sock, SG_MAX_SOCKET_QUEUE ); } else { FG_LOG( FG_IO, FG_ALERT, - "Error: bidirection mode not available yet for sockets." ); + "Error: bidirection mode not available for UDP sockets." ); return false; } diff --git a/simgear/math/Makefile.am b/simgear/math/Makefile.am index 6fad91b0..5ad69704 100644 --- a/simgear/math/Makefile.am +++ b/simgear/math/Makefile.am @@ -9,24 +9,25 @@ endif lib_LIBRARIES = libsgmath.a include_HEADERS = \ - fg_geodesy.hxx \ fg_memory.h \ fg_random.h \ interpolater.hxx \ leastsqs.hxx \ + localconsts.hxx \ point3d.hxx \ polar3d.hxx \ + sg_geodesy.hxx \ sg_types.hxx \ vector.hxx EXTRA_DIST = linintp2.h linintp2.inl sphrintp.h sphrintp.inl libsgmath_a_SOURCES = \ - fg_geodesy.cxx \ fg_random.c \ interpolater.cxx \ leastsqs.cxx \ polar3d.cxx \ + sg_geodesy.cxx \ vector.cxx INCLUDES += -I$(top_srcdir) $(ZLIB_INCL) diff --git a/simgear/math/fg_geodesy.cxx b/simgear/math/fg_geodesy.cxx deleted file mode 100644 index 506ae877..00000000 --- a/simgear/math/fg_geodesy.cxx +++ /dev/null @@ -1,449 +0,0 @@ -// fg_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 FG_HAVE_STD_INCLUDES -# include -# include -#else -# include -# include -#endif - -#include -#include - -#include "point3d.hxx" -#include "fg_geodesy.hxx" - -#ifndef FG_HAVE_NATIVE_SGI_COMPILERS -FG_USING_STD(cout); -#endif - -// ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator -#define ONE_SECOND 4.848136811E-6 - - -// fgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r) -// INPUTS: -// lat_geoc Geocentric latitude, radians, + = North -// radius C.G. radius to earth center (meters) -// -// 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 fgGeocToGeod( double lat_geoc, double radius, double - *lat_geod, double *alt, double *sea_level_r ) -{ - 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( ( (FG_PI_2 - lat_geoc) < ONE_SECOND ) // near North pole - || ( (FG_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); - // cout << " x_alpha = " << x_alpha << endl; - double tmp = RESQ_M - x_alpha * x_alpha; - if ( tmp < 0.0 ) { tmp = 0.0; } - mu_alpha = atan2(sqrt(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); - - // check for domain error - if ( errno == EDOM ) { - FG_LOG( FG_GENERAL, FG_ALERT, "Domain ERROR in fgGeocToGeod!!!!" ); - *alt = 0.0; - } - - denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a); - 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)); - - // check for domain error - if ( errno == EDOM ) { - FG_LOG( FG_GENERAL, FG_ALERT, "Domain ERROR in fgGeocToGeod!!!!" ); - *sea_level_r = 0.0; - } - } - -} - - -// fgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) -// INPUTS: -// lat_geod Geodetic latitude, radians, + = North -// alt C.G. altitude above mean sea level (meters) -// -// 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 -// - - -void fgGeodToGeoc( 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; - - 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)); - 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 -// Computations, 1985 -// -// "The Nested Coefficient Method for Accurate Solutions of Direct and -// Inverse Geodetic Problems With Any Length" -// -// Zhang Xue-Lian -// pp 747-763 -// -// modified for FlightGear to use WGS84 only -- Norman Vine - -#define GEOD_INV_PI FG_PI - -// s == distance -// az = azimuth - -// for WGS_84 a = 6378137.000, rf = 298.257223563; - -static 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 -// 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 ) -{ - double a = 6378137.000, rf = 298.257223563; - 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); - double e2 = f*(2.0-f); - double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG; - double sinphi1 = sin(phi1), cosphi1 = cos(phi1); - double azm1 = az1*RADDEG; - double sinaz1 = sin(azm1), cosaz1 = cos(azm1); - - - if( fabs(s) < 0.01 ) { // distance < centimeter => congruency - *lat2 = lat1; - *lon2 = lon1; - *az2 = 180.0 + az1; - if( *az2 > 360.0 ) *az2 -= 360.0; - return 0; - } else if( cosphi1 ) { // non-polar origin - // u1 is reduced latitude - double tanu1 = sqrt(1.0-e2)*sinphi1/cosphi1; - double sig1 = atan2(tanu1,cosaz1); - double cosu1 = 1.0/sqrt( 1.0 + tanu1*tanu1 ), sinu1 = tanu1*cosu1; - double sinaz = cosu1*sinaz1, cos2saz = 1.0-sinaz*sinaz; - double us = cos2saz*e2/(1.0-e2); - - // Terms - double ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/16384.0, - tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0, - tc = 0; - - // FIRST ESTIMATE OF SIGMA (SIG) - double first = s/(b*ta); // !! - double sig = first; - double c2sigm, sinsig,cossig, temp,denom,rnumer, dlams, dlam; - do { - c2sigm = cos(2.0*sig1+sig); - sinsig = sin(sig); cossig = cos(sig); - temp = sig; - sig = first + - tb*sinsig*(c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm) - - tb*c2sigm*(-3.0+4.0*sinsig*sinsig) - *(-3.0+4.0*c2sigm*c2sigm)/6.0) - /4.0); - } while( fabs(sig-temp) > testv); - - // LATITUDE OF POINT 2 - // DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER) - temp = sinu1*sinsig-cosu1*cossig*cosaz1; - denom = (1.0-f)*sqrt(sinaz*sinaz+temp*temp); - - // NUMERATOR - rnumer = sinu1*cossig+cosu1*sinsig*cosaz1; - *lat2 = atan2(rnumer,denom)/RADDEG; - - // DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS ) - rnumer = sinsig*sinaz1; - denom = cosu1*cossig-sinu1*sinsig*cosaz1; - dlams = atan2(rnumer,denom); - - // TERM C - tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0; - - // DIFFERENCE IN LONGITUDE - dlam = dlams-(1.0-tc)*f*sinaz*(sig+tc*sinsig* - (c2sigm+ - tc*cossig*(-1.0+2.0* - c2sigm*c2sigm))); - *lon2 = (lam1+dlam)/RADDEG; - if (*lon2 > 180.0 ) *lon2 -= 360.0; - if (*lon2 < -180.0 ) *lon2 += 360.0; - - // AZIMUTH - FROM NORTH - *az2 = atan2(-sinaz,temp)/RADDEG; - if ( fabs(*az2) < testv ) *az2 = 0.0; - if( *az2 < 0.0) *az2 += 360.0; - return 0; - } 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 ); - } -} - - -// given alt, 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 ) -{ - double a = 6378137.000, rf = 298.257223563; - int iter=0; - 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); - // double e2 = f*(2.0-f); // unused in this routine - double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG; - double sinphi1 = sin(phi1), cosphi1 = cos(phi1); - double phi2 = lat2*RADDEG, lam2 = lon2*RADDEG; - double sinphi2 = sin(phi2), cosphi2 = cos(phi2); - - if( (fabs(lat1-lat2) < testv && - ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) ) - { - // TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */ - *az1 = 0.0; *az2 = 0.0; *s = 0.0; - 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 ); - 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 ); - k = k; // avoid compiler error since return result is unused - *s /= 2.0; - *az2 = *az1 + 180.0; - if( *az2 > 360.0 ) *az2 -= 360.0; - return 0; - } else if( (fabs( fabs(lon1-lon2) - 180 ) < testv) && - (fabs(lat1+lat2) < testv) ) - { - // 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 ); - *az2 = *az1; - *s = s1 + s2; - return 0; - } else { - // antipodal and polar points don't get here - double dlam = lam2 - lam1, dlams = dlam; - double sdlams,cdlams, sig,sinsig,cossig, sinaz, - cos2saz, c2sigm; - double tc,temp, us,rnumer,denom, ta,tb; - double cosu1,sinu1, sinu2,cosu2; - - // Reduced latitudes - temp = (1.0-f)*sinphi1/cosphi1; - cosu1 = 1.0/sqrt(1.0+temp*temp); - sinu1 = temp*cosu1; - temp = (1.0-f)*sinphi2/cosphi2; - cosu2 = 1.0/sqrt(1.0+temp*temp); - sinu2 = temp*cosu2; - - do { - sdlams = sin(dlams), cdlams = cos(dlams); - sinsig = sqrt(cosu2*cosu2*sdlams*sdlams+ - (cosu1*sinu2-sinu1*cosu2*cdlams)* - (cosu1*sinu2-sinu1*cosu2*cdlams)); - cossig = sinu1*sinu2+cosu1*cosu2*cdlams; - - sig = atan2(sinsig,cossig); - sinaz = cosu1*cosu2*sdlams/sinsig; - cos2saz = 1.0-sinaz*sinaz; - c2sigm = (sinu1 == 0.0 || sinu2 == 0.0 ? cossig : - cossig-2.0*sinu1*sinu2/cos2saz); - tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0; - temp = dlams; - dlams = dlam+(1.0-tc)*f*sinaz* - (sig+tc*sinsig* - (c2sigm+tc*cossig*(-1.0+2.0*c2sigm*c2sigm))); - if (fabs(dlams) > GEOD_INV_PI && iter++ > 50) { - return iter; - } - } while ( fabs(temp-dlams) > testv); - - us = cos2saz*(a*a-b*b)/(b*b); // !! - // BACK AZIMUTH FROM NORTH - rnumer = -(cosu1*sdlams); - denom = sinu1*cosu2-cosu1*sinu2*cdlams; - *az2 = atan2(rnumer,denom)/RADDEG; - if( fabs(*az2) < testv ) *az2 = 0.0; - if(*az2 < 0.0) *az2 += 360.0; - - // FORWARD AZIMUTH FROM NORTH - rnumer = cosu2*sdlams; - denom = cosu1*sinu2-sinu1*cosu2*cdlams; - *az1 = atan2(rnumer,denom)/RADDEG; - if( fabs(*az1) < testv ) *az1 = 0.0; - if(*az1 < 0.0) *az1 += 360.0; - - // Terms a & b - ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/ - 16384.0; - tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0; - - // GEODETIC DISTANCE - *s = b*ta*(sig-tb*sinsig* - (c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm)-tb* - c2sigm*(-3.0+4.0*sinsig*sinsig)* - (-3.0+4.0*c2sigm*c2sigm)/6.0)/ - 4.0)); - 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. - ---------------------------------------------------------------------------*/ - - diff --git a/simgear/math/fg_geodesy.hxx b/simgear/math/fg_geodesy.hxx deleted file mode 100644 index cff20210..00000000 --- a/simgear/math/fg_geodesy.hxx +++ /dev/null @@ -1,175 +0,0 @@ -// fg_geodesy.hxx -- 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$ - - -#ifndef _FG_GEODESY_HXX -#define _FG_GEODESY_HXX - - -#ifndef __cplusplus -# error This library requires C++ -#endif - - -#include -#include - - -// fgGeocToGeod(lat_geoc, radius, *lat_geod, *alt, *sea_level_r) -// INPUTS: -// lat_geoc Geocentric latitude, radians, + = North -// radius C.G. radius to earth center (meters) -// -// 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 fgGeocToGeod( double lat_geoc, double radius, double - *lat_geod, double *alt, double *sea_level_r ); - - -// fgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) -// INPUTS: -// lat_geod Geodetic latitude, radians, + = North -// alt C.G. altitude above mean sea level (meters) -// -// 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 -// - -void fgGeodToGeoc( double lat_geod, double alt, double *sl_radius, - double *lat_geoc ); - - -// convert a geodetic point lon(radians), lat(radians), elev(meter) to -// a cartesian point - -inline Point3D fgGeodToCart(const Point3D& geod) { - double gc_lon, gc_lat, sl_radius; - - // printf("A geodetic point is (%.2f, %.2f, %.2f)\n", - // geod[0], geod[1], geod[2]); - - gc_lon = geod.lon(); - fgGeodToGeoc(geod.lat(), geod.radius(), &sl_radius, &gc_lat); - - // printf("A geocentric point is (%.2f, %.2f, %.2f)\n", gc_lon, - // gc_lat, sl_radius+geod[2]); - - Point3D pp = Point3D( gc_lon, gc_lat, sl_radius + geod.radius()); - return fgPolarToCart3d(pp); -} - - -// given, alt, 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 ); - - -// given alt, 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 ); - - -/*************************************************************************** - - 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. - ---------------------------------------------------------------------------*/ - - -#endif // _FG_GEODESY_HXX diff --git a/simgear/math/localconsts.hxx b/simgear/math/localconsts.hxx new file mode 100644 index 00000000..14745e1e --- /dev/null +++ b/simgear/math/localconsts.hxx @@ -0,0 +1,42 @@ +// localconsts.hxx -- various constant that are shared +// +// Written by Curtis Olson, started September 2000. +// +// Copyright (C) 2000 Curtis L. Olson - curt@flightgear.org +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Library General Public +// License as published by the Free Software Foundation; either +// version 2 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Library General Public License for more details. +// +// You should have received a copy of the GNU Library General Public +// License along with this library; if not, write to the +// Free Software Foundation, Inc., 59 Temple Place - Suite 330, +// Boston, MA 02111-1307, USA. +// +// $Id$ + + +#ifndef _SG_LOCAL_CONSTS_HXX +#define _SG_LOCAL_CONSTS_HXX + + +// Value of earth flattening parameter from ref [8] +// +// Note: FP = f +// E = 1-f +// EPS = sqrt(1-(1-f)^2) +// + +static const double FP = 0.003352813178; +static const double E = 0.996647186; +static const double EPS = 0.081819221; +static const double INVG = 0.031080997; + + +#endif // _SG_LOCAL_CONSTS_HXX diff --git a/simgear/math/point3d.hxx b/simgear/math/point3d.hxx index 8ce271c3..26284a15 100644 --- a/simgear/math/point3d.hxx +++ b/simgear/math/point3d.hxx @@ -46,6 +46,8 @@ # include #endif +#include "localconsts.hxx" + // I don't understand ... or should be included // already depending on how you defined FG_HAVE_STD_INCLUDES, but I // can go ahead and add this -- CLO diff --git a/simgear/math/polar3d.hxx b/simgear/math/polar3d.hxx index 8dfc6a44..acd24ae7 100644 --- a/simgear/math/polar3d.hxx +++ b/simgear/math/polar3d.hxx @@ -40,13 +40,13 @@ // Find the Altitude above the Ellipsoid (WGS84) given the Earth // Centered Cartesian coordinate vector Distances are specified in // meters. -double fgGeodAltFromCart(const Point3D& cp); +double sgGeodAltFromCart(const Point3D& cp); // Convert a polar coordinate to a cartesian coordinate. Lon and Lat -// must be specified in radians. The FG convention is for distances +// must be specified in radians. The SG convention is for distances // to be specified in meters -inline Point3D fgPolarToCart3d(const Point3D& p) { +inline Point3D sgPolarToCart3d(const Point3D& p) { double tmp = cos( p.lat() ) * p.radius(); return Point3D( cos( p.lon() ) * tmp, @@ -57,7 +57,7 @@ inline Point3D fgPolarToCart3d(const Point3D& p) { // Convert a cartesian coordinate to polar coordinates (lon/lat // specified in radians. Distances are specified in meters. -inline Point3D fgCartToPolar3d(const Point3D& cp) { +inline Point3D sgCartToPolar3d(const Point3D& cp) { return Point3D( atan2( cp.y(), cp.x() ), FG_PI_2 - atan2( sqrt(cp.x()*cp.x() + cp.y()*cp.y()), cp.z() ), diff --git a/simgear/math/sg_geodesy.cxx b/simgear/math/sg_geodesy.cxx new file mode 100644 index 00000000..f376032d --- /dev/null +++ b/simgear/math/sg_geodesy.cxx @@ -0,0 +1,451 @@ +// 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 FG_HAVE_STD_INCLUDES +# include +# include +#else +# include +# include +#endif + +#include +#include + +#include "point3d.hxx" +#include "sg_geodesy.hxx" +#include "localconsts.hxx" + + +#ifndef FG_HAVE_NATIVE_SGI_COMPILERS +FG_USING_STD(cout); +#endif + +// ONE_SECOND is pi/180/60/60, or about 100 feet at earths' equator +#define ONE_SECOND 4.848136811E-6 + + +// 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) +// +// 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 ) +{ + 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( ( (FG_PI_2 - lat_geoc) < ONE_SECOND ) // near North pole + || ( (FG_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); + // cout << " x_alpha = " << x_alpha << endl; + double tmp = RESQ_M - x_alpha * x_alpha; + if ( tmp < 0.0 ) { tmp = 0.0; } + mu_alpha = atan2(sqrt(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); + + // check for domain error + if ( errno == EDOM ) { + FG_LOG( FG_GENERAL, FG_ALERT, "Domain ERROR in fgGeocToGeod!!!!" ); + *alt = 0.0; + } + + denom = sqrt(1-EPS*EPS*sin_mu_a*sin_mu_a); + 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)); + + // check for domain error + if ( errno == EDOM ) { + FG_LOG( FG_GENERAL, FG_ALERT, "Domain ERROR in sgGeocToGeod!!!!" ); + *sea_level_r = 0.0; + } + } + +} + + +// sgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) +// INPUTS: +// lat_geod Geodetic latitude, radians, + = North +// alt C.G. altitude above mean sea level (meters) +// +// 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 +// + + +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; + + 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)); + 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 +// Computations, 1985 +// +// "The Nested Coefficient Method for Accurate Solutions of Direct and +// Inverse Geodetic Problems With Any Length" +// +// Zhang Xue-Lian +// pp 747-763 +// +// modified for FlightGear to use WGS84 only -- Norman Vine + +#define GEOD_INV_PI FG_PI + +// s == distance +// az = azimuth + +// for WGS_84 a = 6378137.000, rf = 298.257223563; + +static 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 +// 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 ) +{ + double a = 6378137.000, rf = 298.257223563; + 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); + double e2 = f*(2.0-f); + double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG; + double sinphi1 = sin(phi1), cosphi1 = cos(phi1); + double azm1 = az1*RADDEG; + double sinaz1 = sin(azm1), cosaz1 = cos(azm1); + + + if( fabs(s) < 0.01 ) { // distance < centimeter => congruency + *lat2 = lat1; + *lon2 = lon1; + *az2 = 180.0 + az1; + if( *az2 > 360.0 ) *az2 -= 360.0; + return 0; + } else if( cosphi1 ) { // non-polar origin + // u1 is reduced latitude + double tanu1 = sqrt(1.0-e2)*sinphi1/cosphi1; + double sig1 = atan2(tanu1,cosaz1); + double cosu1 = 1.0/sqrt( 1.0 + tanu1*tanu1 ), sinu1 = tanu1*cosu1; + double sinaz = cosu1*sinaz1, cos2saz = 1.0-sinaz*sinaz; + double us = cos2saz*e2/(1.0-e2); + + // Terms + double ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/16384.0, + tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0, + tc = 0; + + // FIRST ESTIMATE OF SIGMA (SIG) + double first = s/(b*ta); // !! + double sig = first; + double c2sigm, sinsig,cossig, temp,denom,rnumer, dlams, dlam; + do { + c2sigm = cos(2.0*sig1+sig); + sinsig = sin(sig); cossig = cos(sig); + temp = sig; + sig = first + + tb*sinsig*(c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm) - + tb*c2sigm*(-3.0+4.0*sinsig*sinsig) + *(-3.0+4.0*c2sigm*c2sigm)/6.0) + /4.0); + } while( fabs(sig-temp) > testv); + + // LATITUDE OF POINT 2 + // DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER) + temp = sinu1*sinsig-cosu1*cossig*cosaz1; + denom = (1.0-f)*sqrt(sinaz*sinaz+temp*temp); + + // NUMERATOR + rnumer = sinu1*cossig+cosu1*sinsig*cosaz1; + *lat2 = atan2(rnumer,denom)/RADDEG; + + // DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS ) + rnumer = sinsig*sinaz1; + denom = cosu1*cossig-sinu1*sinsig*cosaz1; + dlams = atan2(rnumer,denom); + + // TERM C + tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0; + + // DIFFERENCE IN LONGITUDE + dlam = dlams-(1.0-tc)*f*sinaz*(sig+tc*sinsig* + (c2sigm+ + tc*cossig*(-1.0+2.0* + c2sigm*c2sigm))); + *lon2 = (lam1+dlam)/RADDEG; + if (*lon2 > 180.0 ) *lon2 -= 360.0; + if (*lon2 < -180.0 ) *lon2 += 360.0; + + // AZIMUTH - FROM NORTH + *az2 = atan2(-sinaz,temp)/RADDEG; + if ( fabs(*az2) < testv ) *az2 = 0.0; + if( *az2 < 0.0) *az2 += 360.0; + return 0; + } 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 ); + } +} + + +// given alt, 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 ) +{ + double a = 6378137.000, rf = 298.257223563; + int iter=0; + 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); + // double e2 = f*(2.0-f); // unused in this routine + double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG; + double sinphi1 = sin(phi1), cosphi1 = cos(phi1); + double phi2 = lat2*RADDEG, lam2 = lon2*RADDEG; + double sinphi2 = sin(phi2), cosphi2 = cos(phi2); + + if( (fabs(lat1-lat2) < testv && + ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) ) + { + // TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */ + *az1 = 0.0; *az2 = 0.0; *s = 0.0; + 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 ); + 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 ); + k = k; // avoid compiler error since return result is unused + *s /= 2.0; + *az2 = *az1 + 180.0; + if( *az2 > 360.0 ) *az2 -= 360.0; + return 0; + } else if( (fabs( fabs(lon1-lon2) - 180 ) < testv) && + (fabs(lat1+lat2) < testv) ) + { + // 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 ); + *az2 = *az1; + *s = s1 + s2; + return 0; + } else { + // antipodal and polar points don't get here + double dlam = lam2 - lam1, dlams = dlam; + double sdlams,cdlams, sig,sinsig,cossig, sinaz, + cos2saz, c2sigm; + double tc,temp, us,rnumer,denom, ta,tb; + double cosu1,sinu1, sinu2,cosu2; + + // Reduced latitudes + temp = (1.0-f)*sinphi1/cosphi1; + cosu1 = 1.0/sqrt(1.0+temp*temp); + sinu1 = temp*cosu1; + temp = (1.0-f)*sinphi2/cosphi2; + cosu2 = 1.0/sqrt(1.0+temp*temp); + sinu2 = temp*cosu2; + + do { + sdlams = sin(dlams), cdlams = cos(dlams); + sinsig = sqrt(cosu2*cosu2*sdlams*sdlams+ + (cosu1*sinu2-sinu1*cosu2*cdlams)* + (cosu1*sinu2-sinu1*cosu2*cdlams)); + cossig = sinu1*sinu2+cosu1*cosu2*cdlams; + + sig = atan2(sinsig,cossig); + sinaz = cosu1*cosu2*sdlams/sinsig; + cos2saz = 1.0-sinaz*sinaz; + c2sigm = (sinu1 == 0.0 || sinu2 == 0.0 ? cossig : + cossig-2.0*sinu1*sinu2/cos2saz); + tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0; + temp = dlams; + dlams = dlam+(1.0-tc)*f*sinaz* + (sig+tc*sinsig* + (c2sigm+tc*cossig*(-1.0+2.0*c2sigm*c2sigm))); + if (fabs(dlams) > GEOD_INV_PI && iter++ > 50) { + return iter; + } + } while ( fabs(temp-dlams) > testv); + + us = cos2saz*(a*a-b*b)/(b*b); // !! + // BACK AZIMUTH FROM NORTH + rnumer = -(cosu1*sdlams); + denom = sinu1*cosu2-cosu1*sinu2*cdlams; + *az2 = atan2(rnumer,denom)/RADDEG; + if( fabs(*az2) < testv ) *az2 = 0.0; + if(*az2 < 0.0) *az2 += 360.0; + + // FORWARD AZIMUTH FROM NORTH + rnumer = cosu2*sdlams; + denom = cosu1*sinu2-sinu1*cosu2*cdlams; + *az1 = atan2(rnumer,denom)/RADDEG; + if( fabs(*az1) < testv ) *az1 = 0.0; + if(*az1 < 0.0) *az1 += 360.0; + + // Terms a & b + ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/ + 16384.0; + tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0; + + // GEODETIC DISTANCE + *s = b*ta*(sig-tb*sinsig* + (c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm)-tb* + c2sigm*(-3.0+4.0*sinsig*sinsig)* + (-3.0+4.0*c2sigm*c2sigm)/6.0)/ + 4.0)); + 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. + +--------------------------------------------------------------------------*/ + + diff --git a/simgear/math/sg_geodesy.hxx b/simgear/math/sg_geodesy.hxx new file mode 100644 index 00000000..b9f7d6db --- /dev/null +++ b/simgear/math/sg_geodesy.hxx @@ -0,0 +1,175 @@ +// sg_geodesy.hxx -- 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$ + + +#ifndef _SG_GEODESY_HXX +#define _SG_GEODESY_HXX + + +#ifndef __cplusplus +# error This library requires C++ +#endif + + +#include +#include + + +// 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) +// +// 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 ); + + +// sgGeodToGeoc( lat_geod, alt, *sl_radius, *lat_geoc ) +// INPUTS: +// lat_geod Geodetic latitude, radians, + = North +// alt C.G. altitude above mean sea level (meters) +// +// 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 +// + +void sgGeodToGeoc( double lat_geod, double alt, double *sl_radius, + double *lat_geoc ); + + +// convert a geodetic point lon(radians), lat(radians), elev(meter) to +// a cartesian point + +inline Point3D sgGeodToCart(const Point3D& geod) { + double gc_lon, gc_lat, sl_radius; + + // printf("A geodetic point is (%.2f, %.2f, %.2f)\n", + // geod[0], geod[1], geod[2]); + + gc_lon = geod.lon(); + sgGeodToGeoc(geod.lat(), geod.radius(), &sl_radius, &gc_lat); + + // printf("A geocentric point is (%.2f, %.2f, %.2f)\n", gc_lon, + // gc_lat, sl_radius+geod[2]); + + Point3D pp = Point3D( gc_lon, gc_lat, sl_radius + geod.radius()); + return sgPolarToCart3d(pp); +} + + +// given, alt, 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 ); + + +// given alt, 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 ); + + +/*************************************************************************** + + 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. + +--------------------------------------------------------------------------*/ + + +#endif // _SG_GEODESY_HXX diff --git a/simgear/timing/sg_time.cxx b/simgear/timing/sg_time.cxx index f9467739..16d50d3e 100644 --- a/simgear/timing/sg_time.cxx +++ b/simgear/timing/sg_time.cxx @@ -63,6 +63,11 @@ #define RADHR(x) DEGHR(x*RAD_TO_DEG) +static const double MJD0 = 2415020.0; +static const double J2000 = 2451545.0 - MJD0; +static const double SIDRATE = 0.9972695677; + + SGTime::SGTime( double lon, double lat, const string& root ) { FG_LOG( FG_EVENT, FG_INFO, "Initializing Time" );