dvm3_rotmat.cc

00001 // File:    dvm3_rotmat.cc
00002 // Author:  Terry Gaetz
00003 
00004 // --8<--8<--8<--8<--
00005 //
00006 // Copyright (C) 2006 Smithsonian Astrophysical Observatory
00007 //
00008 // This file is part of dvm3
00009 //
00010 // dvm3 is free software; you can redistribute it and/or
00011 // modify it under the terms of the GNU General Public License
00012 // as published by the Free Software Foundation; either version 2
00013 // of the License, or (at your option) any later version.
00014 //
00015 // dvm3 is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 // GNU General Public License for more details.
00019 //
00020 // You should have received a copy of the GNU General Public License
00021 // along with this program; if not, write to the 
00022 //       Free Software Foundation, Inc. 
00023 //       51 Franklin Street, Fifth Floor
00024 //       Boston, MA  02110-1301, USA
00025 //
00026 // -->8-->8-->8-->8--
00027 
00028 #include <dvm3/dvm3_rotmat.h>   // dvm3_RotMat
00029 #include <cstdlib>              // exit()
00030 #include <iostream>
00031 
00032 using namespace std;
00033 
00034 /****************************************************************************
00035  */
00036 
00037 //#########################################################################
00038 // dtor, ctors, initializers...
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 // construct rotation matrix based on 3 Euler angles; the type of Euler
00100 // angles is specified by angle_type.
00101 //
00102 // The matrix generated by this routine does the opposite
00103 // of what the original rigid body motion did to the body
00104 // centered coordinate system.  in other words, it does
00105 // what is required to a location or direction to transform
00106 // it from std to bcs.
00107 //
00108 // NOTE:  arguments are in radians.
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 //: Construct a rotation matrix based on two (non-colinear) vectors.
00120 //  The rotation matrix is constructed by sequential application
00121 //  cross products.
00122 //  <p>
00123 //  If type == E_XY, the vectors passed in are assumed to lie in the
00124 //  x-y plane with v0 an x-like vector and v1 a y-like vector;
00125 //  e_x is v0.  e_z is constructed from e_x cross v1.
00126 //  e_y is constructed from e_z cross e_x.  The three vectors 
00127 //  e_x, e_y, and e_z are normalized.
00128 //  <p>
00129 //  If type == E_YZ, the vectors passed in are assumed to lie in the
00130 //  y-z plane with v0 an y-like vector and v1 a z-like vector;
00131 //  e_y is v0.  e_x is constructed from e_y cross v1.
00132 //  e_z is constructed from e_x cross e_y.  The three vectors 
00133 //  e_x, e_y, and e_z are normalized.
00134 //  <p>
00135 //  If type == E_ZX, the vectors passed in are assumed to lie in the
00136 //  x-z plane with v0 an z-like vector and v1 a x-like vector;
00137 //  e_z is v0.  e_y is constructed from e_z cross v1.
00138 //  e_x is constructed from e_y cross e_z.  The three vectors 
00139 //  e_x, e_y, and e_z are normalized.
00140 //  <p>
00141 //  If type == E_YX, the vectors passed in are assumed to lie in the
00142 //  x-y plane with v0 a y-like vector and v1 an x-like vector;
00143 //  e_y is v0.  e_z is constructed from v1 cross e_y.
00144 //  e_x is constructed from e_y cross e_z.  The three vectors 
00145 //  e_x, e_y, and e_z are normalized.
00146 //  <p>
00147 //  If type == E_ZY, the vectors passed in are assumed to lie in the
00148 //  y-z plane with v0 a z-like vector and v1 a y-like vector;
00149 //  e_z is v0.  e_x is constructed from v1 cross e_z.
00150 //  e_y is constructed from e_z cross e_x.  The three vectors 
00151 //  e_x, e_y, and e_z are normalized.
00152 //  <p>
00153 //  If type == E_XZ, the vectors passed in are assumed to lie in the
00154 //  x-z plane with v0 an x-like vector and v1 a z-like vector;
00155 //  e_x is v0.  e_y is constructed from v1 cross e_x.
00156 //  e_z is constructed from e_x cross e_y.  The three vectors 
00157 //  e_x, e_y, and e_z are normalized.
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 //: Construct a rotation matrix based on two (non-colinear) vectors.
00171 //  The rotation matrix is constructed by sequential application
00172 //  cross products.
00173 //  <p>
00174 //  If type == E_XY, the vectors passed in are assumed to lie in the
00175 //  x-y plane with v0 an x-like vector and v1 a y-like vector;
00176 //  e_x is v0.  e_z is constructed from e_x cross v1.
00177 //  e_y is constructed from e_z cross e_x.  The three vectors 
00178 //  e_x, e_y, and e_z are normalized.
00179 //  <p>
00180 //  If type == E_YZ, the vectors passed in are assumed to lie in the
00181 //  y-z plane with v0 an y-like vector and v1 a z-like vector;
00182 //  e_y is v0.  e_x is constructed from e_y cross v1.
00183 //  e_z is constructed from e_x cross e_y.  The three vectors 
00184 //  e_x, e_y, and e_z are normalized.
00185 //  <p>
00186 //  If type == E_ZX, the vectors passed in are assumed to lie in the
00187 //  x-z plane with v0 an z-like vector and v1 a x-like vector;
00188 //  e_z is v0.  e_y is constructed from e_z cross v1.
00189 //  e_x is constructed from e_y cross e_z.  The three vectors 
00190 //  e_x, e_y, and e_z are normalized.
00191 //  <p>
00192 //  If type == E_YX, the vectors passed in are assumed to lie in the
00193 //  x-y plane with v0 a y-like vector and v1 an x-like vector;
00194 //  e_y is v0.  e_z is constructed from v1 cross e_y.
00195 //  e_x is constructed from e_y cross e_z.  The three vectors 
00196 //  e_x, e_y, and e_z are normalized.
00197 //  <p>
00198 //  If type == E_ZY, the vectors passed in are assumed to lie in the
00199 //  y-z plane with v0 a z-like vector and v1 a y-like vector;
00200 //  e_z is v0.  e_x is constructed from v1 cross e_z.
00201 //  e_y is constructed from e_z cross e_x.  The three vectors 
00202 //  e_x, e_y, and e_z are normalized.
00203 //  <p>
00204 //  If type == E_XZ, the vectors passed in are assumed to lie in the
00205 //  x-z plane with v0 an x-like vector and v1 a z-like vector;
00206 //  e_x is v0.  e_y is constructed from v1 cross e_x.
00207 //  e_z is constructed from e_x cross e_y.  The three vectors 
00208 //  e_x, e_y, and e_z are normalized.
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 // initializers...
00221 //#########################################################################
00222 
00223 //=========================================================================
00224 //
00225 // Initialize a rotation matrix based on 3 Euler angles; the type of Euler
00226 // angles is specified by angle_type.
00227 //
00228 // The matrix generated by this routine does the opposite
00229 // of what the original rigid body motion did to the body
00230 // centered coordinate system.  in other words, it does
00231 // what is required to a location or direction to transform
00232 // it from std to bcs.
00233 //
00234 // NOTE:  arguments are in radians.
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 //: Initialize a rotation matrix based on two (non-colinear) vectors.
00344 //  The rotation matrix is constructed by sequential application
00345 //  cross products.
00346 //  <p>
00347 //  If type == E_XY, the vectors passed in are assumed to lie in the
00348 //  x-y plane with v0 an x-like vector and v1 a y-like vector;
00349 //  e_x is v0.  e_z is constructed from e_x cross v1.
00350 //  e_y is constructed from e_z cross e_x.  The three vectors 
00351 //  e_x, e_y, and e_z are normalized.
00352 //  <p>
00353 //  If type == E_YZ, the vectors passed in are assumed to lie in the
00354 //  y-z plane with v0 an y-like vector and v1 a z-like vector;
00355 //  e_y is v0.  e_x is constructed from e_y cross v1.
00356 //  e_z is constructed from e_x cross e_y.  The three vectors 
00357 //  e_x, e_y, and e_z are normalized.
00358 //  <p>
00359 //  If type == E_ZX, the vectors passed in are assumed to lie in the
00360 //  x-z plane with v0 an z-like vector and v1 a x-like vector;
00361 //  e_z is v0.  e_y is constructed from e_z cross v1.
00362 //  e_x is constructed from e_y cross e_z.  The three vectors 
00363 //  e_x, e_y, and e_z are normalized.
00364 //  <p>
00365 //  If type == E_YX, the vectors passed in are assumed to lie in the
00366 //  x-y plane with v0 a y-like vector and v1 an x-like vector;
00367 //  e_y is v0.  e_z is constructed from v1 cross e_y.
00368 //  e_x is constructed from e_y cross e_z.  The three vectors 
00369 //  e_x, e_y, and e_z are normalized.
00370 //  <p>
00371 //  If type == E_ZY, the vectors passed in are assumed to lie in the
00372 //  y-z plane with v0 a z-like vector and v1 a y-like vector;
00373 //  e_z is v0.  e_x is constructed from v1 cross e_z.
00374 //  e_y is constructed from e_z cross e_x.  The three vectors 
00375 //  e_x, e_y, and e_z are normalized.
00376 //  <p>
00377 //  If type == E_XZ, the vectors passed in are assumed to lie in the
00378 //  x-z plane with v0 an x-like vector and v1 a z-like vector;
00379 //  e_x is v0.  e_y is constructed from v1 cross e_x.
00380 //  e_z is constructed from e_x cross e_y.  The three vectors 
00381 //  e_x, e_y, and e_z are normalized.
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 //: Initialize a rotation matrix based on two (non-colinear) vectors.
00462 //  The rotation matrix is constructed by sequential application
00463 //  cross products.
00464 //  <p>
00465 //  If type == E_XY, the vectors passed in are assumed to lie in the
00466 //  x-y plane with v0 an x-like vector and v1 a y-like vector;
00467 //  e_x is v0.  e_z is constructed from e_x cross v1.
00468 //  e_y is constructed from e_z cross e_x.  The three vectors 
00469 //  e_x, e_y, and e_z are normalized.
00470 //  <p>
00471 //  If type == E_YZ, the vectors passed in are assumed to lie in the
00472 //  y-z plane with v0 an y-like vector and v1 a z-like vector;
00473 //  e_y is v0.  e_x is constructed from e_y cross v1.
00474 //  e_z is constructed from e_x cross e_y.  The three vectors 
00475 //  e_x, e_y, and e_z are normalized.
00476 //  <p>
00477 //  If type == E_ZX, the vectors passed in are assumed to lie in the
00478 //  x-z plane with v0 an z-like vector and v1 a x-like vector;
00479 //  e_z is v0.  e_y is constructed from e_z cross v1.
00480 //  e_x is constructed from e_y cross e_z.  The three vectors 
00481 //  e_x, e_y, and e_z are normalized.
00482 //  <p>
00483 //  If type == E_YX, the vectors passed in are assumed to lie in the
00484 //  x-y plane with v0 a y-like vector and v1 an x-like vector;
00485 //  e_y is v0.  e_z is constructed from v1 cross e_y.
00486 //  e_x is constructed from e_y cross e_z.  The three vectors 
00487 //  e_x, e_y, and e_z are normalized.
00488 //  <p>
00489 //  If type == E_ZY, the vectors passed in are assumed to lie in the
00490 //  y-z plane with v0 a z-like vector and v1 a y-like vector;
00491 //  e_z is v0.  e_x is constructed from v1 cross e_z.
00492 //  e_y is constructed from e_z cross e_x.  The three vectors 
00493 //  e_x, e_y, and e_z are normalized.
00494 //  <p>
00495 //  If type == E_XZ, the vectors passed in are assumed to lie in the
00496 //  x-z plane with v0 an x-like vector and v1 a z-like vector;
00497 //  e_x is v0.  e_y is constructed from v1 cross e_x.
00498 //  e_z is constructed from e_x cross e_y.  The three vectors 
00499 //  e_x, e_y, and e_z are normalized.
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 // mutators
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 // boolean functions
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 // std::ostream&
00615 // operator<<( std::ostream& os, const dvm3_RotMat& m )
00616 // {
00617 //   return m.print_on( os, "[", "]" );
00618 // }

Generated on Tue Dec 2 15:44:47 2008 for dvm3 by  doxygen 1.5.6