OpenWalnut
1.4.0
|
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