QuOc

 

smallMat.h

Go to the documentation of this file.
00001 /* Copyright (c) 2001-2010 AG Rumpf, INS, Universitaet Bonn                      */
00002 /*                                                                               */
00003 /* The contents of this file are subject to the terms of the Common Development  */
00004 /* and Distribution License Version 1.0 (the "License"); you may not use         */
00005 /* this file except in compliance with the License. You may obtain a copy of     */
00006 /* the License at http://www.sun.com/cddl/                                       */
00007 /*                                                                               */
00008 /* Software distributed under the License is distributed on an "AS IS" basis,    */
00009 /* WITHOUT WARRANTY OF ANY KIND, either express or implied.                      */
00010 /*                                                                               */
00011 #ifndef __SMALLMAT_H
00012 #define __SMALLMAT_H
00013 
00014 #include <aol.h>
00015 #include <vec.h>
00016 #include <smallVec.h>
00017 
00018 namespace aol {
00019 
00020 //! A Matrix of any x- and y- dimension (template-parameters)
00021 //! derived from the vec-class (no use of STL-vectors) (ON)
00022 //! @ingroup Matrix
00023 template <int numRows, int numCols, typename _DataType>
00024 class Mat {
00025 protected:
00026   Vec<numCols, _DataType> _row[numRows];
00027 
00028 public:
00029   typedef _DataType DataType;
00030 
00031   //! Constructor
00032   Mat() {
00033     for ( int i = 0; i < numRows; ++i )
00034       for ( int j = 0; j < numCols; ++j )
00035         this->_row[i][j] = ZOTrait<_DataType>::zero;
00036   }
00037 
00038   //! Copy-constructor
00039   Mat ( const Mat<numRows, numCols, _DataType> &rhs ) {
00040     for ( int i = 0; i < numRows; ++i )
00041       for ( int j = 0; j < numCols; ++j )
00042         this->_row[i][j] = rhs._row[i][j];
00043   }
00044 
00045   //! Copy-constructor for structure or data copying
00046   Mat ( const Mat<numRows, numCols, _DataType> &rhs, CopyFlag copyFlag ) {
00047     switch ( copyFlag ) {
00048       case DEEP_COPY:
00049         for ( int i = 0; i < numRows; ++i )
00050           for ( int j = 0; j < numCols; ++j )
00051             this->_row[i][j] = rhs._row[i][j];
00052         break;
00053       case STRUCT_COPY:
00054         for ( int i = 0; i < numRows; ++i )
00055           for ( int j = 0; j < numCols; ++j )
00056             this->_row[i][j] = ZOTrait<DataType>::zero;
00057         break;
00058       default:
00059         string errorMessage = strprintf( "Copying a Mat is not possible with copyFlag=%d", copyFlag );
00060         throw Exception( errorMessage, __FILE__, __LINE__);
00061     }
00062   }
00063 
00064   // ---------------------------------------------
00065 
00066   //! operator=
00067   Mat<numRows, numCols, _DataType>& operator= ( const Mat<numRows, numCols, _DataType> &rhs ) {
00068     for ( int i = 0; i < numRows; ++i )
00069       for ( int j = 0; j < numCols; ++j )
00070         this->_row[i][j] = rhs._row[i][j];
00071     return *this;
00072   }
00073 
00074   Vec<numCols, _DataType>&         operator[] ( const int i )       {
00075 #ifdef BOUNDS_CHECK
00076     rowBoundsCheck ( i, __FILE__, __LINE__ );
00077 #endif
00078     return this->_row[i];
00079   }
00080 
00081   const Vec<numCols, _DataType>&   operator[] ( const int i ) const {
00082 #ifdef BOUNDS_CHECK
00083     rowBoundsCheck ( i, __FILE__, __LINE__ );
00084 #endif
00085     return this->_row[i];
00086   }
00087 
00088   _DataType get ( const int I, const int J ) const           {
00089 #ifdef BOUNDS_CHECK
00090     rowBoundsCheck ( I, __FILE__, __LINE__ );
00091 #endif
00092     return this->_row[I][J];
00093   }
00094   void set ( const int I, const int J, _DataType Value ) {
00095 #ifdef BOUNDS_CHECK
00096     rowBoundsCheck ( I, __FILE__, __LINE__ );
00097 #endif
00098     this->_row[I][J] = Value;
00099   }
00100 
00101   _DataType getDiag ( const int I ) {
00102 #ifdef BOUNDS_CHECK
00103     rowBoundsCheck ( I, __FILE__, __LINE__ );
00104 #endif
00105     return get(I, I);
00106   }
00107 
00108   int getNumCols() const { return numCols; }
00109   int getNumRows()    const { return numRows; }
00110 
00111   //! equality comparison of two Mats
00112   bool operator== ( const Mat<numRows, numCols, _DataType> &other ) const {
00113     bool ret = true;
00114     for ( int i = 0; i < numRows; ++i )
00115       for ( int j = 0; j < numCols; ++j )
00116         ret &= (*this)[i][j] == other[i][j];
00117     return ( ret );
00118   }
00119 
00120   //! inequality comparison of two Mats
00121   bool operator!= ( const Mat<numRows, numCols, _DataType> &other ) const {
00122     return ( ! ( *this == other ) );
00123   }
00124 
00125   void add ( const int I, const int J, const _DataType Value ) {
00126 #ifdef BOUNDS_CHECK
00127     rowBoundsCheck ( I, __FILE__, __LINE__ );
00128 #endif
00129 
00130     this->_row[I][J] += Value;
00131   }
00132 
00133   void setZero( ) {
00134     for ( int i = 0; i < numRows; ++i )
00135       this->_row[i].setZero();
00136   }
00137 
00138   void setIdentity() {
00139     QUOC_ASSERT(numRows == numCols);
00140     setZero();
00141     for (int i = 0; i < numRows; ++i)
00142       (*this)[i][i] = 1.;
00143   }
00144 
00145   template <class T>
00146   void setRow ( const int Index, const Vec<numCols, T> &Vec ) {
00147 #ifdef BOUNDS_CHECK
00148     rowBoundsCheck ( Index, __FILE__, __LINE__ );
00149 #endif
00150     for ( int j = 0; j < numCols; ++j )
00151       this->_row[Index][j] = static_cast<T> ( Vec.get ( j ) );
00152   }
00153 
00154   template <class T>
00155   void getRow ( const int Index, Vec<numCols, T> &Dest ) const {
00156 #ifdef BOUNDS_CHECK
00157     rowBoundsCheck ( Index, __FILE__, __LINE__ );
00158 #endif
00159     for ( int j = 0; j < numCols; ++j )
00160       Dest[j] = static_cast<T> ( this->_row[Index][j] );
00161   }
00162 
00163   template <class T>
00164   void getRow ( const int Index, Vector<T> &Dest ) const {
00165 #ifdef BOUNDS_CHECK
00166     rowBoundsCheck ( Index, __FILE__, __LINE__ );
00167 #endif
00168     for ( int j = 0; j < numCols; ++j )
00169       Dest[j] = static_cast<T> ( this->_row[Index][j] );
00170   }
00171 
00172   void scaleRow ( const int Index, const DataType Factor ) { 
00173 #ifdef BOUNDS_CHECK
00174     rowBoundsCheck ( Index, __FILE__, __LINE__ );
00175 #endif
00176     for ( int j = 0; j < numCols; ++j )
00177       this->_row[Index][j] *= Factor;
00178   }
00179 
00180   template <class T>
00181   void setCol ( const int Index, const Vec<numRows, T> &Vec ) {
00182     for ( int j = 0; j < numRows; ++j )
00183       this->_row[j][Index] = static_cast<T> ( Vec.get ( j ) );
00184   }
00185 
00186   template <class T>
00187   void getCol ( const int Index, Vec<numRows, T> &Dest ) const {
00188     for ( int j = 0; j < numRows; ++j )
00189       Dest[j] = static_cast<T> ( this->_row[j][Index] );
00190   }
00191 
00192   template <class T>
00193   void getCol ( const int Index, Vector<T> &Dest ) const {
00194     for ( int j = 0; j < numRows; ++j )
00195       Dest[j] = static_cast<T> ( this->_row[j][Index] );
00196   }
00197 
00198   template <class T>
00199   void getColumn ( const int Index, Vec<numRows, T> &Dest ) const {
00200     getCol(Index, Dest);
00201   }
00202 
00203   template <class T>
00204   void getColumn ( const int Index, Vector<T> &Dest ) const {
00205     getCol(Index, Dest);
00206   }
00207 
00208   template <class T>
00209   void getSubColumn ( int startRow, int col, Vector<T> & dest) const {
00210     QUOC_ASSERT(startRow + dest.size() <= numRows);
00211     for (int i = 0; i < dest.size(); ++i)
00212       dest[i] = (*this)[startRow + i][col];
00213   }
00214 
00215   template <class T, int size>
00216   void getSubColumn ( int startRow, int col, Vec<size, T> & dest) const {
00217     QUOC_ASSERT(startRow + size <= numRows);
00218     for (int i = 0; i < size; ++i)
00219       dest[i] = (*this)[startRow + i][col];
00220   }
00221 
00222   template <class T>
00223   void getSubRow ( int row, int startCol, Vector<T> & dest) const {
00224     QUOC_ASSERT(startCol + dest.size() <= numCols);
00225     for (int i = 0; i < dest.size(); ++i)
00226       dest[i] = (*this)[row][startCol + i];
00227   }
00228 
00229   template <class T, int size>
00230   void getSubRow ( int row, int startCol, Vec<size, T> & dest) const {
00231     QUOC_ASSERT(startCol + size <= numCols);
00232     for (int i = 0; i < size; ++i)
00233       dest[i] = (*this)[row][startCol + i];
00234   }
00235 
00236   template <class T>
00237   void setSubColumn ( int startRow, int col, const Vector<T> & subCol) {
00238     QUOC_ASSERT(startRow + subCol.size() <= numRows);
00239     for (int i = 0; i < subCol.size(); ++i)
00240       (*this)[startRow + i][col] = subCol[i];
00241   }
00242 
00243   template <class T, int size>
00244   void setSubColumn ( int startRow, int col, const Vec<size, T> & subCol) {
00245     QUOC_ASSERT(startRow + size <= numRows);
00246     for (int i = 0; i < size; ++i)
00247       (*this)[startRow + i][col] = subCol[i];
00248   }
00249 
00250   template <class T>
00251   void setSubRow ( int row, int startCol, const Vector<T> & subRow) {
00252     QUOC_ASSERT(startCol + subRow.size() <= numCols);
00253     for (int i = 0; i < subRow.size(); ++i)
00254       (*this)[row][startCol + i] = subRow[i];
00255   }
00256 
00257   template <class T, int size>
00258   void setSubRow ( int row, int startCol, const Vec<size, T> & subRow) {
00259     QUOC_ASSERT(startCol + size <= numCols);
00260     for (int i = 0; i < size; ++i)
00261       (*this)[row][startCol + i] = subRow[i];
00262   }
00263 
00264   template <class T, int rowsSubMatrix, int colsSubMatrix>
00265   void getSubMatrix ( int startRow, int startCol, Mat<rowsSubMatrix,colsSubMatrix,T> & subMatrix) const {
00266     QUOC_ASSERT(startRow + rowsSubMatrix <= numRows);
00267     QUOC_ASSERT(startCol + colsSubMatrix <= numCols);
00268     for (int i = 0; i < rowsSubMatrix; ++i)
00269       for (int j = 0; j < colsSubMatrix; ++j)
00270         subMatrix[i][j] = (*this)[startRow + i][startCol + j];
00271   }
00272 
00273   template <class T, int rowsSubMatrix, int colsSubMatrix>
00274   void setSubMatrix ( int startRow, int startCol, const Mat<rowsSubMatrix,colsSubMatrix,T> & subMatrix) {
00275     QUOC_ASSERT(startRow + rowsSubMatrix <= numRows);
00276     QUOC_ASSERT(startCol + colsSubMatrix <= numCols);
00277     for (int i = 0; i < rowsSubMatrix; ++i)
00278       for (int j = 0; j < colsSubMatrix; ++j)
00279         (*this)[startRow + i][startCol + j] = subMatrix[i][j];
00280   }
00281 
00282   template <class T>
00283   void multAdd ( const Vec<numCols, T> &Arg, Vec<numRows, T> &Dest ) const {
00284     for ( int i = 0; i < numRows; ++i ) {
00285       for ( int j = 0; j < numCols; ++j )
00286         Dest[i] += this->_row[i][j] * Arg.get ( j );
00287     }
00288   }
00289 
00290   //! \f$ A \mapsto \frac{1}{2}(A + A^T) \f$
00291   void symmetrize ( ) {
00292     if( numRows != numCols )
00293       throw aol::Exception( "symmetrize needs numRows == numCols!", __FILE__, __LINE__ );
00294     for ( int i = 1; i < numRows; ++i )
00295       for ( int j = 0; j < i; ++j )
00296         (*this) [i][j] = (*this) [j][i] = 0.5 * ( (*this) [i][j] + (*this) [j][i] );
00297   }
00298 
00299   bool isExactlyDiagonal () const {
00300     bool ret = true;
00301     for ( int i = 0; i < numRows; ++i )
00302       for ( int j = 0; j < numCols; ++j )
00303         if ( i != j && this->_row[i][j] != ZOTrait<_DataType>::zero )
00304           ret = false;
00305     return ret;
00306   }
00307 
00308   //! \f$ x \mapsto Ax \f$
00309   //
00310   template <class T>
00311   void mult ( const Vec<numCols, T> &Arg, Vec<numRows, T> &Dest ) const {
00312     Dest.setZero();
00313     multAdd ( Arg, Dest );
00314   }
00315 
00316   //! \f$ x \mapsto A^Tx \f$
00317   //
00318   template <class T>
00319   void multTransposed ( const Vec<numRows, T> &Arg, Vec<numCols, T> &Dest ) const {
00320     Dest.setZero();
00321     for ( int i = 0; i < numCols; ++i ) {
00322       for ( int j = 0; j < numRows; ++j )
00323         Dest[i] += this->_row[j][i] * Arg.get ( j );
00324     }
00325   }
00326 
00327   void elimRowColTo ( const int Row, const int Col, Mat < numRows - 1, numCols - 1, _DataType > &subMat ) {
00328     for ( int row = 0; row < Row; ++row ) {
00329       for ( int col = 0; col < Col; ++col ) { 
00330         subMat[row][col] = ( *this ) [row][col];
00331       }
00332       for ( int col = Col; col < numCols - 1; ++col ) {
00333         subMat[row][col] = ( *this ) [row][col+1];
00334       }
00335     }
00336     for ( int row = Row; row < numRows - 1; ++row ) {
00337       for ( int col = 0; col < Col; ++col ) { 
00338         subMat[row][col] = ( *this ) [row+1][col];
00339       }
00340       for ( int col = Col; col < numCols - 1; ++col ) {
00341         subMat[row][col] = ( *this ) [row+1][col+1];
00342       }
00343     }
00344   }
00345 
00346   //! \f$ A \mapsto A + B \f$
00347   Mat<numRows, numCols, _DataType> &operator+= ( const Mat<numRows, numCols, _DataType> &Other ) {
00348     for ( int i = 0; i < numRows; ++i ) {
00349       for ( int j = 0; j < numCols; ++j ) {
00350         this->_row[i][j] += Other._row[i][j];
00351       }
00352     }
00353     return *this;
00354   }
00355 
00356   //! \f$ A \mapsto A - B \f$
00357   Mat<numRows, numCols, _DataType> &operator-= ( const Mat<numRows, numCols, _DataType> &Other ) {
00358     for ( int i = 0; i < numRows; ++i ) {
00359       for ( int j = 0; j < numCols; ++j ) {
00360         this->_row[i][j] -= Other._row[i][j];
00361       }
00362     }
00363     return *this;
00364   }
00365 
00366   //! ":"-Product: returns *this : mat
00367   _DataType ddprod ( const Mat<numRows, numCols, _DataType>& mat ) const {
00368     _DataType result = 0;
00369     for ( int i = 0; i < numRows; ++i ) {
00370       for ( int j = 0; j < numCols; ++j ) {
00371         result += this->_row[i][j] * mat._row[i][j];
00372       }
00373     }
00374     return result;
00375   }
00376 
00377   //! Frobenius dot product
00378   _DataType dotProduct ( const Mat<numRows, numCols, _DataType>& mat ) const {
00379     return this->ddprod( mat );
00380   }
00381 
00382   //! ":^T"-Product : returns *this : mat^T
00383   _DataType ddprodT ( const Mat<numRows, numCols, _DataType>& mat ) const {
00384     _DataType result = 0;
00385     for ( int i = 0; i < numRows; ++i ) {
00386       for ( int j = 0; j < numCols; ++j ) {
00387         result += this->_row[i][j] * mat._row[j][i];
00388       }
00389     }
00390     return result;
00391   }
00392 
00393   Vec<numRows, _DataType> operator * ( const Vec<numCols, _DataType>& vec ) const {
00394     Vec<numRows, _DataType> Res;
00395     for ( int i = 0; i < numRows; ++i )
00396       for ( int j = 0; j < numCols; ++j )
00397         Res[i] += this->_row[i][j] * vec[j];
00398 
00399     return Res;
00400   }
00401 
00402   Vector<_DataType> operator * ( const Vector<_DataType>& vec ) const {
00403     Vector<_DataType> Res(numRows);
00404     for ( int i = 0; i < numRows; ++i )
00405       for ( int j = 0; j < numCols; ++j )
00406         Res[i] += this->_row[i][j] * vec[j];
00407 
00408     return Res;
00409   }
00410 
00411   //! Writes the transpose of this to itself. See the other transpose* methods
00412   //! for the naming convention of data im/export
00413   void transpose( ) {
00414     inplaceTranspose ( *this );
00415   }
00416 
00417 
00418   // writes the transpose of mat to this Mat
00419   void transposeFrom ( const Mat<numCols, numRows, _DataType> &mat ) {
00420     for ( int i = 0; i < numRows; ++i )
00421       for ( int j = 0; j < numCols; ++j )
00422         (*this)[i][j] = mat[j][i];
00423   }
00424 
00425   // writes the transpose of this Mat to mat
00426   void transposeTo ( Mat<numCols, numRows, _DataType> &mat ) const {
00427     for ( int i = 0; i < numCols; ++i )
00428       for ( int j = 0; j < numRows; ++j )
00429         mat[i][j] = (*this)[j][i];
00430   }
00431 
00432   // returns a Mat which is the transpose of this Mat
00433   Mat<numCols, numRows, _DataType> transposed ( ) const {
00434     Mat<numCols, numRows, _DataType> ret;
00435     this->transposeTo ( ret );
00436     return ret;
00437   }
00438 
00439   //! \f$ A \mapsto \alpha A \f$
00440   Mat<numRows, numCols, _DataType> &operator*= ( const _DataType Alpha ) {
00441     for ( int i = 0; i < numRows; ++i )
00442       this->_row[i] *= Alpha;
00443     return *this;
00444   }
00445 
00446   //! \f$ A \mapsto \alpha^{-1} A \f$
00447   Mat<numRows, numCols, _DataType> &operator/= ( const _DataType Alpha ) {
00448     for ( int i = 0; i < numRows; ++i )
00449       this->_row[i] /= Alpha;
00450     return *this;
00451   }
00452 
00453   void makeTensorProduct ( const Vec<numRows, _DataType> &A, const Vec<numCols, _DataType> &B ) {
00454     for ( int i = 0; i < numRows; ++i )
00455       for ( int j = 0; j < numCols; ++j )
00456         this->_row[i][j] = A[i] * B[j];
00457   }
00458 
00459   //! \f$ A = I-(a\otimes a) \f$
00460   void makeProjectionMatrix ( const Vec<numRows, _DataType> &A ) {
00461 #ifdef BOUNDS_CHECK
00462   if( numRows != numCols )
00463     throw aol::Exception( "makeProjectionMatrix needs numRows == numCols!", __FILE__, __LINE__ );
00464 #endif
00465     for ( int i = 0; i < numRows; ++i ){
00466       this->_row[i][i] = aol::ZOTrait<_DataType>::one - aol::Sqr( A[i] );
00467 
00468       for( int j = i+1; j < numCols; ++j){
00469         this->_row[i][j] = - A[i] * A[j];
00470       }
00471 
00472       for( int j = 0; j < i; ++j){
00473         this->_row[i][j] = this->_row[j][i];
00474       }
00475     }
00476   }
00477 
00478 
00479   template <class AnotherType, int dimBoth>
00480   void makeProduct ( const Mat<numRows, dimBoth, AnotherType> &A,
00481                      const Mat<dimBoth, numCols, AnotherType> &B ) {
00482     setZero( );
00483     for ( int i = 0; i < numRows; ++i ) {
00484       for ( int j = 0; j < numCols; ++j ) {
00485         for ( int k = 0; k < dimBoth; ++k ) {
00486           this->_row[i][j] += static_cast<_DataType> ( A[i][k] * B[k][j] );
00487         }
00488       }
00489     }
00490   }
00491 
00492   template <class AnotherType, int dimBoth>
00493   void makeProductAtransposedB ( const Mat<dimBoth, numRows, AnotherType> &A,
00494                                  const Mat<dimBoth, numCols, AnotherType> &B ) {
00495     setZero( );
00496     for ( int i = 0; i < numRows; ++i ) {
00497       for ( int j = 0; j < numCols; ++j ) {
00498         for ( int k = 0; k < dimBoth; ++k ) {
00499           this->_row[i][j] += static_cast<_DataType> ( A[k][i] * B[k][j] );
00500         }
00501       }
00502     }
00503   }
00504 
00505   template <class AnotherType, int dimBoth>
00506   void makeProductABtransposed ( const Mat<numRows, dimBoth, AnotherType> &A,
00507                                  const Mat<numCols, dimBoth, AnotherType> &B ) {
00508     setZero( );
00509     for ( int i = 0; i < numRows; ++i ) {
00510       for ( int j = 0; j < numCols; ++j ) {
00511         for ( int k = 0; k < dimBoth; ++k ) {
00512           this->_row[i][j] += static_cast<_DataType> ( A[i][k] * B[j][k] );
00513         }
00514       }
00515     }
00516   }
00517 
00518   //! \f$ \mbox{*this} \mapsto \frac{1}{2}(A^{T}B + B^{T}A) \f$
00519   template <class AnotherType, int dimBoth>
00520   void makeSymmetricProduct ( const Mat<dimBoth, numRows, AnotherType> &A,
00521                                  const Mat<dimBoth, numCols, AnotherType> &B ) {
00522     if ( numCols != numRows ) {
00523       throw aol::Exception ( "Mat<...>::makeSymmetricProduct works only for quadratic matrices", __FILE__, __LINE__ );
00524     }
00525     setZero( );
00526     Mat<numRows, numCols, _DataType> aux;
00527     aux.makeProductAtransposedB( A, B );
00528     makeProductAtransposedB( B, A );
00529     (*this)+=aux;
00530     (*this)*=.5; 
00531   }
00532 
00533   //! dagger product: returns *this dagger A
00534   //! with M dagger A = (M M^T)^-1 M A^T
00535   Mat<numRows, numRows, _DataType> daggerProduct(const Mat<numRows, numCols, _DataType> & A) const
00536   {
00537     Mat<numRows, numRows, _DataType> MMT;
00538     MMT.makeProductABtransposed(*this, *this);
00539     Mat<numRows, numRows, _DataType> MMTinv;
00540     MMTinv.makeInverse(MMT);
00541 
00542     // abuse MMT for *this * A^T
00543     MMT.makeProductABtransposed(*this, A);
00544 
00545     Mat<numRows, numRows, _DataType> erg;
00546     erg.makeProduct(MMTinv, MMT);
00547     return erg;
00548   }
00549 
00550   //! sharp product: returns *this sharp A
00551   //! with M sharp A = (M M^T)^-1 A M^T
00552   Mat<numRows, numRows, _DataType> sharpProduct(const Mat<numRows, numCols, _DataType> & A) const
00553   {
00554     Mat<numRows, numRows, _DataType> MMT;
00555     MMT.makeProductABtransposed(*this, *this);
00556     Mat<numRows, numRows, _DataType> MMTinv;
00557     MMTinv.makeInverse(MMT);
00558 
00559     // abuse MMT for *this * A^T
00560     MMT.makeProductABtransposed(A, *this);
00561 
00562     Mat<numRows, numRows, _DataType> erg;
00563     erg.makeProduct(MMTinv, MMT);
00564     return erg;
00565   }
00566 
00567   _DataType normSqr() const {
00568     _DataType res = 0;
00569     for ( int i = 0; i < numRows; ++i ) {
00570       res += this->_row[i].normSqr();
00571     }
00572     return res;
00573   }
00574 
00575   /**
00576    * Frobenius norm.
00577    *
00578    * \author Wirth
00579    */
00580   _DataType norm() const {
00581     return sqrt( this->normSqr() );
00582   }
00583 
00584   _DataType infinityNorm() const {
00585     _DataType res = 0;
00586     for ( int i = 0; i < numRows; ++i ) {
00587       _DataType temp = 0;
00588       for ( int j = 0; j < numCols; ++j ) {
00589         temp += Abs ( this->_row[i][j] );
00590       }
00591       if ( temp > res ) res = temp;
00592     }
00593     return ( res );
00594   }
00595 
00596 
00597   ostream& print ( ostream& out ) const {
00598     for ( int i = 0; i < numRows - 1; ++i )
00599       out << this->_row[i] << endl;
00600     out << this->_row[numRows-1];
00601     return ( out );
00602   }
00603 
00604   istream& read ( istream& in ) {
00605     for ( int i = 0; i < numRows; ++i )
00606       in >> this->_row [i];
00607     return in;
00608   }
00609 
00610   //! swap two rows
00611   void swapRows ( const int i, const int j ) {
00612     _DataType tmp;
00613     for ( int l = 0; l < numCols; ++l ) {
00614       tmp = this->_row[i][l];
00615       this->_row[i][l] = this->_row[j][l];
00616       this->_row[j][l] = tmp;
00617     }
00618   }
00619 
00620   //! operator *=
00621   Mat<numRows, numCols, _DataType> &operator*= ( const Mat<numCols, numCols, _DataType>& mat ) {
00622     if ( static_cast<const void*>(this) == static_cast<const void*>(&mat) ) {
00623       cerr << "Matrix<_DataType>::operator*= :  don't multiply with the same matrix!" << endl;
00624     } else {
00625       aol::Vec<numCols, _DataType> vec;
00626       int i, j, k ;
00627       _DataType val;
00628       for ( i = 0; i < numRows ; ++i ) {
00629         vec = _row[i];
00630 
00631         for ( j = 0; j < numCols ; ++j ) {
00632           val = 0;
00633           for ( k = 0; k < numCols; ++k ) {
00634             // Note: vec[k] is a copy of this->_row[i][k]
00635             val += mat[k][j] * vec[k];
00636           }
00637           _row[i][j] = val;
00638         }
00639       }
00640     }
00641     return *this;
00642   }
00643 
00644   //! \f$ A \mapsto BA \f$
00645   void leftMult ( const Mat<numRows, numRows, _DataType>& mat ) {
00646     if ( static_cast<const void*>(this) == static_cast<const void*>(&mat) ) 
00647       cerr << "Matrix<_DataType>::leftMult :  don't multiply with the same matrix!" << endl;
00648     else {
00649       aol::Vec<numRows, _DataType> vec;
00650       for ( int j = 0; j < numCols; ++j ) {
00651         // save j-th column in vec
00652         for ( int k = 0; k < numRows; ++k )
00653           vec[k] = _row[k][j];
00654         for ( int i = 0; i < numRows ; ++i ) {
00655           _row[i][j] = 0;
00656           for ( int k = 0; k < numRows; ++k ) 
00657             _row[i][j] += mat[i][k] * vec[k];
00658         }
00659       }
00660     }
00661   }
00662 
00663   //! \f$ A \mapsto A + \alpha B \f$
00664   Mat<numRows, numCols, _DataType> & addMultiple ( const Mat<numRows, numRows, _DataType>& mat, const _DataType& alpha ) {
00665     if ( static_cast<const void*>(this) == static_cast<const void*>(&mat) ) 
00666       cerr << "Matrix<_DataType>::addMultiple :  don't add the same matrix!" << endl;
00667     else {
00668       for ( int j = 0; j < numCols; ++j ) 
00669         for ( int i = 0; i < numRows ; ++i ) 
00670             _row[i][j] += alpha * mat[i][j];
00671     }
00672     return *this;
00673   }
00674 
00675 
00676   /** computes the inverse of the given argument matrix and stores it into this matrix.
00677    *  This method uses the Gauss-Jacobi Algorithm and throws an exception if the matrix
00678    *  is not invertible
00679    */
00680   void makeInverse ( const Mat<numRows, numCols, _DataType> &a ) {
00681     if ( numCols != numRows ) {
00682       throw aol::Exception ( "Mat<...>::makeInverse works only for quadratic matrices", __FILE__, __LINE__ );
00683     }
00684 
00685     const int N = numCols;
00686     int  indxc[numCols], indxr[numCols], ipiv[numCols] = { 0 };
00687     int  i, icol = 0, irow = 0, j, k, l, ll;
00688     _DataType big, dum, pivinv;
00689 
00690     ( *this ) = a;
00691 
00692     for ( i = 0; i < N; ++i ) {
00693       big = 0.0;
00694       for ( j = 0; j < N; ++j )
00695         if ( ipiv[j] != 1 ) {
00696           for ( k = 0; k < N; ++k ) {
00697             if ( ipiv[k] == 0 ) {
00698               if ( fabs ( this->_row[j][k] ) >= big ) {
00699                 big = fabs ( this->_row[j][k] );
00700                 irow = j;
00701                 icol = k;
00702               }
00703             } else
00704               if ( ipiv[k] > 1 ) throw aol::Exception ( "Mat<...>::makeInverse: Singular Matrix-1", __FILE__, __LINE__ );
00705           }
00706         }
00707       ++ ( ipiv[icol] );
00708       if ( irow != icol ) {
00709         for ( l = 0; l < N; ++l ) std::swap ( this->_row[irow][l], this->_row[icol][l] );
00710       }
00711       indxr[i] = irow;
00712       indxc[i] = icol;
00713       if ( this->_row[icol][icol] == 0.0 ) throw aol::Exception ( "Mat<...>::makeInverse: Singular Matrix-2", __FILE__, __LINE__ );
00714       pivinv = aol::NumberTrait<_DataType>::one / this->_row[icol][icol];
00715       this->_row[icol][icol] = 1.0;
00716 
00717       for ( l = 0; l < N; ++l ) this->_row[icol][l] *= pivinv;
00718       for ( ll = 0; ll < N; ++ll )
00719         if ( ll != icol ) {
00720           dum = this->_row[ll][icol];
00721           this->_row[ll][icol] = 0.0;
00722           for ( l = 0; l < N; ++l ) this->_row[ll][l] -= this->_row[icol][l] * dum;
00723         }
00724     }
00725     for ( l = N - 1; l >= 0; l-- ) {
00726       if ( indxr[l] != indxc[l] )
00727         for ( k = 0; k < N; ++k )
00728           std::swap ( this->_row[k][indxr[l]], this->_row[k][indxc[l]] );
00729     }
00730   }
00731 
00732 protected:
00733 #ifdef BOUNDS_CHECK
00734   inline void rowBoundsCheck ( const int i, const char* fi, const int li ) const {
00735     if ( i < 0 || i >= numRows ) {
00736       char errmsg[1024];
00737       sprintf( errmsg, "aol::Mat: index %d out of bounds (%d)", i, numRows );
00738       throw aol::OutOfBoundsException ( errmsg, fi, li );
00739     }
00740   }
00741 #endif
00742 
00743 
00744 };
00745 
00746 //! transpose symmetrix Mat<N,N, _DataType> to itself (in place)
00747 template <int N, typename _DataType>
00748 void inplaceTranspose ( aol::Mat<N, N, _DataType> &mat ) {
00749   for ( int i = 0; i < N; ++i ) {
00750     for ( int j = i + 1; j < N; ++j ) {
00751       const _DataType t = mat[i][j];
00752       mat[i][j] = mat[j][i];
00753       mat[j][i] = t;
00754     }
00755   }
00756 }
00757 
00758 
00759 //////////////////////////////////////////////////////////////
00760 //////////////////////////////////////////////////////////////
00761 // THE CLASS MATRIX22 DERIVED FROM MAT
00762 // / /////////////////////////////////////////////////////////
00763 
00764 //! A simple 2x2 matrix.
00765 //! Including all methods which aren't in class Mat
00766 //! @ingroup Matrix
00767 
00768 
00769 template <typename _DataType>
00770 class Matrix22 : public Mat<2, 2, _DataType> {
00771 
00772 
00773 public:
00774   typedef _DataType DataType;
00775 
00776   //! Constructor
00777   Matrix22()
00778       : Mat<2, 2, _DataType>() { }
00779 
00780   //! Constructor (for return value optimization)
00781   //! fills this with:
00782   //! [ a b ]
00783   //! [ c d ]
00784   Matrix22 ( const _DataType a, const _DataType b,
00785              const _DataType c, const _DataType d ) {
00786     this->_row[0][0] = a;     this->_row[0][1] = b;
00787     this->_row[1][0] = c;     this->_row[1][1] = d;
00788   }
00789 
00790   //! Copy-constructor
00791   Matrix22 ( const Matrix22<_DataType> &rhs )
00792       : Mat<2, 2, _DataType> ( rhs ) {}
00793 
00794   Matrix22 ( const Mat<2, 2, _DataType> & rhs )
00795     : Mat<2, 2, _DataType> ( rhs ) {}
00796 
00797   //! fills matrix with parameters passed
00798   void fill ( const _DataType a, const _DataType b,
00799               const _DataType c, const _DataType d ) {
00800 
00801     this->_row[0][0] = a;
00802     this->_row[0][1] = b;
00803     this->_row[1][0] = c;
00804     this->_row[1][1] = d;
00805   }
00806 
00807 
00808   void make_inverse ( Matrix22<_DataType> &inv ) const {
00809     _DataType determinante = det();
00810     if ( determinante == _DataType ( 0 ) ) {
00811       throw ( Exception ( "matrix not invertible.", __FILE__, __LINE__ ) );
00812     }
00813 
00814     inv[0][0] =  ( *this ) [1][1];
00815     inv[0][1] = - ( *this ) [0][1];
00816     inv[1][0] = - ( *this ) [1][0];
00817     inv[1][1] =  ( *this ) [0][0];
00818     inv /= determinante;
00819   }
00820 
00821   //! return the inverse if it exists (otherwise throw...)
00822   Matrix22<_DataType> inverse() const {
00823     Matrix22<_DataType> inv;
00824     make_inverse ( inv );
00825     return inv;
00826   }
00827 
00828   void setIdentity( ) {
00829     for ( int i = 0; i < 2; ++i ) {
00830       for ( int j = 0; j < 2; ++j ) {
00831         this->_row[i][j] = static_cast<_DataType> ( i == j );
00832       }
00833     }
00834   }
00835 
00836   //! The cofactor matrix is defined as \f$ \mbox{Cof} A := \mbox{det} A \cdot A^{-T} \f$
00837   //
00838   template <class T>
00839   void makeCofactorMatrix ( const Matrix22<T> &Mat ) {
00840     this->_row[0][0] =   Mat._row[1][1];
00841     this->_row[1][1] =   Mat._row[0][0];
00842     this->_row[1][0] = - Mat._row[0][1];
00843     this->_row[0][1] = - Mat._row[1][0];
00844   }
00845 
00846   //! qc::Computes the matrix \f$ \partial_{a_{ij}} \mbox{Cof} A \f$.
00847   //
00848   template <typename T>
00849   void makeDerivedCofactorMatrix ( const Matrix22<T> &, const int I, const int J ) {
00850     this->setZero();
00851     if ( I == J ) this->_row[ ( I+1 ) %2 ][ ( J+1 ) %2 ] = 1.;
00852     else this->_row[ ( I+1 ) %2 ][ ( J+1 ) %2 ] = -1.;
00853   }
00854 
00855   template <class T>
00856   static void multWithDerivedCofactorMatrix ( const int I, const int J,
00857                                               const Vec2<T> &Arg, Vec2<T> &Dest ) {
00858     if ( I == J ) {
00859       Dest[ I ] = 0.;
00860       Dest[ ( I+1 ) %2 ] = Arg.get ( ( I + 1 ) % 2 );
00861     } else {
00862       Dest[ J ] =  - Arg.get ( I );
00863       Dest[ I ] = 0.;
00864     }
00865   }
00866 
00867   //! computes the determinant of this matrix.
00868   _DataType det( ) const {
00869     return this->_row[0][0]*this->_row[1][1] - this->_row[1][0]*this->_row[0][1];
00870   }
00871 
00872   //! computes the trace of this matrix.
00873   _DataType tr( ) const {
00874     return this->_row[0][0] + this->_row[1][1];
00875   }
00876 
00877   using Mat<2, 2, _DataType>::operator*=;
00878 
00879   /**
00880    * \f$ A\mapsto A*B \f$
00881    */
00882   Matrix22<_DataType> &operator*= ( const Matrix22<_DataType> &Other ) {
00883     Matrix22<_DataType> tmp;
00884     tmp = *this;
00885     this->setZero( );
00886     for ( int i = 0; i < 2; ++i ) {
00887       for ( int j = 0; j < 2; ++j ) {
00888         for ( int k = 0; k < 2; ++k ) {
00889           this->_row[i][j] += tmp._row[i][k] * Other._row[k][j];
00890         }
00891       }
00892     }
00893     return *this;
00894   }
00895 
00896   void eigenValues ( Vec2<_DataType> &Eig ) const;
00897 
00898   void eigenVector ( Vec2<_DataType> &EigV, _DataType lambda ) const {
00899     EigV[0] = -this->_row[0][1];
00900     EigV[1] = this->_row[0][0] - lambda;
00901 
00902     if ( EigV [0] == 0 && EigV [1] == 0 ) {
00903 
00904       EigV[0] = this->_row[1][1] - lambda;
00905       EigV[1] = -this->_row[1][0];
00906     }
00907 
00908     EigV.normalize ();
00909   }
00910 
00911   //! Stores eigenvectors in columns
00912   void eigenVectors ( Matrix22 &Eig, Vec2<_DataType> lambda ) const {
00913     if ( lambda [0] == lambda [1] ) {
00914       Eig.setIdentity ();
00915       return;
00916     }
00917 
00918     Vec2<_DataType> eig [2];
00919     for ( int i = 0; i < 2; ++i )
00920       eigenVector ( eig [i], lambda [i] );
00921 
00922     Eig.fill ( eig [0][0], eig [1][0], eig [0][1], eig [1][1] );
00923   }
00924 
00925   //! Stores eigenvectors in columns
00926   void eigenVectors ( Matrix22 &Eig ) const {
00927     Vec2<_DataType> lambda;
00928     eigenValues ( lambda );
00929 
00930     if ( lambda [0] == lambda [1] ) {
00931       Eig.setIdentity ();
00932       return;
00933     }
00934 
00935     Vec2<_DataType> eig [2];
00936     for ( int i = 0; i < 2; ++i )
00937       eigenVector ( eig [i], lambda [i] );
00938 
00939     Eig.fill ( eig [0][0], eig [1][0], eig [0][1], eig [1][1] );
00940   }
00941 
00942   Vec2<_DataType> operator* ( const Vec2<_DataType>& vec ) const {
00943     Vec2<_DataType> Res;
00944     for ( int i = 0; i < 2; ++i )
00945       for ( int j = 0; j < 2; ++j )
00946         Res[i] += this->_row[i][j] * vec[j];
00947 
00948     return( Res );
00949   }
00950   
00951   void makeRotation ( _DataType Alpha ) {
00952     const _DataType co = cos ( Alpha ), si = sin ( Alpha );
00953     this->_row[0][0] =  co;
00954     this->_row[0][1] = -si;
00955     this->_row[1][0] =  si;
00956     this->_row[1][1] =  co;
00957   }
00958 
00959 };
00960 
00961 
00962 // forward declaration for conversion constructor
00963 template <typename _DataType> class Matrix33Symm;
00964 
00965 //////////////////////////////////////////////////////////////
00966 //////////////////////////////////////////////////////////////
00967 // THE CLASS MATRIX33 DERIVEDED FROM MAT
00968 // / /////////////////////////////////////////////////////////
00969 
00970 //! A simple 3x3 matrix.
00971 //! Including all methods which aren't in class Mat
00972 //! @ingroup Matrix
00973 
00974 template <typename _DataType>
00975 class Matrix33 : public Mat<3, 3, _DataType> {
00976 
00977 public:
00978   typedef _DataType DataType;
00979 
00980   //! Constructor
00981   Matrix33()
00982       : Mat<3, 3, _DataType>() { }
00983 
00984   //! Constructor (for return value optimization)
00985   //! fills this with:
00986   //! [ a b c ]
00987   //! [ d e f ]
00988   //! [ g h i ]
00989   Matrix33 ( const _DataType a, const _DataType b, const _DataType c,
00990              const _DataType d, const _DataType e, const _DataType f,
00991              const _DataType g, const _DataType h, const _DataType i ) {
00992     this->_row[0][0] = a;     this->_row[0][1] = b;    this->_row[0][2] = c;
00993     this->_row[1][0] = d;     this->_row[1][1] = e;    this->_row[1][2] = f;
00994     this->_row[2][0] = g;     this->_row[2][1] = h;    this->_row[2][2] = i;
00995   }
00996 
00997   //! Copy-constructor
00998   Matrix33 ( const Matrix33<_DataType> &rhs )
00999       : Mat<3, 3, _DataType> ( rhs ) {}
01000 
01001   //! Copy-constructor
01002   Matrix33 ( const Mat<3, 3, _DataType> &rhs )
01003       : Mat<3, 3, _DataType> ( rhs ) {}
01004 
01005   //! Copy from symmetric matrix
01006   explicit Matrix33 ( const Matrix33Symm<_DataType>& mat );
01007 
01008   //! fills matrix with parameters passed
01009   void fill (  const _DataType a, const _DataType b, const _DataType c,
01010                const _DataType d, const _DataType e, const _DataType f,
01011                const _DataType g, const _DataType h, const _DataType i ) {
01012 
01013     this->_row[0][0] = a;     this->_row[0][1] = b;    this->_row[0][2] = c;
01014     this->_row[1][0] = d;     this->_row[1][1] = e;    this->_row[1][2] = f;
01015     this->_row[2][0] = g;     this->_row[2][1] = h;    this->_row[2][2] = i;
01016   }
01017 
01018   //! calc the inverse matrix with adjoints
01019   Matrix33<_DataType> inverse() {
01020     _DataType determinante ( det() );
01021     if ( determinante == _DataType ( 0 ) ) {
01022       throw ( Exception ( "matrix not invertible.", __FILE__, __LINE__ ) );
01023     }
01024 
01025     Matrix33 res;
01026     _DataType A;
01027 
01028     for ( int i = 0; i < 3; ++i ) {
01029       for ( int j = 0; j < 3; ++j ) {
01030         // calc the determinant, the sign is automatically ok
01031         A = this->_row[ ( i+1 ) % 3][ ( j+1 ) % 3] * this->_row[ ( i+2 ) % 3][ ( j+2 ) % 3]
01032             - this->_row[ ( i+1 ) % 3][ ( j+2 ) % 3] * this->_row[ ( i+2 ) % 3][ ( j+1 ) % 3];
01033         // consider the transposed matrix:
01034         res.set ( j, i, A / determinante );
01035       }
01036     }
01037 
01038     return res;
01039   }
01040 
01041   void setIdentity( ) {
01042     for ( int i = 0; i < 3; ++i ) {
01043       for ( int j = 0; j < 3; ++j ) {
01044         ( this->_row[i] ) [j] = static_cast<_DataType> ( i == j );
01045       }
01046     }
01047   }
01048 
01049   using Mat<3, 3, _DataType>::setRow;
01050 
01051   void setRow ( const int Index, const _DataType V1, _DataType V2, _DataType V3 ) {
01052     ( this->_row[Index] ) [0] = V1;
01053     ( this->_row[Index] ) [1] = V2;
01054     ( this->_row[Index] ) [2] = V3;
01055   }
01056 
01057   //! The Cofactormatrix is defined as \f$ \mbox{Cof} A := \mbox{det} A \cdot A^{-T} \f$
01058   //
01059   template <class T>
01060   void makeCofactorMatrix ( const Matrix33<T> &Mat ) {
01061     this->_row[0][0] =   Mat.get ( 1, 1 ) * Mat.get ( 2, 2 ) - Mat.get ( 1, 2 ) * Mat.get ( 2, 1 );
01062     this->_row[1][0] = - Mat.get ( 0, 1 ) * Mat.get ( 2, 2 ) + Mat.get ( 2, 1 ) * Mat.get ( 0, 2 );
01063     this->_row[2][0] =   Mat.get ( 0, 1 ) * Mat.get ( 1, 2 ) - Mat.get ( 1, 1 ) * Mat.get ( 0, 2 );
01064 
01065     this->_row[0][1] = - Mat.get ( 1, 0 ) * Mat.get ( 2, 2 ) + Mat.get ( 2, 0 ) * Mat.get ( 1, 2 );
01066     this->_row[1][1] =   Mat.get ( 0, 0 ) * Mat.get ( 2, 2 ) - Mat.get ( 0, 2 ) * Mat.get ( 2, 0 );
01067     this->_row[2][1] = - Mat.get ( 0, 0 ) * Mat.get ( 1, 2 ) + Mat.get ( 1, 0 ) * Mat.get ( 0, 2 );
01068 
01069     this->_row[0][2] =   Mat.get ( 1, 0 ) * Mat.get ( 2, 1 ) - Mat.get ( 1, 1 ) * Mat.get ( 2, 0 );
01070     this->_row[1][2] = - Mat.get ( 0, 0 ) * Mat.get ( 2, 1 ) + Mat.get ( 0, 1 ) * Mat.get ( 2, 0 );
01071     this->_row[2][2] =   Mat.get ( 0, 0 ) * Mat.get ( 1, 1 ) - Mat.get ( 1, 0 ) * Mat.get ( 0, 1 );
01072   }
01073 
01074 
01075 
01076   //! qc::Computes the matrix \f$ \partial_{a_{ij}} \mbox{Cof} A \f$.
01077   //
01078   template <class T>
01079   void makeDerivedCofactorMatrix ( const Matrix33<T> &Mat, int I, int J ) {
01080     this->setZero();
01081 
01082     this->_row[ (I + 1) % 3 ][ (J + 1) % 3 ] =   Mat.get( (I + 2) % 3, (J + 2) % 3 );
01083     this->_row[ (I + 2) % 3 ][ (J + 2) % 3 ] =   Mat.get( (I + 1) % 3, (J + 1) % 3 );
01084     this->_row[ (I + 1) % 3 ][ (J + 2) % 3 ] = - Mat.get( (I + 2) % 3, (J + 1) % 3 );
01085     this->_row[ (I + 2) % 3 ][ (J + 1) % 3 ] = - Mat.get( (I + 1) % 3, (J + 2) % 3 );
01086 
01087   }
01088 
01089   //! computes the determinant of this matrix.
01090   //
01091   _DataType det() const {
01092     return this->_row[0][0]*this->_row[1][1]*this->_row[2][2] + this->_row[0][1]*this->_row[1][2]*this->_row[2][0] +
01093            this->_row[0][2]*this->_row[1][0]*this->_row[2][1] - this->_row[0][2]*this->_row[1][1]*this->_row[2][0] -
01094            this->_row[0][1]*this->_row[1][0]*this->_row[2][2] - this->_row[0][0]*this->_row[1][2]*this->_row[2][1];
01095   }
01096 
01097   //! computes the trace of this matrix.
01098   //
01099   _DataType tr() {
01100     return this->_row[0][0] + this->_row[1][1] + this->_row[2][2];
01101   }
01102 
01103   using Mat<3, 3, _DataType>::operator*=;
01104 
01105   //! \f$ A\mapsto A*B \f$
01106   //
01107   Matrix33<_DataType> &operator*= ( const Matrix33<_DataType> &Other ) {
01108     Matrix33<_DataType> tmp;
01109     tmp = *this;
01110     this->setZero( );
01111     for ( int i = 0; i < 3; ++i ) {
01112       for ( int j = 0; j < 3; ++j ) {
01113         for ( int k = 0; k < 3; ++k ) {
01114           this->_row[i][j] += tmp._row[i][k] * Other._row[k][j];
01115         }
01116       }
01117     }
01118     return *this;
01119   }
01120 
01121   //! \f$ A \mapsto A^T \f$
01122   //
01123   void transpose() {
01124     inplaceTranspose ( *this );
01125   }
01126   
01127   //! return (this matrix) transposed (two copies necessary)
01128   aol::Matrix33<_DataType> transposed () const {
01129     aol::Matrix33<_DataType> tmp = *this;
01130     tmp.transpose();
01131     return ( tmp );
01132   }
01133 
01134   //! Set to rotation matrix in the yz plane (about x axis)
01135   void setRotationAboutX ( _DataType alpha ) {
01136     const _DataType co = cos ( alpha ), si = sin ( alpha );
01137     this->_row[0][0] = 1.0;    this->_row[0][1] = 0.0;    this->_row[0][2] = 0.0; 
01138     this->_row[1][0] = 0.0;    this->_row[1][1] = co ;    this->_row[1][2] = -si; 
01139     this->_row[2][0] = 0.0;    this->_row[2][1] = si ;    this->_row[2][2] = co ; 
01140   }
01141 
01142   //! Set to rotation matrix in the xz plane (about y axis)
01143   void setRotationAboutY ( _DataType alpha ) {
01144     const _DataType co = cos ( alpha ), si = sin ( alpha );
01145     this->_row[0][0] = co ;    this->_row[0][1] = 0.0;    this->_row[0][2] = -si; 
01146     this->_row[1][0] = 0.0;    this->_row[1][1] = 1.0;    this->_row[1][2] = 0.0; 
01147     this->_row[2][0] = si ;    this->_row[2][1] = 0.0;    this->_row[2][2] = co ; 
01148   }
01149 
01150   //! Set to rotation matrix in the xy plane (about z axis)
01151   void setRotationAboutZ ( _DataType alpha ) {
01152     const _DataType co = cos ( alpha ), si = sin ( alpha );
01153     this->_row[0][0] = co ;    this->_row[0][1] =-si ;    this->_row[0][2] = 0.0; 
01154     this->_row[1][0] = si ;    this->_row[1][1] = co ;    this->_row[1][2] = 0.0; 
01155     this->_row[2][0] = 0.0;    this->_row[2][1] = 0.0;    this->_row[2][2] = 1.0; 
01156   }
01157 
01158   //! Set to rotation matrix about the ith axis (angle=``alpha'',axis=``Axis'')
01159   void setRotationAboutAxis ( _DataType Alpha, short Axis ) {
01160     switch ( Axis ) {
01161     case 0: this->setRotationAboutX( Alpha ); break;
01162     case 1: this->setRotationAboutY( Alpha ); break;
01163     case 2: this->setRotationAboutZ( Alpha ); break;
01164     default: this->setRotationAboutZ( Alpha );
01165     }
01166   }
01167 
01168   Vec3<_DataType> operator* ( const Vec3<_DataType>& vec ) const {
01169     Vec3<_DataType> Res;
01170     for ( int i = 0; i < 3; ++i )
01171       for ( int j = 0; j < 3; ++j )
01172         Res[i] += this->_row[i][j] * vec[j];
01173 
01174     return( Res );
01175   }
01176 
01177 };
01178 
01179 
01180 /** a simple 4x4 matrix
01181  */
01182 template <typename _DataType>
01183 class Matrix44 : public Mat<4, 4, _DataType> {
01184 
01185 public:
01186   typedef _DataType DataType;
01187 
01188   /** a default constructor
01189    */
01190   Matrix44<_DataType> ( void ) : Mat<4, 4, _DataType>() {}
01191 
01192   //! Constructor
01193   //! fills this with:
01194   //! [ a b c d ]
01195   //! [ e f g h ]
01196   //! [ i j k l ]
01197   //! [ m n o p ]
01198   Matrix44 ( const _DataType a, const _DataType b, const _DataType c, const _DataType d,
01199              const _DataType e, const _DataType f, const _DataType g, const _DataType h,
01200              const _DataType i, const _DataType j, const _DataType k, const _DataType l,
01201              const _DataType m, const _DataType n, const _DataType o, const _DataType p ) {
01202     this->_row[0][0] = a;    this->_row[0][1] = b;    this->_row[0][2] = c;    this->_row[0][3] = d;
01203     this->_row[1][0] = e;    this->_row[1][1] = f;    this->_row[1][2] = g;    this->_row[1][3] = h;
01204     this->_row[2][0] = i;    this->_row[2][1] = j;    this->_row[2][2] = k;    this->_row[2][3] = l;
01205     this->_row[3][0] = m;    this->_row[3][1] = n;    this->_row[3][2] = o;    this->_row[3][3] = p;
01206   }
01207 
01208   Matrix44(const Mat<4, 4, _DataType> & rhs) : Mat<4, 4, _DataType>(rhs) {
01209   }
01210 
01211   /** compute the determinant of this matrix
01212    */
01213   _DataType det ( void ) const {
01214     // We Laplace-expand the determinant along the first row
01215     _DataType determinant = aol::NumberTrait<_DataType>::zero;
01216     aol::Matrix33<_DataType> dummyMatrix;
01217 
01218     dummyMatrix.fill ( this->_row[1][1], this->_row[1][2], this->_row[1][3],
01219                        this->_row[2][1], this->_row[2][2], this->_row[2][3],
01220                        this->_row[3][1], this->_row[3][2], this->_row[3][3] );
01221     determinant += this->_row[0][0] * dummyMatrix.det();
01222 
01223     dummyMatrix.fill ( this->_row[1][0], this->_row[1][2], this->_row[1][3],
01224                        this->_row[2][0], this->_row[2][2], this->_row[2][3],
01225                        this->_row[3][0], this->_row[3][2], this->_row[3][3] );
01226     determinant -= this->_row[0][1] * dummyMatrix.det();
01227 
01228     dummyMatrix.fill ( this->_row[1][0], this->_row[1][1], this->_row[1][3],
01229                        this->_row[2][0], this->_row[2][1], this->_row[2][3],
01230                        this->_row[3][0], this->_row[3][1], this->_row[3][3] );
01231     determinant += this->_row[0][2] * dummyMatrix.det();
01232 
01233     dummyMatrix.fill ( this->_row[1][0], this->_row[1][1], this->_row[1][2],
01234                        this->_row[2][0], this->_row[2][1], this->_row[2][2],
01235                        this->_row[3][0], this->_row[3][1], this->_row[3][2] );
01236     determinant -= this->_row[0][3] * dummyMatrix.det();
01237 
01238     return determinant;
01239   }
01240 };
01241 
01242 
01243 template <int numRows, int numCols, class T>
01244 inline ostream &operator<< ( ostream &os, const Mat<numRows, numCols, T> &m ) {
01245   return m.print ( os );
01246 }
01247 
01248 template <int numRows, int numCols, class T>
01249 inline istream &operator>> ( istream &is, Mat<numRows, numCols, T> &m ) {
01250   return m.read ( is );
01251 }
01252 
01253 template <class _DataType>
01254 class Matrix33Symm {
01255 public:
01256   typedef _DataType DataType;
01257 
01258   //! Constructor
01259   Matrix33Symm()
01260       : _row ( 3 ) { }
01261 
01262   //! Copy-constructor
01263   Matrix33Symm ( const Matrix33Symm<_DataType> &rhs )
01264       : _row ( 3 ) {
01265     _row[0] = rhs._row[0];
01266     _row[1] = rhs._row[1];
01267     _row[2] = rhs._row[2];
01268   }
01269 
01270   //! Constructor (for return value optimization)
01271   //! fills this with:
01272   //! [ a b c ]
01273   //! [ d e f ]
01274   //! [ g h i ]
01275   Matrix33Symm ( const _DataType a, const _DataType b, const _DataType c,
01276                  const _DataType d, const _DataType e, const _DataType f,
01277                  const _DataType g, const _DataType h, const _DataType i )
01278       : _row ( 3 ) {
01279 
01280     if ( b != d || c != g || f != h ) throw aol::InconsistentDataException ( "Matrix33Symm::Matrix33Symm : must be symmetric", __FILE__, __LINE__ );
01281 
01282     _row[0][0] = a;
01283     _row[0][1] = b;
01284     _row[0][2] = c;
01285     _row[1][0] = d;
01286     _row[1][1] = e;
01287     _row[1][2] = f;
01288     _row[2][0] = g;
01289     _row[2][1] = h;
01290     _row[2][2] = i;
01291   }
01292 
01293   //! operator=
01294   Matrix33Symm<_DataType>& operator= ( const Matrix33Symm<_DataType> &rhs ) {
01295     _row[0] = rhs._row[0];
01296     _row[1] = rhs._row[1];
01297     _row[2] = rhs._row[2];
01298     return *this;
01299   }
01300 
01301 
01302   //! Calulates the Eigenvalues and Eigenvectors of the matrix using Jacobi-iteration.
01303   //! eigenVals[i] corresponds to the i-th column of eigenVecs
01304   void eigenVectors ( Vec3<_DataType> &eigenVals, Matrix33<_DataType> &eigenVecs );
01305 
01306 
01307   ostream& print ( ostream& out ) const {
01308     out << _row[0] << endl;
01309     out << _row[1] << endl;
01310     out << _row[2];
01311     return out;
01312   }
01313 
01314   const Vec3<_DataType>&   operator[] ( const int i ) const {
01315     return _row[i];
01316   }
01317 
01318   _DataType get ( const int i, const int j ) const {
01319       return ( _row[i] ) [j];
01320     }
01321   void     set ( const int i, const int j, const _DataType Value ) {
01322     ( _row[i] ) [j] = Value;
01323     ( _row[j] ) [i] = Value;
01324   }
01325 
01326   void add ( const int I, const int J, const _DataType Value ) {
01327     _row[I][J] += Value;
01328     _row[J][I] += Value;
01329   }
01330 
01331   void setZero() {
01332     _row[0].setZero();
01333     _row[1].setZero();
01334     _row[2].setZero();
01335   }
01336 
01337   template<class T>
01338   void setIdentity() {
01339     for ( int i = 0; i < 3; ++i ) {
01340       for ( int j = 0; j < 3; ++j ) {
01341         ( _row[i] ) [j] = static_cast<T> ( i == j );
01342       }
01343     }
01344   }
01345 
01346   void setRow ( const int index, const _DataType v1, _DataType v2, _DataType v3 ) {
01347     ( _row[index] ) [0] = v1;
01348     ( _row[index] ) [1] = v2;
01349     ( _row[index] ) [2] = v3;
01350     ( _row[0] ) [index] = v1;
01351     ( _row[1] ) [index] = v2;
01352     ( _row[2] ) [index] = v3;
01353   }
01354 
01355   template <class T>
01356   void setRow ( const int /*index*/, const Vec3<T> &vec ) {
01357     ( _row[this->Index] ) [0] = static_cast<T> ( vec.get ( 0 ) );
01358     ( _row[0] ) [this->Index] = static_cast<T> ( vec.get ( 0 ) );
01359 
01360     ( _row[this->Index] ) [1] = static_cast<T> ( vec.get ( 1 ) );
01361     ( _row[1] ) [this->Index] = static_cast<T> ( vec.get ( 1 ) );
01362 
01363     ( _row[this->Index] ) [2] = static_cast<T> ( vec.get ( 2 ) );
01364     ( _row[2] ) [this->Index] = static_cast<T> ( vec.get ( 2 ) );
01365   }
01366 
01367   template <class T>
01368   void getRow ( const int index, Vec3<T> &dest ) const {
01369     dest[0] = static_cast<T> ( ( _row ) [index][0] );
01370     dest[1] = static_cast<T> ( ( _row ) [this->Index][1] );
01371     dest[2] = static_cast<T> ( ( _row ) [this->Index][2] );
01372   }
01373 
01374   template <class T>
01375   void getColumn ( const int /*index*/, Vec3<T> &dest ) const {
01376     dest[0] = static_cast<T> ( ( _row ) [0][this->Index] );
01377     dest[1] = static_cast<T> ( ( _row ) [1][this->Index] );
01378     dest[2] = static_cast<T> ( ( _row ) [2][this->Index] );
01379   }
01380 
01381 
01382   //! \f$ x \mapsto Ax \f$
01383   //
01384   template <class T>
01385   void mult ( const Vec3<T> &Arg, Vec3<T> &Dest ) const {
01386     Dest[0] = _row[0][0] * Arg.get ( 0 ) + _row[0][1] * Arg.get ( 1 ) + _row[0][2] * Arg.get ( 2 );
01387     Dest[1] = _row[1][0] * Arg.get ( 0 ) + _row[1][1] * Arg.get ( 1 ) + _row[1][2] * Arg.get ( 2 );
01388     Dest[2] = _row[2][0] * Arg.get ( 0 ) + _row[2][1] * Arg.get ( 1 ) + _row[2][2] * Arg.get ( 2 );
01389   }
01390 
01391   //! computes the determinant of this matrix.
01392   //
01393   _DataType det() const {
01394     return
01395       _row[0][0]*_row[1][1]*_row[2][2] +
01396       _row[0][1]*_row[1][2]*_row[2][0] +
01397       _row[0][2]*_row[1][0]*_row[2][1]
01398       - _row[0][2]*_row[1][1]*_row[2][0]
01399       - _row[0][1]*_row[1][0]*_row[2][2]
01400       - _row[0][0]*_row[1][2]*_row[2][1];
01401   }
01402 
01403   //! computes the trace of this matrix.
01404   //
01405   _DataType tr() const {
01406     return _row[0][0] + _row[1][1] + _row[2][2];
01407   }
01408 
01409   //! \f$ A \mapsto A + B \f$
01410   //
01411   Matrix33Symm<_DataType> &operator+= ( const Matrix33Symm<_DataType> &Other ) {
01412     for ( int i = 0; i < 3; ++i ) {
01413       for ( int j = 0; j < 3; ++j ) {
01414         _row[i][j] += Other._row[i][j];
01415       }
01416     }
01417     return *this;
01418   }
01419 
01420   //! \f$ A \mapsto A - B \f$
01421   //
01422   Matrix33Symm<_DataType> &operator-= ( const Matrix33Symm<_DataType> &Other ) {
01423     for ( int i = 0; i < 3; ++i ) {
01424       for ( int j = 0; j < 3; ++j ) {
01425         _row[i][j] -= Other._row[i][j];
01426       }
01427     }
01428     return *this;
01429   }
01430 
01431   //! \f$ A\mapsto A*B \f$
01432   //
01433   Matrix33Symm<_DataType> &operator*= ( const Matrix33Symm<_DataType> &Other ) {
01434     Matrix33Symm<_DataType> tmp;
01435     tmp = *this;
01436     setZero();
01437     for ( int i = 0; i < 3; ++i ) {
01438       for ( int j = 0; j < 3; ++j ) {
01439         for ( int k = 0; k < 3; ++k ) {
01440           _row[i][j] += tmp._row[i][k] * Other._row[k][j];
01441         }
01442       }
01443     }
01444     return *this;
01445   }
01446 
01447   template< class T >
01448   void makeProduct ( const Matrix33Symm<T> &Mat1, const Matrix33Symm<T> &Mat2 ) {
01449     setZero();
01450     for ( int i = 0;i < 3; ++i ) for ( int j = 0;j < 3; ++j ) for ( int k = 0;k < 3; ++k ) {
01451           this->_v[i][j] += Mat1.get ( i, k ) * Mat2.get ( k, j );
01452         }
01453   }
01454 
01455   void makeProduct ( const Matrix33Symm<_DataType> &A, const Matrix33Symm<_DataType> &B ) {
01456     setZero();
01457     for ( int i = 0; i < 3; ++i ) {
01458       for ( int j = 0; j < 3; ++j ) {
01459         for ( int k = 0; k < 3; ++k ) {
01460           _row[i][j] += A._row[i][k] * B._row[k][j];
01461         }
01462       }
01463     }
01464   }
01465 
01466 
01467   //! \f$ A \mapsto \alpha A \f$
01468   //
01469   Matrix33Symm<_DataType> &operator*= ( _DataType Alpha ) {
01470     for ( int i = 0; i < 3; ++i ) 
01471       for ( int j = 0; j < 3; ++j ) 
01472         _row[i][j] *= Alpha;
01473     return *this;
01474   }
01475 
01476 
01477   //! \f$ A \mapsto \alpha^{-1} A \f$
01478   //
01479   Matrix33Symm<_DataType> &operator/= ( _DataType Alpha ) {
01480     for ( int i = 0; i < 3; ++i ) 
01481       for ( int j = 0; j < 3; ++j ) 
01482         _row[i][j] /= Alpha;
01483     return *this;
01484   }
01485 
01486   //! \f$ A \mapsto || A ||_{\infty} \f$
01487   //
01488   _DataType infinityNorm() {
01489     _DataType res = 0;
01490     for ( int i = 0; i < 3; ++i ) {
01491       _DataType temp = 0;
01492       for ( int j = 0; j < 3; ++j ) {
01493         temp += Abs ( _row[i][j] );
01494       }
01495       if ( temp > res ) res = temp;
01496     }
01497     return ( res );
01498   }
01499 
01500 
01501   //! \f$ A \mapsto A^T \f$
01502   //
01503   void transpose() {
01504     // Do nothing
01505   }
01506 
01507 private:
01508   vector< Vec3<_DataType> >  _row;
01509 };
01510 
01511 template <typename RealType, int Dim> struct MatDimTrait {
01512   typedef Mat< Dim, Dim, RealType> MatType;
01513 };
01514 
01515 template <typename RealType> struct MatDimTrait<RealType, 2> {
01516   typedef Matrix22<RealType> MatType;
01517 };
01518 
01519 template <typename RealType> struct MatDimTrait<RealType, 3> {
01520   typedef Matrix33<RealType> MatType;
01521 };
01522 
01523 template <class T>
01524 inline ostream &operator<< ( ostream &os, const Matrix33Symm<T> &m ) {
01525   return m.print ( os );
01526 }
01527 
01528 } // end namespace
01529 
01530 #endif

Generated on Fri Sep 9 2011 21:07:55 for QuocMesh by doxygen 1.7.1