svn-gvsig-desktop / tags / v1_9_Build_1246 / libraries / libjni-proj4 / src / geocent.c
History | View | Annotate | Download (15.5 KB)
1 |
/***************************************************************************/
|
---|---|
2 |
/* RSC IDENTIFIER: GEOCENTRIC
|
3 |
*
|
4 |
* ABSTRACT
|
5 |
*
|
6 |
* This component provides conversions between Geodetic coordinates (latitude,
|
7 |
* longitude in radians and height in meters) and Geocentric coordinates
|
8 |
* (X, Y, Z) in meters.
|
9 |
*
|
10 |
* ERROR HANDLING
|
11 |
*
|
12 |
* This component checks parameters for valid values. If an invalid value
|
13 |
* is found, the error code is combined with the current error code using
|
14 |
* the bitwise or. This combining allows multiple error codes to be
|
15 |
* returned. The possible error codes are:
|
16 |
*
|
17 |
* GEOCENT_NO_ERROR : No errors occurred in function
|
18 |
* GEOCENT_LAT_ERROR : Latitude out of valid range
|
19 |
* (-90 to 90 degrees)
|
20 |
* GEOCENT_LON_ERROR : Longitude out of valid range
|
21 |
* (-180 to 360 degrees)
|
22 |
* GEOCENT_A_ERROR : Semi-major axis lessthan or equal to zero
|
23 |
* GEOCENT_B_ERROR : Semi-minor axis lessthan or equal to zero
|
24 |
* GEOCENT_A_LESS_B_ERROR : Semi-major axis less than semi-minor axis
|
25 |
*
|
26 |
*
|
27 |
* REUSE NOTES
|
28 |
*
|
29 |
* GEOCENTRIC is intended for reuse by any application that performs
|
30 |
* coordinate conversions between geodetic coordinates and geocentric
|
31 |
* coordinates.
|
32 |
*
|
33 |
*
|
34 |
* REFERENCES
|
35 |
*
|
36 |
* An Improved Algorithm for Geocentric to Geodetic Coordinate Conversion,
|
37 |
* Ralph Toms, February 1996 UCRL-JC-123138.
|
38 |
*
|
39 |
* Further information on GEOCENTRIC can be found in the Reuse Manual.
|
40 |
*
|
41 |
* GEOCENTRIC originated from : U.S. Army Topographic Engineering Center
|
42 |
* Geospatial Information Division
|
43 |
* 7701 Telegraph Road
|
44 |
* Alexandria, VA 22310-3864
|
45 |
*
|
46 |
* LICENSES
|
47 |
*
|
48 |
* None apply to this component.
|
49 |
*
|
50 |
* RESTRICTIONS
|
51 |
*
|
52 |
* GEOCENTRIC has no restrictions.
|
53 |
*
|
54 |
* ENVIRONMENT
|
55 |
*
|
56 |
* GEOCENTRIC was tested and certified in the following environments:
|
57 |
*
|
58 |
* 1. Solaris 2.5 with GCC version 2.8.1
|
59 |
* 2. Windows 95 with MS Visual C++ version 6
|
60 |
*
|
61 |
* MODIFICATIONS
|
62 |
*
|
63 |
* Date Description
|
64 |
* ---- -----------
|
65 |
* 25-02-97 Original Code
|
66 |
*
|
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 |
*
|
76 |
* Revision 1.3 2002/01/08 15:04:08 warmerda
|
77 |
* The latitude clamping fix from September in Convert_Geodetic_To_Geocentric
|
78 |
* was botched. Fixed up now.
|
79 |
*
|
80 |
*/
|
81 |
|
82 |
|
83 |
/***************************************************************************/
|
84 |
/*
|
85 |
* INCLUDES
|
86 |
*/
|
87 |
#include <math.h> |
88 |
#include "geocent.h" |
89 |
/*
|
90 |
* math.h - is needed for calls to sin, cos, tan and sqrt.
|
91 |
* geocent.h - is needed for Error codes and prototype error checking.
|
92 |
*/
|
93 |
|
94 |
|
95 |
/***************************************************************************/
|
96 |
/*
|
97 |
* DEFINES
|
98 |
*/
|
99 |
#define PI 3.14159265358979323e0 |
100 |
#define PI_OVER_2 (PI / 2.0e0) |
101 |
#define FALSE 0 |
102 |
#define TRUE 1 |
103 |
#define COS_67P5 0.38268343236508977 /* cosine of 67.5 degrees */ |
104 |
#define AD_C 1.0026000 /* Toms region 1 constant */ |
105 |
|
106 |
|
107 |
/***************************************************************************/
|
108 |
/*
|
109 |
* GLOBAL DECLARATIONS
|
110 |
*/
|
111 |
/* Ellipsoid parameters, default to WGS 84 */
|
112 |
double Geocent_a = 6378137.0; /* Semi-major axis of ellipsoid in meters */ |
113 |
double Geocent_b = 6356752.3142; /* Semi-minor axis of ellipsoid */ |
114 |
|
115 |
double Geocent_a2 = 40680631590769.0; /* Square of semi-major axis */ |
116 |
double Geocent_b2 = 40408299984087.05; /* Square of semi-minor axis */ |
117 |
double Geocent_e2 = 0.0066943799901413800; /* Eccentricity squared */ |
118 |
double Geocent_ep2 = 0.00673949675658690300; /* 2nd eccentricity squared */ |
119 |
/*
|
120 |
* These state variables are for optimization purposes. The only function
|
121 |
* that should modify them is Set_Geocentric_Parameters.
|
122 |
*/
|
123 |
|
124 |
|
125 |
/***************************************************************************/
|
126 |
/*
|
127 |
* FUNCTIONS
|
128 |
*/
|
129 |
|
130 |
|
131 |
long pj_Set_Geocentric_Parameters (double a, double b) |
132 |
|
133 |
{ /* BEGIN Set_Geocentric_Parameters */
|
134 |
/*
|
135 |
* The function Set_Geocentric_Parameters receives the ellipsoid parameters
|
136 |
* as inputs and sets the corresponding state variables.
|
137 |
*
|
138 |
* a : Semi-major axis, in meters. (input)
|
139 |
* b : Semi-minor axis, in meters. (input)
|
140 |
*/
|
141 |
long Error_Code = GEOCENT_NO_ERROR;
|
142 |
|
143 |
if (a <= 0.0) |
144 |
Error_Code |= GEOCENT_A_ERROR; |
145 |
if (b <= 0.0) |
146 |
Error_Code |= GEOCENT_B_ERROR; |
147 |
if (a < b)
|
148 |
Error_Code |= GEOCENT_A_LESS_B_ERROR; |
149 |
if (!Error_Code)
|
150 |
{ |
151 |
Geocent_a = a; |
152 |
Geocent_b = b; |
153 |
Geocent_a2 = a * a; |
154 |
Geocent_b2 = b * b; |
155 |
Geocent_e2 = (Geocent_a2 - Geocent_b2) / Geocent_a2; |
156 |
Geocent_ep2 = (Geocent_a2 - Geocent_b2) / Geocent_b2; |
157 |
} |
158 |
return (Error_Code);
|
159 |
} /* END OF Set_Geocentric_Parameters */
|
160 |
|
161 |
|
162 |
void pj_Get_Geocentric_Parameters (double *a, |
163 |
double *b)
|
164 |
{ /* BEGIN Get_Geocentric_Parameters */
|
165 |
/*
|
166 |
* The function Get_Geocentric_Parameters returns the ellipsoid parameters
|
167 |
* to be used in geocentric coordinate conversions.
|
168 |
*
|
169 |
* a : Semi-major axis, in meters. (output)
|
170 |
* b : Semi-minor axis, in meters. (output)
|
171 |
*/
|
172 |
|
173 |
*a = Geocent_a; |
174 |
*b = Geocent_b; |
175 |
} /* END OF Get_Geocentric_Parameters */
|
176 |
|
177 |
|
178 |
long pj_Convert_Geodetic_To_Geocentric (double Latitude, |
179 |
double Longitude,
|
180 |
double Height,
|
181 |
double *X,
|
182 |
double *Y,
|
183 |
double *Z)
|
184 |
{ /* BEGIN Convert_Geodetic_To_Geocentric */
|
185 |
/*
|
186 |
* The function Convert_Geodetic_To_Geocentric converts geodetic coordinates
|
187 |
* (latitude, longitude, and height) to geocentric coordinates (X, Y, Z),
|
188 |
* according to the current ellipsoid parameters.
|
189 |
*
|
190 |
* Latitude : Geodetic latitude in radians (input)
|
191 |
* Longitude : Geodetic longitude in radians (input)
|
192 |
* Height : Geodetic height, in meters (input)
|
193 |
* X : Calculated Geocentric X coordinate, in meters (output)
|
194 |
* Y : Calculated Geocentric Y coordinate, in meters (output)
|
195 |
* Z : Calculated Geocentric Z coordinate, in meters (output)
|
196 |
*
|
197 |
*/
|
198 |
long Error_Code = GEOCENT_NO_ERROR;
|
199 |
double Rn; /* Earth radius at location */ |
200 |
double Sin_Lat; /* sin(Latitude) */ |
201 |
double Sin2_Lat; /* Square of sin(Latitude) */ |
202 |
double Cos_Lat; /* cos(Latitude) */ |
203 |
|
204 |
/*
|
205 |
** Don't blow up if Latitude is just a little out of the value
|
206 |
** range as it may just be a rounding issue. Also removed longitude
|
207 |
** test, it should be wrapped by cos() and sin(). NFW for PROJ.4, Sep/2001.
|
208 |
*/
|
209 |
if( Latitude < -PI_OVER_2 && Latitude > -1.001 * PI_OVER_2 ) |
210 |
Latitude = -PI_OVER_2; |
211 |
else if( Latitude > PI_OVER_2 && Latitude < 1.001 * PI_OVER_2 ) |
212 |
Latitude = PI_OVER_2; |
213 |
else if ((Latitude < -PI_OVER_2) || (Latitude > PI_OVER_2)) |
214 |
{ /* Latitude out of range */
|
215 |
Error_Code |= GEOCENT_LAT_ERROR; |
216 |
} |
217 |
|
218 |
if (!Error_Code)
|
219 |
{ /* no errors */
|
220 |
if (Longitude > PI)
|
221 |
Longitude -= (2*PI);
|
222 |
Sin_Lat = sin(Latitude); |
223 |
Cos_Lat = cos(Latitude); |
224 |
Sin2_Lat = Sin_Lat * Sin_Lat; |
225 |
Rn = Geocent_a / (sqrt(1.0e0 - Geocent_e2 * Sin2_Lat)); |
226 |
*X = (Rn + Height) * Cos_Lat * cos(Longitude); |
227 |
*Y = (Rn + Height) * Cos_Lat * sin(Longitude); |
228 |
*Z = ((Rn * (1 - Geocent_e2)) + Height) * Sin_Lat;
|
229 |
|
230 |
} |
231 |
return (Error_Code);
|
232 |
} /* END OF Convert_Geodetic_To_Geocentric */
|
233 |
|
234 |
/*
|
235 |
* The function Convert_Geocentric_To_Geodetic converts geocentric
|
236 |
* coordinates (X, Y, Z) to geodetic coordinates (latitude, longitude,
|
237 |
* and height), according to the current ellipsoid parameters.
|
238 |
*
|
239 |
* X : Geocentric X coordinate, in meters. (input)
|
240 |
* Y : Geocentric Y coordinate, in meters. (input)
|
241 |
* Z : Geocentric Z coordinate, in meters. (input)
|
242 |
* Latitude : Calculated latitude value in radians. (output)
|
243 |
* Longitude : Calculated longitude value in radians. (output)
|
244 |
* Height : Calculated height value, in meters. (output)
|
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 |
/*
|
258 |
* The method used here is derived from 'An Improved Algorithm for
|
259 |
* Geocentric to Geodetic Coordinate Conversion', by Ralph Toms, Feb 1996
|
260 |
*/
|
261 |
|
262 |
/* Note: Variable names follow the notation used in Toms, Feb 1996 */
|
263 |
|
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) */ |
463 |
} /* END OF Convert_Geocentric_To_Geodetic */
|