00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079 #include <math.h>
00080 #include <float.h>
00081 #include <stdlib.h>
00082 #include "Geocentric.h"
00083 #include "CartesianCoordinates.h"
00084 #include "GeodeticCoordinates.h"
00085 #include "CoordinateConversionException.h"
00086 #include "ErrorMessages.h"
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098 using namespace MSP::CCS;
00099
00100
00101
00102
00103
00104
00105
00106 const double PI = 3.14159265358979323e0;
00107 const double PI_OVER_2 = (PI / 2.0e0);
00108 const int FALSE = 0;
00109 const int TRUE = 1;
00110 const double COS_67P5 = 0.38268343236508977;
00111 const double AD_C = 1.0026000;
00112
00113
00114
00115
00116
00117
00118
00119 Geocentric::Geocentric(
00120 double ellipsoidSemiMajorAxis,
00121 double ellipsoidFlattening ) :
00122 CoordinateSystem(),
00123 Geocent_e2( 0.0066943799901413800 ),
00124 Geocent_ep2( 0.00673949675658690300 )
00125 {
00126
00127
00128
00129
00130
00131
00132
00133
00134 double inv_f = 1 / ellipsoidFlattening;
00135 char errorStatus[500] = "";
00136
00137 if (ellipsoidSemiMajorAxis <= 0.0)
00138 strcat( errorStatus, MSP::CCS::ErrorMessages::semiMajorAxis );
00139 if ((inv_f < 250) || (inv_f > 350))
00140 {
00141 strcat( errorStatus, MSP::CCS::ErrorMessages::ellipsoidFlattening );
00142 }
00143
00144 if( strlen( errorStatus ) > 0)
00145 throw CoordinateConversionException( errorStatus );
00146
00147 semiMajorAxis = ellipsoidSemiMajorAxis;
00148 flattening = ellipsoidFlattening;
00149
00150 Geocent_e2 = 2 * flattening - flattening * flattening;
00151 Geocent_ep2 = (1 / (1 - Geocent_e2)) - 1;
00152
00153
00154 Geocent_algorithm = UNDEFINED;
00155 }
00156
00157
00158 Geocentric::Geocentric( const Geocentric &g )
00159 {
00160 semiMajorAxis = g.semiMajorAxis;
00161 flattening = g.flattening;
00162 Geocent_e2 = g.Geocent_e2;
00163 Geocent_ep2 = g.Geocent_ep2;
00164 }
00165
00166
00167 Geocentric::~Geocentric()
00168 {
00169 }
00170
00171
00172 Geocentric& Geocentric::operator=( const Geocentric &g )
00173 {
00174 if( this != &g )
00175 {
00176 semiMajorAxis = g.semiMajorAxis;
00177 flattening = g.flattening;
00178 Geocent_e2 = g.Geocent_e2;
00179 Geocent_ep2 = g.Geocent_ep2;
00180 }
00181
00182 return *this;
00183 }
00184
00185
00186 MSP::CCS::CartesianCoordinates* Geocentric::convertFromGeodetic( const MSP::CCS::GeodeticCoordinates* geodeticCoordinates )
00187 {
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202 double Rn;
00203 double Sin_Lat;
00204 double Sin2_Lat;
00205 double Cos_Lat;
00206 char errorStatus[50] = "";
00207
00208 double longitude = geodeticCoordinates->longitude();
00209 double latitude = geodeticCoordinates->latitude();
00210 double height = geodeticCoordinates->height();
00211
00212 if ((latitude < -PI_OVER_2) || (latitude > PI_OVER_2))
00213 {
00214 strcat( errorStatus, MSP::CCS::ErrorMessages::latitude );
00215 }
00216 if ((longitude < -PI) || (longitude > (2*PI)))
00217 {
00218 strcat( errorStatus, MSP::CCS::ErrorMessages::longitude );
00219 }
00220
00221 if( strlen( errorStatus ) > 0)
00222 throw CoordinateConversionException( errorStatus );
00223
00224 if (longitude > PI)
00225 longitude -= (2*PI);
00226 Sin_Lat = sin(latitude);
00227 Cos_Lat = cos(latitude);
00228 Sin2_Lat = Sin_Lat * Sin_Lat;
00229 Rn = semiMajorAxis / (sqrt(1.0e0 - Geocent_e2 * Sin2_Lat));
00230 double X = (Rn + height) * Cos_Lat * cos(longitude);
00231 double Y = (Rn + height) * Cos_Lat * sin(longitude);
00232 double Z = ((Rn * (1 - Geocent_e2)) + height) * Sin_Lat;
00233
00234 return new CartesianCoordinates( CoordinateType::geocentric, X, Y, Z );
00235 }
00236
00237
00238 MSP::CCS::GeodeticCoordinates* Geocentric::convertToGeodetic(
00239 MSP::CCS::CartesianCoordinates* cartesianCoordinates )
00240 {
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259 double X = cartesianCoordinates->x();
00260 double Y = cartesianCoordinates->y();
00261 double Z = cartesianCoordinates->z();
00262 double latitude, longitude, height;
00263
00264 if( Geocent_algorithm == UNDEFINED )
00265 {
00266 Geocent_algorithm = ITERATIVE;
00267 char *geotransConv = getenv( "MSPCCS_USE_LEGACY_GEOTRANS" );
00268 if( geotransConv != NULL )
00269 {
00270 Geocent_algorithm = GEOTRANS;
00271 }
00272 }
00273
00274 if( Geocent_algorithm == ITERATIVE )
00275 {
00276 geocentricToGeodetic( X, Y, Z, latitude, longitude, height );
00277 }
00278 else
00279 {
00280 double W;
00281 double W2;
00282 double T0;
00283 double T1;
00284 double S0;
00285 double S1;
00286 double Sin_B0;
00287 double Sin3_B0;
00288 double Cos_B0;
00289 double Sin_p1;
00290 double Cos_p1;
00291 double Rn;
00292 double Sum;
00293 int At_Pole;
00294 double Geocent_b = semiMajorAxis * (1 - flattening);
00295
00296 At_Pole = FALSE;
00297 if (X != 0.0)
00298 {
00299 longitude = atan2(Y,X);
00300 }
00301 else
00302 {
00303 if (Y > 0)
00304 {
00305 longitude = PI_OVER_2;
00306 }
00307 else if (Y < 0)
00308 {
00309 longitude = -PI_OVER_2;
00310 }
00311 else
00312 {
00313 At_Pole = TRUE;
00314 longitude = 0.0;
00315 if (Z > 0.0)
00316 {
00317 latitude = PI_OVER_2;
00318 }
00319 else if (Z < 0.0)
00320 {
00321 latitude = -PI_OVER_2;
00322 }
00323 else
00324 {
00325 latitude = PI_OVER_2;
00326 height = -Geocent_b;
00327 return new GeodeticCoordinates(
00328 CoordinateType::geodetic, longitude, latitude, height );
00329 }
00330 }
00331 }
00332 W2 = X*X + Y*Y;
00333 W = sqrt(W2);
00334 T0 = Z * AD_C;
00335 S0 = sqrt(T0 * T0 + W2);
00336 Sin_B0 = T0 / S0;
00337 Cos_B0 = W / S0;
00338 Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
00339 T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
00340 Sum = W - semiMajorAxis * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
00341 S1 = sqrt(T1*T1 + Sum * Sum);
00342 Sin_p1 = T1 / S1;
00343 Cos_p1 = Sum / S1;
00344 Rn = semiMajorAxis / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
00345 if (Cos_p1 >= COS_67P5)
00346 {
00347 height = W / Cos_p1 - Rn;
00348 }
00349 else if (Cos_p1 <= -COS_67P5)
00350 {
00351 height = W / -Cos_p1 - Rn;
00352 }
00353 else
00354 {
00355 height = Z / Sin_p1 + Rn * (Geocent_e2 - 1.0);
00356 }
00357 if (At_Pole == FALSE)
00358 {
00359 latitude = atan(Sin_p1 / Cos_p1);
00360 }
00361 }
00362
00363 return new GeodeticCoordinates(
00364 CoordinateType::geodetic, longitude, latitude, height );
00365 }
00366
00367 void Geocentric::geocentricToGeodetic(
00368 const double x,
00369 const double y,
00370 const double z,
00371 double &lat,
00372 double &lon,
00373 double &ht )
00374 {
00375 double equatorial_radius = semiMajorAxis;
00376 double eccentricity_squared = Geocent_e2;
00377
00378 double rho, c, s, ct2, e1, e2a;
00379
00380 e1 = 1.0 - eccentricity_squared;
00381 e2a = eccentricity_squared * equatorial_radius;
00382
00383 rho = sqrt(x * x + y * y);
00384
00385 if (z == 0.0)
00386 {
00387 if (rho < e2a)
00388 {
00389 ct2 = rho*rho*e1/(e2a*e2a-rho*rho);
00390 c = sqrt(ct2 / (1.0 + ct2));
00391 s = sqrt(1.0 / (1.0 + ct2));
00392 }
00393 else
00394 {
00395 c = 1.0;
00396 s = 0.0;
00397 }
00398
00399 lat = 0.0;
00400 }
00401 else
00402 {
00403 double ct, new_ct, zabs;
00404 double f, new_f, df_dct, e2;
00405
00406 zabs = fabs(z);
00407
00408 new_ct = rho / zabs;
00409 new_f = DBL_MAX;
00410
00411 do
00412 {
00413 ct = new_ct;
00414 f = new_f;
00415
00416 e2 = sqrt(e1 + ct*ct);
00417
00418 new_f = rho - zabs*ct - e2a*ct/e2;
00419
00420 if (new_f == 0.0) break;
00421
00422 df_dct = -zabs - (e2a*e1)/(e2*e2*e2);
00423
00424 new_ct = ct - new_f / df_dct;
00425
00426 if (new_ct < 0.0) new_ct = 0.0;
00427 }
00428 while (fabs(new_f) < fabs(f));
00429
00430 s = 1.0 / sqrt(1.0 + ct * ct);
00431 c = ct * s;
00432
00433 if (z < 0.0)
00434 {
00435 s = -s;
00436 lat = -atan(1.0 / ct);
00437 } else
00438 {
00439 lat = atan(1.0 / ct);
00440 }
00441 }
00442
00443 lon = atan2(y, x);
00444
00445 ht = rho*c + z*s - equatorial_radius*sqrt(1.0 - eccentricity_squared*s*s);
00446
00447 return;
00448 }
00449
00450
00451