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
00080
00081 #include <math.h>
00082 #include "Geocentric.h"
00083 #include "LocalCartesian.h"
00084 #include "LocalCartesianParameters.h"
00085 #include "CartesianCoordinates.h"
00086 #include "GeodeticCoordinates.h"
00087 #include "CoordinateConversionException.h"
00088 #include "ErrorMessages.h"
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102 using namespace MSP::CCS;
00103
00104
00105
00106
00107
00108
00109
00110 const double PI = 3.14159265358979323e0;
00111 const double PI_OVER_2 = ( PI / 2.0e0);
00112 const double TWO_PI = (2.0 * PI);
00113
00114
00115
00116
00117
00118
00119
00120 LocalCartesian::LocalCartesian( double ellipsoidSemiMajorAxis, double ellipsoidFlattening, double originLongitude, double originLatitude, double originHeight, double orientation ) :
00121 CoordinateSystem(),
00122 geocentric( 0 ),
00123 es2( 0.0066943799901413800 ),
00124 u0( 6378137.0 ),
00125 v0( 0.0 ),
00126 w0( 0.0 ),
00127 LocalCart_Origin_Long( 0.0 ),
00128 LocalCart_Origin_Lat( 0.0 ),
00129 LocalCart_Origin_Height( 0.0 ),
00130 LocalCart_Orientation( 0.0 ),
00131 Sin_LocalCart_Origin_Lat( 0.0 ),
00132 Cos_LocalCart_Origin_Lat( 1.0 ),
00133 Sin_LocalCart_Origin_Lon( 0.0 ),
00134 Cos_LocalCart_Origin_Lon( 1.0 ),
00135 Sin_LocalCart_Orientation( 0.0 ),
00136 Cos_LocalCart_Orientation( 1.0 ),
00137 Sin_Lat_Sin_Orient( 0.0 ),
00138 Sin_Lat_Cos_Orient( 0.0 )
00139 {
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153 double N0;
00154 double inv_f = 1 / ellipsoidFlattening;
00155 double val;
00156 char errorStatus[500] = "";
00157
00158 if (ellipsoidSemiMajorAxis <= 0.0)
00159 {
00160 strcat( errorStatus, ErrorMessages::semiMajorAxis );
00161 }
00162 if ((inv_f < 250) || (inv_f > 350))
00163 {
00164 strcat( errorStatus, ErrorMessages::ellipsoidFlattening );
00165 }
00166 if ((originLatitude < -PI_OVER_2) || (originLatitude > PI_OVER_2))
00167 {
00168 strcat( errorStatus, ErrorMessages::originLatitude );
00169 }
00170 if ((originLongitude < -PI) || (originLongitude > TWO_PI))
00171 {
00172 strcat( errorStatus, ErrorMessages::originLongitude );
00173 }
00174 if ((orientation < -PI) || (orientation > TWO_PI))
00175 {
00176 strcat( errorStatus, ErrorMessages::orientation );
00177 }
00178
00179 if( strlen( errorStatus ) > 0)
00180 throw CoordinateConversionException( errorStatus );
00181
00182 semiMajorAxis = ellipsoidSemiMajorAxis;
00183 flattening = ellipsoidFlattening;
00184
00185 LocalCart_Origin_Lat = originLatitude;
00186 if (originLongitude > PI)
00187 originLongitude -= TWO_PI;
00188 LocalCart_Origin_Long = originLongitude;
00189 LocalCart_Origin_Height = originHeight;
00190 if (orientation > PI)
00191 orientation -= TWO_PI;
00192 LocalCart_Orientation = orientation;
00193 es2 = 2 * flattening - flattening * flattening;
00194
00195 Sin_LocalCart_Origin_Lat = sin(LocalCart_Origin_Lat);
00196 Cos_LocalCart_Origin_Lat = cos(LocalCart_Origin_Lat);
00197 Sin_LocalCart_Origin_Lon = sin(LocalCart_Origin_Long);
00198 Cos_LocalCart_Origin_Lon = cos(LocalCart_Origin_Long);
00199 Sin_LocalCart_Orientation = sin(LocalCart_Orientation);
00200 Cos_LocalCart_Orientation = cos(LocalCart_Orientation);
00201
00202 Sin_Lat_Sin_Orient = Sin_LocalCart_Origin_Lat * Sin_LocalCart_Orientation;
00203 Sin_Lat_Cos_Orient = Sin_LocalCart_Origin_Lat * Cos_LocalCart_Orientation;
00204
00205 N0 = semiMajorAxis / sqrt(1 - es2 * Sin_LocalCart_Origin_Lat * Sin_LocalCart_Origin_Lat);
00206
00207 val = (N0 + LocalCart_Origin_Height) * Cos_LocalCart_Origin_Lat;
00208 u0 = val * Cos_LocalCart_Origin_Lon;
00209 v0 = val * Sin_LocalCart_Origin_Lon;
00210 w0 = ((N0 * (1 - es2)) + LocalCart_Origin_Height) * Sin_LocalCart_Origin_Lat;
00211
00212 geocentric = new Geocentric( semiMajorAxis, flattening );
00213 }
00214
00215
00216 LocalCartesian::LocalCartesian( const LocalCartesian &lc )
00217 {
00218 geocentric = new Geocentric( *( lc.geocentric ) );
00219 semiMajorAxis = lc.semiMajorAxis;
00220 flattening = lc.flattening;
00221 es2 = lc.es2;
00222 u0 = lc.u0;
00223 v0 = lc.v0;
00224 w0 = lc.w0;
00225 LocalCart_Origin_Long = lc.LocalCart_Origin_Long;
00226 LocalCart_Origin_Lat = lc.LocalCart_Origin_Lat;
00227 LocalCart_Origin_Height = lc.LocalCart_Origin_Height;
00228 LocalCart_Orientation = lc.LocalCart_Orientation;
00229 Sin_LocalCart_Origin_Lat = lc.Sin_LocalCart_Origin_Lat;
00230 Cos_LocalCart_Origin_Lat = lc.Cos_LocalCart_Origin_Lat;
00231 Sin_LocalCart_Origin_Lon = lc.Sin_LocalCart_Origin_Lon;
00232 Cos_LocalCart_Origin_Lon = lc.Cos_LocalCart_Origin_Lon;
00233 Sin_LocalCart_Orientation = lc.Sin_LocalCart_Orientation;
00234 Cos_LocalCart_Orientation = lc.Cos_LocalCart_Orientation;
00235 Sin_Lat_Sin_Orient = lc.Sin_Lat_Sin_Orient;
00236 Sin_Lat_Cos_Orient = lc.Sin_Lat_Cos_Orient;
00237 }
00238
00239
00240 LocalCartesian::~LocalCartesian()
00241 {
00242 delete geocentric;
00243 geocentric = 0;
00244 }
00245
00246
00247 LocalCartesian& LocalCartesian::operator=( const LocalCartesian &lc )
00248 {
00249 if( this != &lc )
00250 {
00251 geocentric->operator=( *lc.geocentric );
00252 semiMajorAxis = lc.semiMajorAxis;
00253 flattening = lc.flattening;
00254 es2 = lc.es2;
00255 u0 = lc.u0;
00256 v0 = lc.v0;
00257 w0 = lc.w0;
00258 LocalCart_Origin_Long = lc.LocalCart_Origin_Long;
00259 LocalCart_Origin_Lat = lc.LocalCart_Origin_Lat;
00260 LocalCart_Origin_Height = lc.LocalCart_Origin_Height;
00261 LocalCart_Orientation = lc.LocalCart_Orientation;
00262 Sin_LocalCart_Origin_Lat = lc.Sin_LocalCart_Origin_Lat;
00263 Cos_LocalCart_Origin_Lat = lc.Cos_LocalCart_Origin_Lat;
00264 Sin_LocalCart_Origin_Lon = lc.Sin_LocalCart_Origin_Lon;
00265 Cos_LocalCart_Origin_Lon = lc.Cos_LocalCart_Origin_Lon;
00266 Sin_LocalCart_Orientation = lc.Sin_LocalCart_Orientation;
00267 Cos_LocalCart_Orientation = lc.Cos_LocalCart_Orientation;
00268 Sin_Lat_Sin_Orient = lc.Sin_Lat_Sin_Orient;
00269 Sin_Lat_Cos_Orient = lc.Sin_Lat_Cos_Orient;
00270 }
00271
00272 return *this;
00273 }
00274
00275
00276 LocalCartesianParameters* LocalCartesian::getParameters() const
00277 {
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291 return new LocalCartesianParameters( CoordinateType::localCartesian, LocalCart_Origin_Long, LocalCart_Origin_Lat, LocalCart_Origin_Height, LocalCart_Orientation );
00292 }
00293
00294
00295 MSP::CCS::CartesianCoordinates* LocalCartesian::convertFromGeodetic( MSP::CCS::GeodeticCoordinates* geodeticCoordinates )
00296 {
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 char errorStatus[50] = "";
00312
00313 double longitude = geodeticCoordinates->longitude();
00314 double latitude = geodeticCoordinates->latitude();
00315 double height = geodeticCoordinates->height();
00316
00317 if ((latitude < -PI_OVER_2) || (latitude > PI_OVER_2))
00318 {
00319 strcat( errorStatus, ErrorMessages::latitude );
00320 }
00321 if ((longitude < -PI) || (longitude > TWO_PI))
00322 {
00323 strcat( errorStatus, ErrorMessages::longitude );
00324 }
00325
00326 if( strlen( errorStatus ) > 0)
00327 throw CoordinateConversionException( errorStatus );
00328
00329 CartesianCoordinates* geocentricCoordinates = geocentric->convertFromGeodetic( geodeticCoordinates );
00330
00331 CartesianCoordinates* cartesianCoordinates = convertFromGeocentric( geocentricCoordinates );
00332 delete geocentricCoordinates;
00333
00334 return cartesianCoordinates;
00335 }
00336
00337
00338 MSP::CCS::GeodeticCoordinates* LocalCartesian::convertToGeodetic( MSP::CCS::CartesianCoordinates* cartesianCoordinates )
00339 {
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353 CartesianCoordinates* geocentricCoordinates = convertToGeocentric( cartesianCoordinates );
00354
00355 GeodeticCoordinates* geodeticCoordinates = geocentric->convertToGeodetic( geocentricCoordinates );
00356 double longitude = geodeticCoordinates->longitude();
00357
00358 if (longitude > PI)
00359 geodeticCoordinates->setLongitude( longitude -= TWO_PI );
00360 longitude = geodeticCoordinates->longitude();
00361 if (longitude < -PI)
00362 geodeticCoordinates->setLongitude( longitude += TWO_PI );
00363
00364 delete geocentricCoordinates;
00365
00366 return geodeticCoordinates;
00367 }
00368
00369
00370 MSP::CCS::CartesianCoordinates* LocalCartesian::convertFromGeocentric( const MSP::CCS::CartesianCoordinates* cartesianCoordinates )
00371 {
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385 double X, Y, Z;
00386 double u_MINUS_u0, v_MINUS_v0, w_MINUS_w0;
00387
00388 double U = cartesianCoordinates->x();
00389 double V = cartesianCoordinates->y();
00390 double W = cartesianCoordinates->z();
00391
00392 u_MINUS_u0 = U - u0;
00393 v_MINUS_v0 = V - v0;
00394 w_MINUS_w0 = W - w0;
00395
00396 if (LocalCart_Orientation == 0.0)
00397 {
00398 double cos_lon_u_MINUS_u0 = Cos_LocalCart_Origin_Lon * u_MINUS_u0;
00399 double sin_lon_v_MINUS_v0 = Sin_LocalCart_Origin_Lon * v_MINUS_v0;
00400
00401 X = -Sin_LocalCart_Origin_Lon * u_MINUS_u0 + Cos_LocalCart_Origin_Lon * v_MINUS_v0;
00402 Y = -Sin_LocalCart_Origin_Lat * cos_lon_u_MINUS_u0 + -Sin_LocalCart_Origin_Lat * sin_lon_v_MINUS_v0 + Cos_LocalCart_Origin_Lat * w_MINUS_w0;
00403 Z = Cos_LocalCart_Origin_Lat * cos_lon_u_MINUS_u0 + Cos_LocalCart_Origin_Lat * sin_lon_v_MINUS_v0 + Sin_LocalCart_Origin_Lat * w_MINUS_w0;
00404 }
00405 else
00406 {
00407 double cos_lat_w_MINUS_w0 = Cos_LocalCart_Origin_Lat * w_MINUS_w0;
00408
00409 X = (-Cos_LocalCart_Orientation * Sin_LocalCart_Origin_Lon + Sin_Lat_Sin_Orient * Cos_LocalCart_Origin_Lon) * u_MINUS_u0 +
00410 (Cos_LocalCart_Orientation * Cos_LocalCart_Origin_Lon + Sin_Lat_Sin_Orient * Sin_LocalCart_Origin_Lon) * v_MINUS_v0 +
00411 (-Sin_LocalCart_Orientation * cos_lat_w_MINUS_w0);
00412
00413 Y = (-Sin_LocalCart_Orientation * Sin_LocalCart_Origin_Lon - Sin_Lat_Cos_Orient * Cos_LocalCart_Origin_Lon) * u_MINUS_u0 +
00414 (Sin_LocalCart_Orientation * Cos_LocalCart_Origin_Lon - Sin_Lat_Cos_Orient * Sin_LocalCart_Origin_Lon) * v_MINUS_v0 +
00415 (Cos_LocalCart_Orientation * cos_lat_w_MINUS_w0);
00416
00417 Z = (Cos_LocalCart_Origin_Lat * Cos_LocalCart_Origin_Lon) * u_MINUS_u0 +
00418 (Cos_LocalCart_Origin_Lat * Sin_LocalCart_Origin_Lon) * v_MINUS_v0 +
00419 Sin_LocalCart_Origin_Lat * w_MINUS_w0;
00420 }
00421
00422 return new CartesianCoordinates( CoordinateType::localCartesian, X, Y, Z );
00423 }
00424
00425
00426 MSP::CCS::CartesianCoordinates* LocalCartesian::convertToGeocentric( const MSP::CCS::CartesianCoordinates* cartesianCoordinates )
00427 {
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441 double U, V, W;
00442
00443 double X = cartesianCoordinates->x();
00444 double Y = cartesianCoordinates->y();
00445 double Z = cartesianCoordinates->z();
00446
00447 if (LocalCart_Orientation == 0.0)
00448 {
00449 double sin_lat_y = Sin_LocalCart_Origin_Lat * Y;
00450 double cos_lat_z = Cos_LocalCart_Origin_Lat * Z;
00451
00452 U = -Sin_LocalCart_Origin_Lon * X - sin_lat_y * Cos_LocalCart_Origin_Lon + cos_lat_z * Cos_LocalCart_Origin_Lon + u0;
00453 V = Cos_LocalCart_Origin_Lon * X - sin_lat_y * Sin_LocalCart_Origin_Lon + cos_lat_z * Sin_LocalCart_Origin_Lon + v0;
00454 W = Cos_LocalCart_Origin_Lat * Y + Sin_LocalCart_Origin_Lat * Z + w0;
00455 }
00456 else
00457 {
00458 double rotated_x, rotated_y;
00459 double rotated_y_sin_lat, z_cos_lat;
00460
00461 rotated_x = Cos_LocalCart_Orientation * X + Sin_LocalCart_Orientation * Y;
00462 rotated_y = -Sin_LocalCart_Orientation * X + Cos_LocalCart_Orientation * Y;
00463
00464 rotated_y_sin_lat = rotated_y * Sin_LocalCart_Origin_Lat;
00465 z_cos_lat = Z * Cos_LocalCart_Origin_Lat;
00466
00467 U = -Sin_LocalCart_Origin_Lon * rotated_x - Cos_LocalCart_Origin_Lon * rotated_y_sin_lat + Cos_LocalCart_Origin_Lon * z_cos_lat + u0;
00468 V = Cos_LocalCart_Origin_Lon * rotated_x - Sin_LocalCart_Origin_Lon * rotated_y_sin_lat + Sin_LocalCart_Origin_Lon * z_cos_lat + v0;
00469 W = Cos_LocalCart_Origin_Lat * rotated_y + Sin_LocalCart_Origin_Lat * Z + w0;
00470 }
00471
00472 return new CartesianCoordinates( CoordinateType::geocentric, U, V, W );
00473 }
00474
00475