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 #include <dvm3/dvm3_rotmat.h>
00029 #include <cstdlib>
00030 #include <iostream>
00031
00032 using namespace std;
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043 dvm3_RotMat::
00044 dvm3_RotMat()
00045 : dvm3_Matrix()
00046 {
00047 vm_VMath<double,ENelts>::set( data_.m_, 0.0 );
00048 data_.m_[0] = 1.0; data_.m_[4] = 1.0; data_.m_[8] = 1.0;
00049 }
00050
00051
00052 dvm3_RotMat::
00053 dvm3_RotMat( dvm3_Vector const& x,
00054 dvm3_Vector const& y,
00055 dvm3_Vector const& z,
00056 double const tol )
00057 : dvm3_Matrix(x, y, z)
00058 {
00059 if (! is_rotation_matrix(tol) )
00060 {
00061 std::cerr << "dvm3_RotMat::dvm3_RotMat( dvm3_Vector const&,\n"
00062 << " dvm3_Vector const&,\n"
00063 << " dvm3_Vector const&)\n"
00064 << " input vectors are not orthonormal to within tolerance "
00065 << tol << std::endl;
00066 std::exit(1);
00067 }
00068 else
00069 {
00070 orthonormalize();
00071 }
00072 }
00073
00074
00075 dvm3_RotMat::
00076 dvm3_RotMat( double const x[],
00077 double const y[],
00078 double const z[],
00079 double const tol )
00080 : dvm3_Matrix(x, y, z)
00081 {
00082 if (! is_rotation_matrix(tol) )
00083 {
00084 std::cerr << "dvm3_RotMat::dvm3_RotMat( double const v0[],\n"
00085 << " double const v1[],\n"
00086 << " double const v2[])\n"
00087 << " input vectors are not orthonormal to within tolerance "
00088 << tol << std::endl;
00089 std::exit(1);
00090 }
00091 else
00092 {
00093 orthonormalize();
00094 }
00095 }
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 dvm3_RotMat::
00111 dvm3_RotMat( double angle0, double angle1, double angle2,
00112 dvm3_RotMat::EEulerAngleType angle_type )
00113 {
00114 init( angle0, angle1, angle2, angle_type );
00115 }
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 dvm3_RotMat::
00160 dvm3_RotMat( dvm3_Vector const& v0,
00161 dvm3_Vector const& v1,
00162 EInputVecPair type
00163 )
00164 {
00165 init( v0, v1, type );
00166 }
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 dvm3_RotMat::
00211 dvm3_RotMat( double const v0[],
00212 double const v1[],
00213 EInputVecPair type
00214 )
00215 {
00216 init( v0, v1, type );
00217 }
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236 void dvm3_RotMat::
00237 init( double angle0, double angle1, double angle2,
00238 dvm3_RotMat::EEulerAngleType angle_type )
00239 {
00240 switch ( angle_type )
00241 {
00242 case EOSAC_az_el_clock:
00243 {
00244 double cos_az = cos(angle0);
00245 double sin_az = sin(angle0);
00246 double cos_el = cos(angle1);
00247 double sin_el = sin(angle1);
00248 double cos_z = cos(angle2);
00249 double sin_z = sin(angle2);
00250
00251 vm_V3Math<double>::set( &data_.m_[ERow0],
00252 cos_z * cos_az - sin_z * sin_el * sin_az,
00253 -sin_z * cos_az - cos_z * sin_el * sin_az,
00254 cos_el * sin_az );
00255
00256 vm_V3Math<double>::set( &data_.m_[ERow1], sin_z * cos_el,
00257 cos_z * cos_el,
00258 sin_el );
00259
00260 vm_V3Math<double>::set( &data_.m_[ERow2],
00261 -cos_z * sin_az - sin_z * sin_el * cos_az,
00262 sin_z * sin_az - cos_z * sin_el * cos_az,
00263 cos_el * cos_az );
00264 }
00265 break;
00266 case EX_convention:
00267 {
00268 double cos_phi = cos( angle0 );
00269 double sin_phi = sin( angle0 );
00270 double cos_theta = cos( angle1 );
00271 double sin_theta = sin( angle1 );
00272 double cos_psi = cos( angle2 );
00273 double sin_psi = sin( angle2 );
00274
00275 vm_V3Math<double>::set(
00276 &data_.m_[ERow0],
00277 cos_psi * cos_phi - cos_theta * sin_phi * sin_psi,
00278 cos_psi * sin_phi + cos_theta * cos_phi * sin_psi,
00279 sin_psi * sin_theta
00280 );
00281
00282 vm_V3Math<double>::set(
00283 &data_.m_[ERow1],
00284 -1.0 * sin_psi * cos_phi - cos_theta * sin_phi * cos_psi,
00285 -1.0 * sin_psi * sin_phi + cos_theta * cos_phi * cos_psi,
00286 cos_psi * sin_theta
00287 );
00288
00289 vm_V3Math<double>::set(
00290 &data_.m_[ERow2],
00291 sin_theta * sin_phi,
00292 -1.0 * sin_theta * cos_phi,
00293 cos_theta
00294 );
00295 }
00296 break;
00297 case EY_convention:
00298 {
00299 double cos_phi = cos( angle0 );
00300 double sin_phi = sin( angle0 );
00301 double cos_theta = cos( angle1 );
00302 double sin_theta = sin( angle1 );
00303 double cos_psi = cos( angle2 );
00304 double sin_psi = sin( angle2 );
00305
00306 vm_V3Math<double>::set(
00307 &data_.m_[ERow0],
00308 -1.0 * sin_psi * sin_phi + cos_theta * cos_phi * cos_psi,
00309 sin_psi * cos_phi + cos_theta * sin_phi * cos_psi,
00310 -1.0 * cos_psi * sin_theta
00311 );
00312
00313 vm_V3Math<double>::set(
00314 &data_.m_[ERow1],
00315 -1.0 * cos_psi * sin_phi - cos_theta * cos_phi * sin_psi,
00316 cos_psi * cos_phi - cos_theta * sin_phi * sin_psi,
00317 sin_psi * sin_theta
00318 );
00319
00320 vm_V3Math<double>::set(
00321 &data_.m_[ERow2],
00322 sin_theta * cos_phi,
00323 sin_theta * sin_phi,
00324 cos_theta
00325 );
00326 }
00327 break;
00328 default:
00329 {
00330 std::cerr << "dvm3_RotMat::dvm3_RotMat( double angle0,\n"
00331 << " double angle1,\n"
00332 << " double angle2,\n"
00333 << " EEulerAngleType type )\n"
00334 << " invalid Euler angle type encountered." << std::endl;
00335 std::exit(1);
00336 }
00337 break;
00338 }
00339 }
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383 void dvm3_RotMat::
00384 init( dvm3_Vector const& v0,
00385 dvm3_Vector const& v1,
00386 EInputVecPair type
00387 )
00388 {
00389 switch ( type )
00390 {
00391 case E_XY:
00392 vm_V3Math<double>::copy( &data_.m_[ERow0], v0.data_.x_ );
00393 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00394 vm_V3Math<double>::cross( &data_.m_[ERow2],
00395 &data_.m_[ERow0], v1.data_.x_);
00396 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00397 vm_V3Math<double>::cross( &data_.m_[ERow1],
00398 &data_.m_[ERow2], &data_.m_[ERow0]);
00399 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00400 break;
00401
00402 case E_YZ:
00403 vm_V3Math<double>::copy( &data_.m_[ERow1], v0.data_.x_ );
00404 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00405 vm_V3Math<double>::cross( &data_.m_[ERow0],
00406 &data_.m_[ERow1], v1.data_.x_);
00407 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00408 vm_V3Math<double>::cross( &data_.m_[ERow2],
00409 &data_.m_[ERow0], &data_.m_[ERow1]);
00410 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00411 break;
00412
00413 case E_ZX:
00414 vm_V3Math<double>::copy( &data_.m_[ERow2], v0.data_.x_ );
00415 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00416 vm_V3Math<double>::cross( &data_.m_[ERow1],
00417 &data_.m_[ERow2], v1.data_.x_);
00418 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00419 vm_V3Math<double>::cross( &data_.m_[ERow0],
00420 &data_.m_[ERow1], &data_.m_[ERow2]);
00421 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00422 break;
00423
00424 case E_XZ:
00425 vm_V3Math<double>::copy( &data_.m_[ERow0], v0.data_.x_ );
00426 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00427 vm_V3Math<double>::cross( &data_.m_[ERow1],
00428 v1.data_.x_, &data_.m_[ERow0]);
00429 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00430 vm_V3Math<double>::cross( &data_.m_[ERow2],
00431 &data_.m_[ERow0], &data_.m_[ERow1]);
00432 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00433 break;
00434
00435 case E_ZY:
00436 vm_V3Math<double>::copy( &data_.m_[ERow2], v0.data_.x_ );
00437 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00438 vm_V3Math<double>::cross( &data_.m_[ERow0],
00439 v1.data_.x_, &data_.m_[ERow2]);
00440 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00441 vm_V3Math<double>::cross( &data_.m_[ERow1],
00442 &data_.m_[ERow2], &data_.m_[ERow0]);
00443 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00444 break;
00445
00446 case E_YX:
00447 vm_V3Math<double>::copy( &data_.m_[ERow1], v0.data_.x_ );
00448 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00449 vm_V3Math<double>::cross( &data_.m_[ERow2],
00450 v1.data_.x_, &data_.m_[ERow1]);
00451 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00452 vm_V3Math<double>::cross( &data_.m_[ERow0],
00453 &data_.m_[ERow1], &data_.m_[ERow2]);
00454 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00455 break;
00456 }
00457 }
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501 void dvm3_RotMat::
00502 init( double const v0[],
00503 double const v1[],
00504 EInputVecPair type
00505 )
00506 {
00507 switch ( type )
00508 {
00509 case E_XY:
00510 vm_V3Math<double>::copy( &data_.m_[ERow0], v0 );
00511 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00512 vm_V3Math<double>::cross( &data_.m_[ERow2],
00513 &data_.m_[ERow0], v1 );
00514 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00515 vm_V3Math<double>::cross( &data_.m_[ERow1],
00516 &data_.m_[ERow2], &data_.m_[ERow0]);
00517 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00518 break;
00519
00520 case E_YZ:
00521 vm_V3Math<double>::copy( &data_.m_[ERow1], v0 );
00522 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00523 vm_V3Math<double>::cross( &data_.m_[ERow0],
00524 &data_.m_[ERow1], v1 );
00525 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00526 vm_V3Math<double>::cross( &data_.m_[ERow2],
00527 &data_.m_[ERow0], &data_.m_[ERow1]);
00528 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00529 break;
00530
00531 case E_ZX:
00532 vm_V3Math<double>::copy( &data_.m_[ERow2], v0 );
00533 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00534 vm_V3Math<double>::cross( &data_.m_[ERow1],
00535 &data_.m_[ERow2], v1 );
00536 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00537 vm_V3Math<double>::cross( &data_.m_[ERow0],
00538 &data_.m_[ERow1], &data_.m_[ERow2]);
00539 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00540 break;
00541
00542 case E_XZ:
00543 vm_V3Math<double>::copy( &data_.m_[ERow0], v0 );
00544 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00545 vm_V3Math<double>::cross( &data_.m_[ERow1],
00546 v1, &data_.m_[ERow0]);
00547 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00548 vm_V3Math<double>::cross( &data_.m_[ERow2],
00549 &data_.m_[ERow0], &data_.m_[ERow1]);
00550 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00551 break;
00552
00553 case E_ZY:
00554 vm_V3Math<double>::copy( &data_.m_[ERow2], v0 );
00555 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00556 vm_V3Math<double>::cross( &data_.m_[ERow0],
00557 v1, &data_.m_[ERow2]);
00558 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00559 vm_V3Math<double>::cross( &data_.m_[ERow1],
00560 &data_.m_[ERow2], &data_.m_[ERow0]);
00561 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00562 break;
00563
00564 case E_YX:
00565 vm_V3Math<double>::copy( &data_.m_[ERow1], v0 );
00566 vm_V3Math<double>::unitize(&data_.m_[ERow1]);
00567 vm_V3Math<double>::cross( &data_.m_[ERow2],
00568 v1, &data_.m_[ERow1]);
00569 vm_V3Math<double>::unitize(&data_.m_[ERow2]);
00570 vm_V3Math<double>::cross( &data_.m_[ERow0],
00571 &data_.m_[ERow1], &data_.m_[ERow2]);
00572 vm_V3Math<double>::unitize(&data_.m_[ERow0]);
00573 break;
00574 }
00575 }
00576
00577
00578
00579
00580 void dvm3_RotMat::
00581 invert()
00582 {
00583 #ifdef _MYSWAP_
00584 #error "dvm3_RotMat::invert() - _MYSWAP_ already defined!"
00585 #else
00586 #define _MYSWAP_(a,b,temp) {temp=a; a=b; b=temp;}
00587 #endif
00588 double tmp;
00589 _MYSWAP_( data_.m_[1], data_.m_[3], tmp )
00590 _MYSWAP_( data_.m_[2], data_.m_[6], tmp )
00591 _MYSWAP_( data_.m_[5], data_.m_[7], tmp )
00592 }
00593
00594
00595
00596
00597 int dvm3_RotMat::
00598 is_rotation_matrix( double const tol ) const
00599 {
00600 int rc = 0;
00601 rc += !vm_V3Math<double>::is_unit_vector(&data_.m_[ERow0], tol);
00602 rc += !vm_V3Math<double>::is_unit_vector(&data_.m_[ERow1], tol);
00603 rc += !vm_V3Math<double>::is_unit_vector(&data_.m_[ERow2], tol);
00604 rc += !vm_V3Math<double>::are_orthogonal(&data_.m_[ERow0],
00605 &data_.m_[ERow1], tol);
00606 rc += !vm_V3Math<double>::are_orthogonal(&data_.m_[ERow1],
00607 &data_.m_[ERow2], tol);
00608 rc += !vm_V3Math<double>::are_orthogonal(&data_.m_[ERow2],
00609 &data_.m_[ERow0], tol);
00610 return !rc;
00611 }
00612
00613
00614
00615
00616
00617
00618