OpenWalnut  1.4.0
WMatrix.h
00001 //---------------------------------------------------------------------------
00002 //
00003 // Project: OpenWalnut ( http://www.openwalnut.org )
00004 //
00005 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
00006 // For more information see http://www.openwalnut.org/copying
00007 //
00008 // This file is part of OpenWalnut.
00009 //
00010 // OpenWalnut is free software: you can redistribute it and/or modify
00011 // it under the terms of the GNU Lesser General Public License as published by
00012 // the Free Software Foundation, either version 3 of the License, or
00013 // (at your option) any later version.
00014 //
00015 // OpenWalnut 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 Lesser General Public License for more details.
00019 //
00020 // You should have received a copy of the GNU Lesser General Public License
00021 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
00022 //
00023 //---------------------------------------------------------------------------
00024 
00025 #ifndef WMATRIX_H
00026 #define WMATRIX_H
00027 
00028 #include <iostream>
00029 
00030 #include <osg/Matrix>
00031 
00032 #include "WValue.h"
00033 #include "linearAlgebra/WMatrixFixed.h"
00034 #include "linearAlgebra/WVectorFixed.h"
00035 
00036 #include "../WDefines.h"
00037 
00038 /**
00039  * Matrix template class with variable number of rows and columns.
00040  * The access function are row-major, which means that the rows
00041  * are the first parameter or index.
00042  */
00043 template< typename T > class WMatrix : public WValue< T >
00044 {
00045 public:
00046     /**
00047      * Produces a square matrix with the given number of components.
00048      * The components will be set to zero if T is a type representing numbers.
00049      *
00050      * \param n Number of cols and rows in the matrix
00051      */
00052     explicit WMatrix( size_t n );
00053 
00054     /**
00055      * Produces a matrix with the given number of components.
00056      * The components will be set to zero if T is a type representing numbers.
00057      *
00058      * \param nbRows number of rows in the matrix
00059      * \param nbCols number of columns in the matrix
00060      */
00061     WMatrix( size_t nbRows, size_t nbCols );
00062 
00063     /**
00064      * Produces a matrix as copy of the one given as parameter.
00065      * \param newMatrix The matrix to be copied.
00066      */
00067     WMatrix( const WMatrix& newMatrix );
00068 
00069     /**
00070      * Copies the specified 4x4 matrix.
00071      *
00072      * \param newMatrix the matrix to copy
00073      */
00074     WMatrix( const WMatrix4d& newMatrix ); // NOLINT
00075 
00076     /**
00077      * Copies the specified Eigen::MatrixXd.
00078      *
00079      * \param newMatrix the Eigen::MatrixXd matrix to copy
00080      */
00081     WMatrix( const Eigen::MatrixXd& newMatrix ); // NOLINT
00082 
00083     /**
00084      * Copies the specified Eigen::MatrixXf.
00085      *
00086      * \param newMatrix the Eigen::MatrixXf matrix to copy
00087      */
00088     WMatrix( const Eigen::MatrixXf& newMatrix ); // NOLINT
00089 
00090     /**
00091      * Copies the specified Eigen::MatrixXi.
00092      *
00093      * \param newMatrix the Eigen::MatrixXi matrix to copy
00094      */
00095     WMatrix( const Eigen::MatrixXi& newMatrix ); // NOLINT
00096 
00097     /**
00098      * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal.
00099      * \return Reference to the current matrix which is identity matrix now.
00100      */
00101     WMatrix& makeIdentity();
00102 
00103     /**
00104      * Get number of rows.
00105      * \return Number of rows of the matrix.
00106      */
00107     size_t getNbRows() const;
00108 
00109     /**
00110      * Get number of columns.
00111      * \return Number of columns of the matrix.
00112      */
00113     size_t getNbCols() const;
00114 
00115     /**
00116      * Returns a reference to the component an row i, columns j in order to
00117      * provide access to the component.
00118      * \param i row
00119      * \param j column
00120      * \return A reference to the component (i,j)
00121      */
00122     T& operator()( size_t i, size_t j );
00123 
00124     /**
00125      * Returns a const reference to the component an row i, columns j in order to
00126      * provide read-only access to the component.
00127      * \param i row
00128      * \param j column
00129      * \return A const reference to the component (i,j)
00130      */
00131     const T& operator()( size_t i, size_t j ) const;
00132 
00133     /**
00134      * Cast this matrix to an 4x matrix if it is a 4x4 matrix.
00135      *
00136      * \return casted matrix
00137      */
00138     operator WMatrix4d() const;
00139 
00140     /**
00141      * Cast this matrix to an 4x4 osg matrix if it is a 4x4 matrix.
00142      *
00143      * \return casted matrix.
00144      */
00145     operator osg::Matrixd() const;
00146 
00147     /**
00148      * Cast this matrix to an Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >() matrix.
00149      *
00150      * \tparam EigenDataType Data type of Eigen matrix.
00151      *
00152      * \return casted matrix.
00153      */
00154     template< typename EigenDataType >
00155     operator Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >() const;
00156 
00157     /**
00158      * Compares two matrices and returns true if they are equal.
00159      * \param rhs The right hand side of the comparison
00160      * \return Are the matrices equal?
00161      */
00162     bool operator==( const WMatrix& rhs ) const;
00163 
00164     /**
00165      * Compares two matrices and returns true if they are not equal.
00166      * \param rhs The right hand side of the comparison
00167      * \return Are the matrices NOT equal?
00168      */
00169     bool operator!=( const WMatrix& rhs ) const;
00170 
00171     /**
00172      * Assigns the argument WMatrix to this WMatrix.
00173      * \param rhs The right hand side of the assignment
00174      * \return A reference to the left hand side of the assignment (i.e. the current object).
00175      */
00176     WMatrix& operator=( const WMatrix& rhs );
00177 
00178     /**
00179      * Multiplication of the current matrix with andother matrix.
00180      * \param rhs The right hand side of the multiplication
00181      * \return The product of the two matrices.
00182      */
00183     WMatrix operator*( const WMatrix& rhs ) const;
00184 
00185     /**
00186      * Multiplication with a vector.
00187      * \param rhs The right hand side of the multiplication
00188      * \return The product of the matrix and the vector.
00189      */
00190     WValue< T > operator*( const WValue< T >& rhs ) const;
00191 
00192     /**
00193      * Multiplication with a vector.
00194      * \param rhs The right hand side of the multiplication
00195      * \return The product of the matrix and the 3D vector.
00196      */
00197     WVector3d operator*( const WVector3d& rhs ) const;
00198 
00199     /**
00200      * Returns the transposed matrix.
00201      * \return Transposed version of the current matrix.
00202      */
00203     WMatrix transposed() const;
00204 
00205     /**
00206      * Resets the matrix components to zero.
00207      */
00208     void setZero()
00209     {
00210         for( size_t i = 0; i < this->size(); ++i )
00211         {
00212             ( *this )[ i ] = 0.0;
00213         }
00214     }
00215 
00216     /**
00217      * Returns true if the matrix is a square matrix.
00218      * \return true for square matrixes, otherwise false.
00219      */
00220     bool isSquare() const;
00221 
00222     /**
00223      * Returns true if the matrix is a identity matrix.
00224      * \param delta - tolerance parameter when checking the values.
00225      * \return true for identity matrixes, otherwise false.
00226      */
00227     bool isIdentity( T delta = T( 0.0 ) ) const;
00228 
00229 protected:
00230 private:
00231     /**
00232      * This function is used by the constructors that have the different Eigen::MatrixX types as parameter.
00233      * \tparam EigenDataType The data type which is used by the Eigen matrix.
00234      * \param newMatrix The source matrix.
00235      */
00236     template< typename EigenDataType >
00237     void copyFromEigenMatrix( const Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >& newMatrix );
00238 
00239     size_t m_nbCols; //!< Number of columns of the matrix. The number of rows will be computed by (size/m_nbCols).
00240 };
00241 
00242 template< typename T > WMatrix< T >::WMatrix( size_t n )
00243     : WValue< T >( n * n )
00244 {
00245     m_nbCols = n;
00246 }
00247 
00248 template< typename T > WMatrix< T >::WMatrix( size_t nbRows, size_t nbCols )
00249     : WValue< T >( nbRows * nbCols )
00250 {
00251     m_nbCols = nbCols;
00252 }
00253 
00254 /**
00255  * Produces a matrix as copy of the one given as parameter.
00256  * \param newMatrix The matrix to be copied.
00257  */
00258 template< typename T > WMatrix< T >::WMatrix( const WMatrix& newMatrix )
00259     : WValue< T >( newMatrix )
00260 {
00261     m_nbCols = newMatrix.m_nbCols;
00262 }
00263 
00264 template< typename T > WMatrix< T >::WMatrix( const WMatrix4d& newMatrix )
00265     : WValue< T >( 4 * 4 )
00266 {
00267     m_nbCols = 4;
00268     for( size_t i = 0; i < 4; ++i )
00269     {
00270         for( size_t j = 0; j < 4; ++j )
00271         {
00272             ( *this )( i, j ) = newMatrix( i, j );
00273         }
00274     }
00275 }
00276 
00277 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXd& newMatrix )
00278     : WValue< T >( newMatrix.cols() * newMatrix.rows() )
00279 {
00280     copyFromEigenMatrix< double >( newMatrix );
00281 }
00282 
00283 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXf& newMatrix )
00284     : WValue< T >( newMatrix.cols() * newMatrix.rows() )
00285 {
00286     copyFromEigenMatrix< float >( newMatrix );
00287 }
00288 
00289 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXi& newMatrix )
00290     : WValue< T >( newMatrix.cols() * newMatrix.rows() )
00291 {
00292     copyFromEigenMatrix< int >( newMatrix );
00293 }
00294 
00295 template< typename T > WMatrix< T >::operator WMatrix4d() const
00296 {
00297     size_t nbRows = this->size() / m_nbCols;
00298     WAssert( m_nbCols == 4 && nbRows == 4, "This is no 4x4 matrix." );
00299     WMatrix4d m;
00300     for( size_t i = 0; i < nbRows; ++i )
00301     {
00302         for( size_t j = 0; j < m_nbCols; ++j )
00303         {
00304             m( i, j ) = ( *this )( i, j );
00305         }
00306     }
00307     return m;
00308 }
00309 
00310 template< typename T > WMatrix< T >::operator osg::Matrixd() const
00311 {
00312     WAssert( ( getNbRows() == 3 || getNbRows() == 4 ) && ( getNbCols() == 3 || getNbCols() == 4 ),
00313              "Only 3x3 or 4x4 matrices allowed." );
00314 
00315     // handle 4x4 and 3x3 separately
00316     if( getNbRows() == 4 )
00317     {
00318         return osg::Matrixd( ( *this )[ 0 ], ( *this )[ 4 ], ( *this )[ 8 ], ( *this )[ 12 ],
00319                              ( *this )[ 1 ], ( *this )[ 5 ], ( *this )[ 9 ], ( *this )[ 13 ],
00320                              ( *this )[ 2 ], ( *this )[ 6 ], ( *this )[ 10 ], ( *this )[ 14 ],
00321                              ( *this )[ 3 ], ( *this )[ 7 ], ( *this )[ 11 ], ( *this )[ 15 ]
00322                            );
00323     }
00324     else
00325     {
00326         return osg::Matrixd( ( *this )[ 0 ], ( *this )[ 1 ], ( *this )[ 2 ], 0.0,
00327                              ( *this )[ 3 ], ( *this )[ 4 ], ( *this )[ 5 ], 0.0,
00328                              ( *this )[ 6 ], ( *this )[ 7 ], ( *this )[ 8 ], 0.0,
00329                              ( *this )[ 9 ], ( *this )[ 10 ], ( *this )[ 11 ], 1.0
00330                            );
00331     }
00332 }
00333 
00334 template< typename T >
00335 template< typename EigenDataType > WMatrix< T >::operator Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >() const
00336 {
00337     Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic > matrix( this->getNbRows(), this->getNbCols() );
00338     for( int row = 0; row < matrix.rows(); ++row )
00339     {
00340         for( int col = 0; col < matrix.cols(); ++col )
00341         {
00342             matrix( row, col ) = static_cast< EigenDataType >( ( *this )( row, col ) );
00343         }
00344     }
00345     return matrix;
00346 }
00347 
00348 
00349 /**
00350  * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal.
00351  */
00352 template< typename T > WMatrix< T >& WMatrix< T >::makeIdentity()
00353 {
00354     size_t nbRows = this->size() / m_nbCols;
00355     for( size_t i = 0; i < nbRows; ++i )
00356     {
00357         for( size_t j = 0; j < m_nbCols; ++j )
00358         {
00359             if( i == j )
00360             {
00361                 (*this)( i, j ) = 1;
00362             }
00363             else
00364             {
00365                 (*this)( i, j ) = 0;
00366             }
00367         }
00368     }
00369     return *this;
00370 }
00371 
00372 /**
00373  * Get number of rows.
00374  */
00375 template< typename T > size_t WMatrix< T >::getNbRows() const
00376 {
00377     return this->size() / m_nbCols;
00378 }
00379 
00380 /**
00381  * Get number of columns.
00382  */
00383 template< typename T > size_t WMatrix< T >::getNbCols() const
00384 {
00385     return m_nbCols;
00386 }
00387 
00388 /**
00389  * Returns a reference to the component an row i, columns j in order to
00390  * provide access to the component.
00391  * \param i row
00392  * \param j column
00393  */
00394 template< typename T > T& WMatrix< T >::operator()( size_t i, size_t j )
00395 {
00396     WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." );
00397     return (*this)[i * m_nbCols + j];
00398 }
00399 
00400 /**
00401  * Returns a const reference to the component an row i, columns j in order to
00402  * provide read-only access to the component.
00403  * \param i row
00404  * \param j column
00405  */
00406 template< typename T > const T& WMatrix< T >::operator()( size_t i, size_t j ) const
00407 {
00408     WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." );
00409     return (*this)[i * m_nbCols + j];
00410 }
00411 
00412 /**
00413  * Compares two matrices and returns true if they are equal.
00414  * \param rhs The right hand side of the comparison
00415  */
00416 template< typename T > bool WMatrix< T >::operator==( const WMatrix& rhs ) const
00417 {
00418     return WValue< T >::operator==( rhs ) && m_nbCols == rhs.m_nbCols;
00419 }
00420 
00421 /**
00422  * Compares two matrices and returns true if they are not equal.
00423  * \param rhs The right hand side of the comparison
00424  */
00425 template< typename T > bool WMatrix< T >::operator!=( const WMatrix& rhs ) const
00426 {
00427     return WValue< T >::operator!=( rhs ) || m_nbCols != rhs.m_nbCols;
00428 }
00429 
00430 /**
00431  * Assigns the argument WMatrix to this WMatrix.
00432  * \param rhs The right hand side of the assignment
00433  */
00434 template< typename T > WMatrix< T >& WMatrix< T >::operator=( const WMatrix& rhs )
00435 {
00436     WValue< T >::operator=( rhs );
00437     m_nbCols = rhs.m_nbCols;
00438     return *this;
00439 }
00440 
00441 /**
00442  * Returns the transposed matrix.
00443  */
00444 template< typename T > WMatrix< T > WMatrix< T >::transposed() const
00445 {
00446   WMatrix result( m_nbCols, getNbRows() );
00447 
00448   for( std::size_t i = 0; i < getNbRows(); i++ )
00449     for( std::size_t j = 0; j < m_nbCols; j++ )
00450       result( j, i ) = (*this)( i, j);
00451   return result;
00452 }
00453 
00454 template< typename T > WMatrix< T > WMatrix< T >::operator*( const WMatrix< T >& rhs ) const
00455 {
00456     WAssert( rhs.getNbRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
00457     WMatrix< T > result( getNbRows(), rhs.getNbCols() );
00458 
00459     for( size_t r = 0; r < getNbRows(); ++r)
00460     {
00461         for( size_t c = 0; c < rhs.getNbCols(); ++c )
00462         {
00463             for( size_t i = 0; i < getNbCols(); ++i )
00464             {
00465                 result( r, c ) += ( *this )( r, i ) * rhs( i, c );
00466             }
00467         }
00468     }
00469     return result;
00470 }
00471 
00472 template< typename T > WValue< T > WMatrix< T >::operator*( const WValue< T >& rhs ) const
00473 {
00474     WAssert( rhs.size() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
00475     WValue< T > result( getNbRows() );
00476 
00477     for( size_t r = 0; r < getNbRows(); ++r)
00478     {
00479         for( size_t i = 0; i < getNbCols(); ++i )
00480         {
00481             result[r] += ( *this )( r, i ) * rhs[i];
00482         }
00483     }
00484     return result;
00485 }
00486 
00487 template< typename T > WVector3d WMatrix< T >::operator*( const WVector3d& rhs ) const
00488 {
00489     WAssert( rhs.getRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
00490     WVector3d result;
00491 
00492     for( size_t r = 0; r < getNbRows(); ++r)
00493     {
00494         for( size_t i = 0; i < getNbCols(); ++i )
00495         {
00496             result[r] += ( *this )( r, i ) * rhs[i];
00497         }
00498     }
00499     return result;
00500 }
00501 
00502 template< typename T > bool WMatrix< T >::isSquare() const
00503 {
00504     return getNbRows() == getNbCols();
00505 }
00506 
00507 template< typename T > bool WMatrix< T >::isIdentity( T delta ) const
00508 {
00509     if( !isSquare() )
00510     {
00511         return false;
00512     }
00513 
00514     for( size_t row = 0; row < getNbRows(); row++ )
00515     {
00516         for( size_t col = 0; col < getNbCols(); col++ )
00517         {
00518             T val = ( *this )( row, col );
00519             T expected = ( row == col ? T( 1.0 ) : T( 0.0 ) );
00520             if( std::fabs( val - expected ) > delta )
00521             {
00522                 return false;
00523             }
00524         }
00525     }
00526     return true;
00527 }
00528 
00529 template< typename T >
00530 template< typename EigenDataType >
00531 void WMatrix< T >::copyFromEigenMatrix( const Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >& newMatrix )
00532 {
00533     m_nbCols = static_cast< size_t >( newMatrix.cols() );
00534     for( int row = 0; row < newMatrix.rows(); ++row )
00535     {
00536         for( int col = 0; col < newMatrix.cols(); ++col )
00537         {
00538             ( *this )( row, col ) = static_cast< T >( newMatrix( row, col ) );
00539         }
00540     }
00541 }
00542 
00543 template< typename T >
00544 inline std::ostream& operator<<( std::ostream& os, const WMatrix< T >& m )
00545 {
00546     os << std::setprecision( 5 ) << std::fixed;
00547     for( size_t i = 0; i < m.getNbRows(); ++i )
00548     {
00549         if( i == 0 )
00550         {
00551             os << "[ ";
00552         }
00553         else
00554         {
00555             os << "  ";
00556         }
00557         for( size_t j = 0; j < m.getNbCols(); ++j )
00558         {
00559             os << std::setw( 12 ) << m( i, j );
00560             if( j < m.getNbCols() - 1 )
00561             {
00562                 os << ", ";
00563             }
00564             else if( i < m.getNbRows() - 1 )
00565             {
00566                 os << "  ";
00567             }
00568             else
00569             {
00570                 os << " ]";
00571             }
00572         }
00573         os << std::endl;
00574     }
00575     return os;
00576 }
00577 
00578 #endif  // WMATRIX_H