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 #include <math.h>
00079 #include "Geocentric.h"
00080 #include "CartesianCoordinates.h"
00081 #include "GeodeticCoordinates.h"
00082 #include "CoordinateConversionException.h"
00083 #include "ErrorMessages.h"
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095 using namespace MSP::CCS;
00096
00097
00098
00099
00100
00101
00102
00103 const double PI = 3.14159265358979323e0;
00104 const double PI_OVER_2 = (PI / 2.0e0);
00105 const int FALSE = 0;
00106 const int TRUE = 1;
00107 const double COS_67P5 = 0.38268343236508977;
00108 const double AD_C = 1.0026000;
00109
00110
00111
00112
00113
00114
00115
00116 Geocentric::Geocentric( double ellipsoidSemiMajorAxis, double ellipsoidFlattening ) :
00117 CoordinateSystem(),
00118 Geocent_e2( 0.0066943799901413800 ),
00119 Geocent_ep2( 0.00673949675658690300 )
00120 {
00121
00122
00123
00124
00125
00126
00127
00128
00129 double inv_f = 1 / ellipsoidFlattening;
00130 char errorStatus[500] = "";
00131
00132 if (ellipsoidSemiMajorAxis <= 0.0)
00133 strcat( errorStatus, MSP::CCS::ErrorMessages::semiMajorAxis );
00134 if ((inv_f < 250) || (inv_f > 350))
00135 {
00136 strcat( errorStatus, MSP::CCS::ErrorMessages::ellipsoidFlattening );
00137 }
00138
00139 if( strlen( errorStatus ) > 0)
00140 throw CoordinateConversionException( errorStatus );
00141
00142 semiMajorAxis = ellipsoidSemiMajorAxis;
00143 flattening = ellipsoidFlattening;
00144
00145 Geocent_e2 = 2 * flattening - flattening * flattening;
00146 Geocent_ep2 = (1 / (1 - Geocent_e2)) - 1;
00147 }
00148
00149
00150 Geocentric::Geocentric( const Geocentric &g )
00151 {
00152 semiMajorAxis = g.semiMajorAxis;
00153 flattening = g.flattening;
00154 Geocent_e2 = g.Geocent_e2;
00155 Geocent_ep2 = g.Geocent_ep2;
00156 }
00157
00158
00159 Geocentric::~Geocentric()
00160 {
00161 }
00162
00163
00164 Geocentric& Geocentric::operator=( const Geocentric &g )
00165 {
00166 if( this != &g )
00167 {
00168 semiMajorAxis = g.semiMajorAxis;
00169 flattening = g.flattening;
00170 Geocent_e2 = g.Geocent_e2;
00171 Geocent_ep2 = g.Geocent_ep2;
00172 }
00173
00174 return *this;
00175 }
00176
00177
00178 MSP::CCS::CartesianCoordinates* Geocentric::convertFromGeodetic( const MSP::CCS::GeodeticCoordinates* geodeticCoordinates )
00179 {
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194 double Rn;
00195 double Sin_Lat;
00196 double Sin2_Lat;
00197 double Cos_Lat;
00198 char errorStatus[50] = "";
00199
00200 double longitude = geodeticCoordinates->longitude();
00201 double latitude = geodeticCoordinates->latitude();
00202 double height = geodeticCoordinates->height();
00203
00204 if ((latitude < -PI_OVER_2) || (latitude > PI_OVER_2))
00205 {
00206 strcat( errorStatus, MSP::CCS::ErrorMessages::latitude );
00207 }
00208 if ((longitude < -PI) || (longitude > (2*PI)))
00209 {
00210 strcat( errorStatus, MSP::CCS::ErrorMessages::longitude );
00211 }
00212
00213 if( strlen( errorStatus ) > 0)
00214 throw CoordinateConversionException( errorStatus );
00215
00216 if (longitude > PI)
00217 longitude -= (2*PI);
00218 Sin_Lat = sin(latitude);
00219 Cos_Lat = cos(latitude);
00220 Sin2_Lat = Sin_Lat * Sin_Lat;
00221 Rn = semiMajorAxis / (sqrt(1.0e0 - Geocent_e2 * Sin2_Lat));
00222 double X = (Rn + height) * Cos_Lat * cos(longitude);
00223 double Y = (Rn + height) * Cos_Lat * sin(longitude);
00224 double Z = ((Rn * (1 - Geocent_e2)) + height) * Sin_Lat;
00225
00226 return new CartesianCoordinates( CoordinateType::geocentric, X, Y, Z );
00227 }
00228
00229
00230 MSP::CCS::GeodeticCoordinates* Geocentric::convertToGeodetic( MSP::CCS::CartesianCoordinates* cartesianCoordinates )
00231 {
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250 double W;
00251 double W2;
00252 double T0;
00253 double T1;
00254 double S0;
00255 double S1;
00256 double Sin_B0;
00257 double Sin3_B0;
00258 double Cos_B0;
00259 double Sin_p1;
00260 double Cos_p1;
00261 double Rn;
00262 double Sum;
00263 int At_Pole;
00264 double longitude, latitude, height;
00265 double Geocent_b = semiMajorAxis * (1 - flattening);
00266
00267 double X = cartesianCoordinates->x();
00268 double Y = cartesianCoordinates->y();
00269 double Z = cartesianCoordinates->z();
00270
00271 At_Pole = FALSE;
00272 if (X != 0.0)
00273 {
00274 longitude = atan2(Y,X);
00275 }
00276 else
00277 {
00278 if (Y > 0)
00279 {
00280 longitude = PI_OVER_2;
00281 }
00282 else if (Y < 0)
00283 {
00284 longitude = -PI_OVER_2;
00285 }
00286 else
00287 {
00288 At_Pole = TRUE;
00289 longitude = 0.0;
00290 if (Z > 0.0)
00291 {
00292 latitude = PI_OVER_2;
00293 }
00294 else if (Z < 0.0)
00295 {
00296 latitude = -PI_OVER_2;
00297 }
00298 else
00299 {
00300 latitude = PI_OVER_2;
00301 height = -Geocent_b;
00302 return new GeodeticCoordinates( CoordinateType::geodetic, longitude, latitude, height );
00303 }
00304 }
00305 }
00306 W2 = X*X + Y*Y;
00307 W = sqrt(W2);
00308 T0 = Z * AD_C;
00309 S0 = sqrt(T0 * T0 + W2);
00310 Sin_B0 = T0 / S0;
00311 Cos_B0 = W / S0;
00312 Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
00313 T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
00314 Sum = W - semiMajorAxis * Geocent_e2 * Cos_B0 * Cos_B0 * Cos_B0;
00315 S1 = sqrt(T1*T1 + Sum * Sum);
00316 Sin_p1 = T1 / S1;
00317 Cos_p1 = Sum / S1;
00318 Rn = semiMajorAxis / sqrt(1.0 - Geocent_e2 * Sin_p1 * Sin_p1);
00319 if (Cos_p1 >= COS_67P5)
00320 {
00321 height = W / Cos_p1 - Rn;
00322 }
00323 else if (Cos_p1 <= -COS_67P5)
00324 {
00325 height = W / -Cos_p1 - Rn;
00326 }
00327 else
00328 {
00329 height = Z / Sin_p1 + Rn * (Geocent_e2 - 1.0);
00330 }
00331 if (At_Pole == FALSE)
00332 {
00333 latitude = atan(Sin_p1 / Cos_p1);
00334 }
00335
00336 return new GeodeticCoordinates( CoordinateType::geodetic, longitude, latitude, height );
00337 }
00338
00339
00340
00341