Apply iterative solution to geocentric_to_geodetic as suggestion from

Lothar Gorling.
http://bugzilla.remotesensing.org/show_bug.cgi?id=563


git-svn-id: http://svn.osgeo.org/metacrs/proj/trunk@1182 4e78687f-474d-0410-85f9-8d5e500ac6b2

1 files changed, 210 insertions, 80 deletions

diff --git a/src/geocent.c b/src/geocent.c
index fbe6d75a..5d6a104a 100644
--- a/src/geocent.c
+++ b/src/geocent.c
@@ -65,6 +65,11 @@
  *    25-02-97          Original Code
  *
  * $Log$
+ * Revision 1.4  2004/05/03 16:28:01  warmerda
+ * Apply iterative solution to geocentric_to_geodetic as suggestion from
+ * Lothar Gorling.
+ * http://bugzilla.remotesensing.org/show_bug.cgi?id=563
+ *
  * Revision 1.3  2002/01/08 15:04:08  warmerda
  * The latitude clamping fix from September in Convert_Geodetic_To_Geocentric
  * was botched.  Fixed up now.
@@ -223,13 +228,6 @@ long Convert_Geodetic_To_Geocentric (double Latitude,
   return (Error_Code);
 } /* END OF Convert_Geodetic_To_Geocentric */
 
