← Back to branch summary

~ubuntu-branches/ubuntu/breezy/proj/breezy

« back to all changes in this revision

Viewing changes to src/geocent.c

  • Committer: Bazaar Package Importer
  • Author(s): Peter S Galbraith
  • Date: 2004-11-06 19:44:53 UTC
  • mto: This revision was merged to the branch mainline in revision 3.
  • Revision ID: james.westby@ubuntu.com-20041106194453-axnsmkh1zplal8mz
Tags: upstream-4.4.9
Import upstream version 4.4.9

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/geocent.c
65
65
 *    25-02-97          Original Code
66
66
 *
67
67
 * $Log: geocent.c,v $
 
68
 * Revision 1.5  2004/10/25 15:34:36  fwarmerdam
 
69
 * make names of geodetic funcs from geotrans unique
 
70
 *
 
71
 * Revision 1.4  2004/05/03 16:28:01  warmerda
 
72
 * Apply iterative solution to geocentric_to_geodetic as suggestion from
 
73
 * Lothar Gorling.
 
74
 * http://bugzilla.remotesensing.org/show_bug.cgi?id=563
 
75
 *
68
76
 * Revision 1.3  2002/01/08 15:04:08  warmerda
69
77
 * The latitude clamping fix from September in Convert_Geodetic_To_Geocentric
70
78
 * was botched.  Fixed up now.
120
128
 */
121
129
 
122
130
 
123
 
long Set_Geocentric_Parameters (double a, 
124
 
                                double b) 
 
131
long pj_Set_Geocentric_Parameters (double a, double b) 
 
132
 
