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