-void Convert_Geocentric_To_Geodetic (double X,
-                                     double Y, 
-                                     double Z,
-                                     double *Latitude,
-                                     double *Longitude,
-                                     double *Height)
-{ /* BEGIN Convert_Geocentric_To_Geodetic */
 /*
  * The function Convert_Geocentric_To_Geodetic converts geocentric
  * coordinates (X, Y, Z) to geodetic coordinates (latitude, longitude, 
@@ -241,90 +239,222 @@ void Convert_Geocentric_To_Geodetic (double X,
  *    Latitude  : Calculated latitude value in radians.       (output)
  *    Longitude : Calculated longitude value in radians.      (output)
  *    Height    : Calculated height value, in meters.         (output)
- *
+ */
+
+#define USE_ITERATIVE_METHOD
+
+void Convert_Geocentric_To_Geodetic (double X,
+                                     double Y, 
+                                     double Z,
+                                     double *Latitude,
+                                     double *Longitude,
+                                     double *Height)
+{ /* BEGIN Convert_Geocentric_To_Geodetic */
+#if !defined(USE_ITERATIVE_METHOD)
+/*
  * The method used here is derived from 'An Improved Algorithm for
  * Geocentric to Geodetic Coordinate Conversion', by Ralph Toms, Feb 1996
  */
 
 /* Note: Variable names follow the notation used in Toms, Feb 1996 */
 
-  double W;        /* distance from Z axis */
-  double W2;       /* square of distance from Z axis */
-  double T0;       /* initial estimate of vertical component */
-  double T1;       /* corrected estimate of vertical component */
-  double S0;       /* initial estimate of horizontal component */
-  double S1;       /* corrected estimate of horizontal component */
-  double Sin_B0;   /* sin(B0), B0 is estimate of Bowring aux variable */
-  double Sin3_B0;  /* cube of sin(B0) */
-  double Cos_B0;   /* cos(B0) */
-  double Sin_p1;   /* sin(phi1), phi1 is estimated latitude */
-  double Cos_p1;   /* cos(phi1) */
-  double Rn;       /* Earth radius at location */
-  double Sum;      /* numerator of cos(phi1) */
-  int At_Pole;     /* indicates location is in polar region */
-
-  At_Pole = FALSE;
-  if (X != 0.0)
-  {
-    *Longitude = atan2(Y,X);
-  }
-  else
-  {
-    if (Y > 0)
+    double W;        /* distance from Z axis */
+    double W2;       /* square of distance from Z axis */
+    double T0;       /* initial estimate of vertical component */
+    double T1;       /* corrected estimate of vertical component */
+    double S0;       /* initial estimate of horizontal component */
+    double S1;       /* corrected estimate of horizontal component */
+    double Sin_B0;   /* sin(B0), B0 is estimate of Bowring aux variable */
+    double Sin3_B0;  /* cube of sin(B0) */
+    double Cos_B0;   /* cos(B0) */
+    double Sin_p1;   /* sin(phi1), phi1 is estimated latitude */
+    double Cos_p1;   /* cos(phi1) */
+    double Rn;       /* Earth radius at location */
+    double Sum;      /* numerator of cos(phi1) */
+    int At_Pole;     /* indicates location is in polar region */
+
+    At_Pole = FALSE;
+    if (X != 0.0)
+    {
+        *Longitude = atan2(Y,X);
+    }
+    else
     {
-      *Longitude = PI_OVER_2;
+        if (Y > 0)
+        {
+            *Longitude = PI_OVER_2;
+        }
+        else if (Y < 0)
+        {
+            *Longitude = -PI_OVER_2;
+        }
+        else
+        {
+            At_Pole = TRUE;
+            *Longitude = 0.0;
+            if (Z > 0.0)
+            {  /* north pole */
+                *Latitude = PI_OVER_2;
+            }
+            else if (Z < 0.0)
+            {  /* south pole */
+                *Latitude = -PI_OVER_2;
+            }
+            else
+            {  /* center of earth */
+                *Latitude = PI_OVER_2;
+                *Height = -Geocent_b;
+                return;
+            } 
+        }
     }
-    else if (Y < 0)
+    W2 = X*X + Y*Y;
+    W = sqrt(W2);
+    T0 = Z * AD_C;
+    S0 = sqrt(T0 * T0 + W2);
+    Sin_B0 = T0 / S0;
+    Cos_B0 = W / S0;
+    Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
+    T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
+    Sum = W - Geocent_a * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
+    S1 = sqrt(T1*T1 + Sum * Sum);
+    Sin_p1 = T1 / S1;
+    Cos_p1 = Sum / S1;
+    Rn = Geocent_a / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
+    if (Cos_p1 >= COS_67P5)
     {
-      *Longitude = -PI_OVER_2;
+        *Height = W / Cos_p1 - Rn;
+    }
+    else if (Cos_p1 <= -COS_67P5)
+    {
+        *Height = W / -Cos_p1 - Rn;
     }
     else
     {
-      At_Pole = TRUE;
-      *Longitude = 0.0;
-      if (Z > 0.0)
-      {  /* north pole */
-        *Latitude = PI_OVER_2;
-      }
-      else if (Z < 0.0)
-      {  /* south pole */
-        *Latitude = -PI_OVER_2;
-      }
-      else
-      {  /* center of earth */
-        *Latitude = PI_OVER_2;
-        *Height = -Geocent_b;
-        return;
-      } 
+        *Height = Z / Sin_p1 + Rn * (Geocent_e2 - 1.0);
     }
-  }
-  W2 = X*X + Y*Y;
-  W = sqrt(W2);
-  T0 = Z * AD_C;
-  S0 = sqrt(T0 * T0 + W2);
-  Sin_B0 = T0 / S0;
-  Cos_B0 = W / S0;
-  Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
-  T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
-  Sum = W - Geocent_a * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
-  S1 = sqrt(T1*T1 + Sum * Sum);
-  Sin_p1 = T1 / S1;
-  Cos_p1 = Sum / S1;
-  Rn = Geocent_a / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
-  if (Cos_p1 >= COS_67P5)
-  {
-    *Height = W / Cos_p1 - Rn;
-  }
-  else if (Cos_p1 <= -COS_67P5)
-  {
-    *Height = W / -Cos_p1 - Rn;
-  }
-  else
-  {
-    *Height = Z / Sin_p1 + Rn * (Geocent_e2 - 1.0);
-  }
-  if (At_Pole == FALSE)
-  {
-    *Latitude = atan(Sin_p1 / Cos_p1);
-  }
+    if (At_Pole == FALSE)
+    {
+        *Latitude = atan(Sin_p1 / Cos_p1);
+    }
+#else /* defined(USE_ITERATIVE_METHOD) */
+/*
+* Reference...
+* ============
+* Wenzel, H.-G.(1985): Hochauflösende Kugelfunktionsmodelle für
+* das Gravitationspotential der Erde. Wiss. Arb. Univ. Hannover
+* Nr. 137, p. 130-131.
+
+* Programmed by GGA- Leibniz-Institue of Applied Geophysics
+*               Stilleweg 2
+*               D-30655 Hannover
+*               Federal Republic of Germany
+*               Internet: www.gga-hannover.de
+*
+*               Hannover, March 1999, April 2004.
+*               see also: comments in statements
+* remarks:
+* Mathematically exact and because of symmetry of rotation-ellipsoid,
+* each point (X,Y,Z) has at least two solutions (Latitude1,Longitude1,Height1) and
+* (Latitude2,Longitude2,Height2). Is point=(0.,0.,Z) (P=0.), so you get even
+* four solutions,	every two symmetrical to the semi-minor axis.
+* Here Height1 and Height2 have at least a difference in order of
+* radius of curvature (e.g. (0,0,b)=> (90.,0.,0.) or (-90.,0.,-2b);
+* (a+100.)*(sqrt(2.)/2.,sqrt(2.)/2.,0.) => (0.,45.,100.) or
+* (0.,225.,-(2a+100.))).
+* The algorithm always computes (Latitude,Longitude) with smallest |Height|.
+* For normal computations, that means |Height|<10000.m, algorithm normally
+* converges after to 2-3 steps!!!
+* But if |Height| has the amount of length of ellipsoid's axis
+* (e.g. -6300000.m),	algorithm needs about 15 steps.
+*/
+
+/* local defintions and variables */
+/* end-criterium of loop, accuracy of sin(Latitude) */
+#define genau   1.E-12
+#define genau2  (genau*genau)
+#define maxiter 30
+
+    double P;        /* distance between semi-minor axis and location */
+    double RR;       /* distance between center and location */
+    double CT;       /* sin of geocentric latitude */
+    double ST;       /* cos of geocentric latitude */
+    double RX;
+    double RK;
+    double RN;       /* Earth radius at location */
+    double CPHI0;    /* cos of start or old geodetic latitude in iterations */
+    double SPHI0;    /* sin of start or old geodetic latitude in iterations */
+    double CPHI;     /* cos of searched geodetic latitude */
+    double SPHI;     /* sin of searched geodetic latitude */
+    double SDPHI;    /* end-criterium: addition-theorem of sin(Latitude(iter)-Latitude(iter-1)) */
+    int At_Pole;     /* indicates location is in polar region */
+    int iter;        /* # of continous iteration, max. 30 is always enough (s.a.) */
+
+    At_Pole = FALSE;
+    P = sqrt(X*X+Y*Y);
+    RR = sqrt(X*X+Y*Y+Z*Z);
+
+/*	special cases for latitude and longitude */
+    if (P/Geocent_a < genau) {
+
+/*  special case, if P=0. (X=0., Y=0.) */
+        At_Pole = TRUE;
+	*Longitude = 0.;
+
+/*  if (X,Y,Z)=(0.,0.,0.) then Height becomes semi-minor axis
+ *  of ellipsoid (=center of mass), Latitude becomes PI/2 */
+        if (RR/Geocent_a < genau) {
+            *Latitude = PI_OVER_2;
+            *Height   = -Geocent_b;
+            return ;
+
+        }
+    }
+    else {
+/*  ellipsoidal (geodetic) longitude
+ *  interval: -PI < Longitude <= +PI */
+        *Longitude=atan2(Y,X);
+    }
+
+/* --------------------------------------------------------------
+ * Following iterative algorithm was developped by
+ * "Institut für Erdmessung", University of Hannover, July 1988.
+ * Internet: www.ife.uni-hannover.de
+ * Iterative computation of CPHI,SPHI and Height.
+ * Iteration of CPHI and SPHI to 10**-12 radian resp.
+ * 2*10**-7 arcsec.
+ * --------------------------------------------------------------
+ */
+    CT = Z/RR;
+    ST = P/RR;
+    RX = 1.0/sqrt(1.0-Geocent_e2*(2.0-Geocent_e2)*ST*ST);
+    CPHI0 = ST*(1.0-Geocent_e2)*RX;
+    SPHI0 = CT*RX;
+    iter = 0;
+
+/* loop to find sin(Latitude) resp. Latitude
+ * until |sin(Latitude(iter)-Latitude(iter-1))| < genau */
+    do
+    {
+        iter++;
+        RN = Geocent_a/sqrt(1.0-Geocent_e2*SPHI0*SPHI0);
+
+/*  ellipsoidal (geodetic) height */
+        *Height = P*CPHI0+Z*SPHI0-RN*(1.0-Geocent_e2*SPHI0*SPHI0);
+
+        RK = Geocent_e2*RN/(RN+*Height);
+        RX = 1.0/sqrt(1.0-RK*(2.0-RK)*ST*ST);
+        CPHI = ST*(1.0-RK)*RX;
+        SPHI = CT*RX;
+        SDPHI = SPHI*CPHI0-CPHI*SPHI0;
+        CPHI0 = CPHI;
+        SPHI0 = SPHI;
+    }
+    while (SDPHI*SDPHI > genau2 && iter < maxiter);
+
+/*	ellipsoidal (geodetic) latitude */
+    *Latitude=atan(SPHI/fabs(CPHI));
+
+    return;
+#endif /* defined(USE_ITERATIVE_METHOD) */
 } /* END OF Convert_Geocentric_To_Geodetic */