Patch from Melchior Franz:

[simgear.git] / simgear / math / sg_geodesy.cxx

diff --git a/simgear/math/sg_geodesy.cxx b/simgear/math/sg_geodesy.cxx
index f376032d25fea8aac9e8f8b089d8083678603688..4393778d52fccbd5b8967609f9b4c861c4024bd0 100644 (file)
--- a/simgear/math/sg_geodesy.cxx
+++ b/simgear/math/sg_geodesy.cxx
@@ -9,12 +9,14 @@
 
 #include <simgear/compiler.h>
 
-#ifdef FG_HAVE_STD_INCLUDES
+#ifdef SG_HAVE_STD_INCLUDES
 # include <cmath>
 # include <cerrno>
+# include <cstdio>
 #else
 # include <math.h>
 # include <errno.h>
+# include <stdio.h>
 #endif
 
 #include <simgear/constants.h>
@@ -25,12 +27,12 @@
 #include "localconsts.hxx"
 
 
-#ifndef FG_HAVE_NATIVE_SGI_COMPILERS
-FG_USING_STD(cout);
+#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)
@@ -48,24 +50,41 @@ FG_USING_STD(cout);
 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( ( (FG_PI_2 - lat_geoc) < ONE_SECOND )        // near North pole
-       || ( (FG_PI_2 + lat_geoc) < ONE_SECOND ) )   // near South pole
+    if( ( (SGD_PI_2 - lat_geoc) < SG_ONE_SECOND )        // near North pole
+       || ( (SGD_PI_2 + lat_geoc) < SG_ONE_SECOND ) )   // near South pole
     {
        *lat_geod = lat_geoc;
-       *sea_level_r = EQUATORIAL_RADIUS_M*E;
+       *sea_level_r = SG_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);
+       x_alpha = E*SG_EQUATORIAL_RADIUS_M/sqrt(t_lat*t_lat + E*E);
+#ifdef DOMAIN_ERR_DEBUG
+       if ( errno ) {
+           perror("fgGeocToGeod()");
+           SG_LOG( SG_GENERAL, SG_ALERT, "sqrt(" << t_lat*t_lat + E*E << ")" );
+       }
+#endif
        // cout << "  x_alpha = " << x_alpha << endl;
-       double tmp = RESQ_M - x_alpha * x_alpha;
+       double tmp = sqrt(SG_EQ_RAD_SQUARE_M - x_alpha * x_alpha);
        if ( tmp < 0.0 ) { tmp = 0.0; }
-       mu_alpha = atan2(sqrt(tmp),E*x_alpha);
+#ifdef DOMAIN_ERR_DEBUG
+       if ( errno ) {
+           perror("fgGeocToGeod()");
+           SG_LOG( SG_GENERAL, SG_ALERT, "sqrt(" << SG_EQ_RAD_SQUARE_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;
@@ -73,27 +92,31 @@ void sgGeocToGeod( double lat_geoc, double radius, double
        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)/
+#ifdef DOMAIN_ERR_DEBUG
+       if ( errno ) {
+           perror("fgGeocToGeod()");
+           SG_LOG( SG_GENERAL, SG_ALERT, "sqrt(" <<
+                   1-EPS*EPS*sin_mu_a*sin_mu_a << ")" );
+       }
+#endif
+       rho_alpha = SG_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;
+           sqrt(SG_EQ_RAD_SQUARE_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
+#ifdef DOMAIN_ERR_DEBUG
+       if ( errno ) {
+           perror("fgGeocToGeod()");
+           SG_LOG( SG_GENERAL, SG_ALERT, "sqrt(" <<
+                   SG_EQ_RAD_SQUARE_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)
+                   << ")" );
        }
+#endif
+
     }
 
 }
@@ -116,13 +139,25 @@ void sgGeodToGeoc( double lat_geod, double alt, double *sl_radius,
 {
     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));
+       sqrt(SG_EQ_RAD_SQUARE_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl));
+#ifdef DOMAIN_ERR_DEBUG
+       if ( errno ) {
+           perror("fgGeodToGeoc()");
+           SG_LOG( SG_GENERAL, SG_ALERT, "sqrt(" <<
+                   SG_EQ_RAD_SQUARE_M / (1 + ((1/(E*E))-1)*sin_lambda_sl*sin_lambda_sl)
+                   << ")" );
+       }
+#endif
     py = *sl_radius*sin_lambda_sl + alt*sin_mu;
     px = *sl_radius*cos_lambda_sl + alt*cos_mu;
     *lat_geoc = atan2( py, px );
@@ -142,7 +177,7 @@ void sgGeodToGeoc( double lat_geod, double alt, double *sl_radius,
 //
 // modified for FlightGear to use WGS84 only -- Norman Vine
 
-#define GEOD_INV_PI FG_PI
+#define GEOD_INV_PI SGD_PI
 
 // s == distance
 // az = azimuth