125
133
{ /* BEGIN Set_Geocentric_Parameters */
126
134
/*
127
135
 * The function Set_Geocentric_Parameters receives the ellipsoid parameters
151
159
} /* END OF Set_Geocentric_Parameters */
152
160
 
153
161
 
154
 
void Get_Geocentric_Parameters (double *a, 
 
162
void pj_Get_Geocentric_Parameters (double *a, 
155
163
                                double *b)
156
164
{ /* BEGIN Get_Geocentric_Parameters */
157
165
/*
167
175
} /* END OF Get_Geocentric_Parameters */
168
176
 
169
177
 
170
 
long Convert_Geodetic_To_Geocentric (double Latitude,
 
178
long pj_Convert_Geodetic_To_Geocentric (double Latitude,
171
179
                                     double Longitude,
172
180
                                     double Height,
173
181
                                     double *X,
223
231
  return (Error_Code);
224
232
} /* END OF Convert_Geodetic_To_Geocentric */
225
233
 
226
 
void Convert_Geocentric_To_Geodetic (double X,
227
 
                                     double Y, 
228
 
                                     double Z,
229
 
                                     double *Latitude,
230
 
                                     double *Longitude,
231
 
                                     double *Height)
232
 
{ /* BEGIN Convert_Geocentric_To_Geodetic */
233
234
/*
234
235
 * The function Convert_Geocentric_To_Geodetic converts geocentric
235
236
 * coordinates (X, Y, Z) to geodetic coordinates (latitude, longitude, 
241
242
 *    Latitude  : Calculated latitude value in radians.       (output)
242
243
 *    Longitude : Calculated longitude value in radians.      (output)
243
244
 *    Height    : Calculated height value, in meters.         (output)
244
 
 *
 
245
 */
 
246
 
 
247
#define USE_ITERATIVE_METHOD
 
248
 
 
249
void pj_Convert_Geocentric_To_Geodetic (double X,
 
250
                                     double Y, 
 
251
                                     double Z,
 
252
                                     double *Latitude,
 
253
                                     double *Longitude,
 
254
                                     double *Height)
 
255
{ /* BEGIN Convert_Geocentric_To_Geodetic */
 
256
#if !defined(USE_ITERATIVE_METHOD)
 
257
/*
245
258
 * The method used here is derived from 'An Improved Algorithm for
246
259
 * Geocentric to Geodetic Coordinate Conversion', by Ralph Toms, Feb 1996
247
260
 */
248
261
 
249
262
/* Note: Variable names follow the notation used in Toms, Feb 1996 */
250
263
 
251
 
  double W;        /* distance from Z axis */
252
 
  double W2;       /* square of distance from Z axis */
253
 
  double T0;       /* initial estimate of vertical component */
254
 
  double T1;       /* corrected estimate of vertical component */
255
 
  double S0;       /* initial estimate of horizontal component */
256
 
  double S1;       /* corrected estimate of horizontal component */
257
 
  double Sin_B0;   /* sin(B0), B0 is estimate of Bowring aux variable */
258
 
  double Sin3_B0;  /* cube of sin(B0) */
259
 
  double Cos_B0;   /* cos(B0) */
260
 
  double Sin_p1;   /* sin(phi1), phi1 is estimated latitude */
261
 
  double Cos_p1;   /* cos(phi1) */
262
 
  double Rn;       /* Earth radius at location */
263
 
  double Sum;      /* numerator of cos(phi1) */
264
 
  int At_Pole;     /* indicates location is in polar region */
265
 
 
266
 
  At_Pole = FALSE;
267
 
  if (X != 0.0)
268
 
  {
269
 
    *Longitude = atan2(Y,X);
270
 
  }
271
 
  else
272
 
  {
273
 
    if (Y > 0)
274
 
    {
275
 
      *Longitude = PI_OVER_2;
276
 
    }
277
 
    else if (Y < 0)
278
 
    {
279
 
      *Longitude = -PI_OVER_2;
280
 
    }
281
 
    else
282
 
    {
283
 
      At_Pole = TRUE;
284
 
      *Longitude = 0.0;
285
 
      if (Z > 0.0)
286
 
      {  /* north pole */
287
 
        *Latitude = PI_OVER_2;
288
 
      }
289
 
      else if (Z < 0.0)
290
 
      {  /* south pole */
291
 
        *Latitude = -PI_OVER_2;
292
 
      }
293
 
      else
294
 
      {  /* center of earth */
295
 
        *Latitude = PI_OVER_2;
296
 
        *Height = -Geocent_b;
297
 
        return;
298
 
      } 
299
 
    }
300
 
  }
301
 
  W2 = X*X + Y*Y;
302
 
  W = sqrt(W2);
303
 
  T0 = Z * AD_C;
304
 
  S0 = sqrt(T0 * T0 + W2);
305
 
  Sin_B0 = T0 / S0;
306
 
  Cos_B0 = W / S0;
307
 
  Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
308
 
  T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
309
 
  Sum = W - Geocent_a * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
310
 
  S1 = sqrt(T1*T1 + Sum * Sum);
311
 
  Sin_p1 = T1 / S1;
312
 
  Cos_p1 = Sum / S1;
313
 
  Rn = Geocent_a / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
314
 
  if (Cos_p1 >= COS_67P5)
315
 
  {
316
 
    *Height = W / Cos_p1 - Rn;
317
 
  }
318
 
  else if (Cos_p1 <= -COS_67P5)
319
 
  {
320
 
    *Height = W / -Cos_p1 - Rn;
321
 
  }
322
 
  else
323
 
  {
324
 
    *Height = Z / Sin_p1 + Rn * (Geocent_e2 - 1.0);
325
 
  }
326
 
  if (At_Pole == FALSE)
327
 
  {
328
 
    *Latitude = atan(Sin_p1 / Cos_p1);
329
 
  }
 
264
    double W;        /* distance from Z axis */
 
265
    double W2;       /* square of distance from Z axis */
 
266
    double T0;       /* initial estimate of vertical component */
 
267
    double T1;       /* corrected estimate of vertical component */
 
268
    double S0;       /* initial estimate of horizontal component */
 
269
    double S1;       /* corrected estimate of horizontal component */
 
270
    double Sin_B0;   /* sin(B0), B0 is estimate of Bowring aux variable */
 
271
    double Sin3_B0;  /* cube of sin(B0) */
 
272
    double Cos_B0;   /* cos(B0) */
 
273
    double Sin_p1;   /* sin(phi1), phi1 is estimated latitude */
 
274
    double Cos_p1;   /* cos(phi1) */
 
275
    double Rn;       /* Earth radius at location */
 
276
    double Sum;      /* numerator of cos(phi1) */
 
277
    int At_Pole;     /* indicates location is in polar region */
 
278
 
 
279
    At_Pole = FALSE;
 
280
    if (X != 0.0)
 
281
    {
 
282
        *Longitude = atan2(Y,X);
 
283
    }
 
284
    else
 
285
    {
 
286
        if (Y > 0)
 
287
        {
 
288
            *Longitude = PI_OVER_2;
 
289
        }
 
290
        else if (Y < 0)
 
291
        {
 
292
            *Longitude = -PI_OVER_2;
 
293
        }
 
294
        else
 
295
        {
 
296
            At_Pole = TRUE;
 
297
            *Longitude = 0.0;
 
298
            if (Z > 0.0)
 
299
            {  /* north pole */
 
300
                *Latitude = PI_OVER_2;
 
301
            }
 
302
            else if (Z < 0.0)
 
303
            {  /* south pole */
 
304
                *Latitude = -PI_OVER_2;
 
305
            }
 
306
            else
 
307
            {  /* center of earth */
 
308
                *Latitude = PI_OVER_2;
 
309
                *Height = -Geocent_b;
 
310
                return;
 
311
            } 
 
312
        }
 
313
    }
 
314
    W2 = X*X + Y*Y;
 
315
    W = sqrt(W2);
 
316
    T0 = Z * AD_C;
 
317
    S0 = sqrt(T0 * T0 + W2);
 
318
    Sin_B0 = T0 / S0;
 
319
    Cos_B0 = W / S0;
 
320
    Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
 
321
    T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
 
322
    Sum = W - Geocent_a * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
 
323
    S1 = sqrt(T1*T1 + Sum * Sum);
 
324
    Sin_p1 = T1 / S1;
 
325
    Cos_p1 = Sum / S1;
 
326
    Rn = Geocent_a / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
 
327
    if (Cos_p1 >= COS_67P5)
 
328
    {
 
329
        *Height = W / Cos_p1 - Rn;
 
330
    }
 
331
    else if (Cos_p1 <= -COS_67P5)
 
332
    {
 
333
        *Height = W / -Cos_p1 - Rn;
 
334
    }
 
335
    else
 
336
    {
 
337
        *Height = Z / Sin_p1 + Rn * (Geocent_e2 - 1.0);
 
338
    }
 
339
    if (At_Pole == FALSE)
 
340
    {
 
341
        *Latitude = atan(Sin_p1 / Cos_p1);
 
342
    }
 
343
#else /* defined(USE_ITERATIVE_METHOD) */
 
344
/*
 
345
* Reference...
 
346
* ============
 
347
* Wenzel, H.-G.(1985): Hochauflösende Kugelfunktionsmodelle für
 
348
* das Gravitationspotential der Erde. Wiss. Arb. Univ. Hannover
 
349
* Nr. 137, p. 130-131.
 
350
 
 
351
* Programmed by GGA- Leibniz-Institue of Applied Geophysics
 
352
*               Stilleweg 2
 
353
*               D-30655 Hannover
 
354
*               Federal Republic of Germany
 
355
*               Internet: www.gga-hannover.de
 
356
*
 
357
*               Hannover, March 1999, April 2004.
 
358
*               see also: comments in statements
 
359
* remarks:
 
360
* Mathematically exact and because of symmetry of rotation-ellipsoid,
 
361
* each point (X,Y,Z) has at least two solutions (Latitude1,Longitude1,Height1) and
 
362
* (Latitude2,Longitude2,Height2). Is point=(0.,0.,Z) (P=0.), so you get even
 
363
* four solutions,       every two symmetrical to the semi-minor axis.
 
364
* Here Height1 and Height2 have at least a difference in order of
 
365
* radius of curvature (e.g. (0,0,b)=> (90.,0.,0.) or (-90.,0.,-2b);
 
366
* (a+100.)*(sqrt(2.)/2.,sqrt(2.)/2.,0.) => (0.,45.,100.) or
 
367
* (0.,225.,-(2a+100.))).
 
368
* The algorithm always computes (Latitude,Longitude) with smallest |Height|.
 
369
* For normal computations, that means |Height|<10000.m, algorithm normally
 
370
* converges after to 2-3 steps!!!
 
371
* But if |Height| has the amount of length of ellipsoid's axis
 
372
* (e.g. -6300000.m),    algorithm needs about 15 steps.
 
373
*/
 
374
 
 
375
/* local defintions and variables */
 
376
/* end-criterium of loop, accuracy of sin(Latitude) */
 
377
#define genau   1.E-12
 
378
#define genau2  (genau*genau)
 
379
#define maxiter 30
 
380
 
 
381
    double P;        /* distance between semi-minor axis and location */
 
382
    double RR;       /* distance between center and location */
 
383
    double CT;       /* sin of geocentric latitude */
 
384
    double ST;       /* cos of geocentric latitude */
 
385
    double RX;
 
386
    double RK;
 
387
    double RN;       /* Earth radius at location */
 
388
    double CPHI0;    /* cos of start or old geodetic latitude in iterations */
 
389
    double SPHI0;    /* sin of start or old geodetic latitude in iterations */
 
390
    double CPHI;     /* cos of searched geodetic latitude */
 
391
    double SPHI;     /* sin of searched geodetic latitude */
 
392
    double SDPHI;    /* end-criterium: addition-theorem of sin(Latitude(iter)-Latitude(iter-1)) */
 
393
    int At_Pole;     /* indicates location is in polar region */
 
394
    int iter;        /* # of continous iteration, max. 30 is always enough (s.a.) */
 
395
 
 
396
    At_Pole = FALSE;
 
397
    P = sqrt(X*X+Y*Y);
 
398
    RR = sqrt(X*X+Y*Y+Z*Z);
 
399
 
 
400
/*      special cases for latitude and longitude */
 
401
    if (P/Geocent_a < genau) {
 
402
 
 
403
/*  special case, if P=0. (X=0., Y=0.) */
 
404
        At_Pole = TRUE;
 
405
        *Longitude = 0.;
 
406
 
 
407
/*  if (X,Y,Z)=(0.,0.,0.) then Height becomes semi-minor axis
 
408
 *  of ellipsoid (=center of mass), Latitude becomes PI/2 */
 
409
        if (RR/Geocent_a < genau) {
 
410
            *Latitude = PI_OVER_2;
 
411
            *Height   = -Geocent_b;
 
412
            return ;
 
413
 
 
414
        }
 
415
    }
 
416
    else {
 
417
/*  ellipsoidal (geodetic) longitude
 
418
 *  interval: -PI < Longitude <= +PI */
 
419
        *Longitude=atan2(Y,X);
 
420
    }
 
421
 
 
422
/* --------------------------------------------------------------
 
423
 * Following iterative algorithm was developped by
 
424
 * "Institut für Erdmessung", University of Hannover, July 1988.
 
425
 * Internet: www.ife.uni-hannover.de
 
426
 * Iterative computation of CPHI,SPHI and Height.
 
427
 * Iteration of CPHI and SPHI to 10**-12 radian resp.
 
428
 * 2*10**-7 arcsec.
 
429
 * --------------------------------------------------------------
 
430
 */
 
431
    CT = Z/RR;
 
432
    ST = P/RR;
 
433
    RX = 1.0/sqrt(1.0-Geocent_e2*(2.0-Geocent_e2)*ST*ST);
 
434
    CPHI0 = ST*(1.0-Geocent_e2)*RX;
 
435
    SPHI0 = CT*RX;
 
436
    iter = 0;
 
437
 
 
438
/* loop to find sin(Latitude) resp. Latitude
 
439
 * until |sin(Latitude(iter)-Latitude(iter-1))| < genau */
 
440
    do
 
441
    {
 
442
        iter++;
 
443
        RN = Geocent_a/sqrt(1.0-Geocent_e2*SPHI0*SPHI0);
 
444
 
 
445
/*  ellipsoidal (geodetic) height */
 
446
        *Height = P*CPHI0+Z*SPHI0-RN*(1.0-Geocent_e2*SPHI0*SPHI0);
 
447
 
 
448
        RK = Geocent_e2*RN/(RN+*Height);
 
449
        RX = 1.0/sqrt(1.0-RK*(2.0-RK)*ST*ST);
 
450
        CPHI = ST*(1.0-RK)*RX;
 
451
        SPHI = CT*RX;
 
452
        SDPHI = SPHI*CPHI0-CPHI*SPHI0;
 
453
        CPHI0 = CPHI;
 
454
        SPHI0 = SPHI;
 
455
    }
 
456
    while (SDPHI*SDPHI > genau2 && iter < maxiter);
 
457
 
 
458
/*      ellipsoidal (geodetic) latitude */
 
459
    *Latitude=atan(SPHI/fabs(CPHI));
 
460
 
 
461
    return;
 
462
#endif /* defined(USE_ITERATIVE_METHOD) */
330
463
} /* END OF Convert_Geocentric_To_Geodetic */