OpenWalnut 1.3.1
|
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/WLinearAlgebra.h" 00034 00035 #include "../WDefines.h" 00036 00037 /** 00038 * Matrix template class with variable number of rows and columns. 00039 * The access function are row-major, which means that the rows 00040 * are the first parameter or index. 00041 */ 00042 template< typename T > class WMatrix : public WValue< T > 00043 { 00044 public: 00045 /** 00046 * Produces a square matrix with the given number of components. 00047 * The components will be set to zero if T is a type representing numbers. 00048 * 00049 * \param n Number of cols and rows in the matrix 00050 */ 00051 explicit WMatrix( size_t n ); 00052 00053 /** 00054 * Produces a matrix with the given number of components. 00055 * The components will be set to zero if T is a type representing numbers. 00056 * 00057 * \param nbRows number of rows in the matrix 00058 * \param nbCols number of columns in the matrix 00059 */ 00060 WMatrix( size_t nbRows, size_t nbCols ); 00061 00062 /** 00063 * Produces a matrix as copy of the one given as parameter. 00064 * \param newMatrix The matrix to be copied. 00065 */ 00066 WMatrix( const WMatrix& newMatrix ); 00067 00068 /** 00069 * Copies the specified 4x4 matrix. 00070 * 00071 * \param newMatrix the matrix to copy 00072 */ 00073 WMatrix( const WMatrix4d& newMatrix ); // NOLINT 00074 00075 /** 00076 * Copies the specified Eigen::MatrixXd. 00077 * 00078 * \param newMatrix the Eigen::MatrixXd matrix to copy 00079 */ 00080 WMatrix( const Eigen::MatrixXd& newMatrix ); // NOLINT 00081 00082 /** 00083 * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal. 00084 * \return Reference to the current matrix which is identity matrix now. 00085 */ 00086 WMatrix& makeIdentity(); 00087 00088 /** 00089 * Get number of rows. 00090 * \return Number of rows of the matrix. 00091 */ 00092 size_t getNbRows() const; 00093 00094 /** 00095 * Get number of columns. 00096 * \return Number of columns of the matrix. 00097 */ 00098 size_t getNbCols() const; 00099 00100 /** 00101 * Returns a reference to the component an row i, columns j in order to 00102 * provide access to the component. 00103 * \param i row 00104 * \param j column 00105 * \return A reference to the component (i,j) 00106 */ 00107 T& operator()( size_t i, size_t j ); 00108 00109 /** 00110 * Returns a const reference to the component an row i, columns j in order to 00111 * provide read-only access to the component. 00112 * \param i row 00113 * \param j column 00114 * \return A const reference to the component (i,j) 00115 */ 00116 const T& operator()( size_t i, size_t j ) const; 00117 00118 /** 00119 * Cast this matrix to an 4x matrix if it is a 4x4 matrix. 00120 * 00121 * \return casted matrix 00122 */ 00123 operator WMatrix4d() const; 00124 00125 /** 00126 * Cast this matrix to an 4x4 osg matrix if it is a 4x4 matrix. 00127 * 00128 * \return casted matrix. 00129 */ 00130 operator osg::Matrixd() const; 00131 00132 /** 00133 * Cast this matrix to an Eigen::MatrixXd matrix. 00134 * 00135 * \return casted matrix. 00136 */ 00137 operator Eigen::MatrixXd() const; 00138 00139 /** 00140 * Compares two matrices and returns true if they are equal. 00141 * \param rhs The right hand side of the comparison 00142 * \return Are the matrices equal? 00143 */ 00144 bool operator==( const WMatrix& rhs ) const; 00145 00146 /** 00147 * Compares two matrices and returns true if they are not equal. 00148 * \param rhs The right hand side of the comparison 00149 * \return Are the matrices NOT equal? 00150 */ 00151 bool operator!=( const WMatrix& rhs ) const; 00152 00153 /** 00154 * Assigns the argument WMatrix to this WMatrix. 00155 * \param rhs The right hand side of the assignment 00156 * \return A reference to the left hand side of the assignment (i.e. the current object). 00157 */ 00158 WMatrix& operator=( const WMatrix& rhs ); 00159 00160 /** 00161 * Multiplication of the current matrix with andother matrix. 00162 * \param rhs The right hand side of the multiplication 00163 * \return The product of the two matrices. 00164 */ 00165 WMatrix operator*( const WMatrix& rhs ) const; 00166 00167 /** 00168 * Multiplication with a vector. 00169 * \param rhs The right hand side of the multiplication 00170 * \return The product of the matrix and the vector. 00171 */ 00172 WValue< T > operator*( const WValue< T >& rhs ) const; 00173 00174 /** 00175 * Multiplication with a vector. 00176 * \param rhs The right hand side of the multiplication 00177 * \return The product of the matrix and the 3D vector. 00178 */ 00179 WVector3d operator*( const WVector3d& rhs ) const; 00180 00181 /** 00182 * Returns the transposed matrix. 00183 * \return Transposed version of the current matrix. 00184 */ 00185 WMatrix transposed() const; 00186 00187 /** 00188 * Resets the matrix components to zero. 00189 */ 00190 void setZero() 00191 { 00192 for( size_t i = 0; i < this->size(); ++i ) 00193 { 00194 ( *this )[ i ] = 0.0; 00195 } 00196 } 00197 00198 protected: 00199 private: 00200 size_t m_nbCols; //!< Number of columns of the matrix. The number of rows will be computed by (size/m_nbCols). 00201 }; 00202 00203 template< typename T > WMatrix< T >::WMatrix( size_t n ) 00204 : WValue< T >( n * n ) 00205 { 00206 m_nbCols = n; 00207 } 00208 00209 template< typename T > WMatrix< T >::WMatrix( size_t nbRows, size_t nbCols ) 00210 : WValue< T >( nbRows * nbCols ) 00211 { 00212 m_nbCols = nbCols; 00213 } 00214 00215 /** 00216 * Produces a matrix as copy of the one given as parameter. 00217 * \param newMatrix The matrix to be copied. 00218 */ 00219 template< typename T > WMatrix< T >::WMatrix( const WMatrix& newMatrix ) 00220 : WValue< T >( newMatrix ) 00221 { 00222 m_nbCols = newMatrix.m_nbCols; 00223 } 00224 00225 template< typename T > WMatrix< T >::WMatrix( const WMatrix4d& newMatrix ) 00226 : WValue< T >( 4 * 4 ) 00227 { 00228 m_nbCols = 4; 00229 for( size_t i = 0; i < 4; ++i ) 00230 { 00231 for( size_t j = 0; j < 4; ++j ) 00232 { 00233 ( *this )( i, j ) = newMatrix( i, j ); 00234 } 00235 } 00236 } 00237 00238 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXd& newMatrix ) 00239 : WValue< T >( newMatrix.cols() * newMatrix.rows() ) 00240 { 00241 m_nbCols = static_cast< size_t >( newMatrix.cols() ); 00242 for( int row = 0; row < newMatrix.rows(); ++row ) 00243 { 00244 for( int col = 0; col < newMatrix.cols(); ++col ) 00245 { 00246 ( *this )( row, col ) = static_cast< T >( newMatrix( row, col ) ); 00247 } 00248 } 00249 } 00250 00251 00252 template< typename T > WMatrix< T >::operator WMatrix4d() const 00253 { 00254 size_t nbRows = this->size() / m_nbCols; 00255 WAssert( m_nbCols == 4 && nbRows == 4, "This is no 4x4 matrix." ); 00256 WMatrix4d m; 00257 for( size_t i = 0; i < nbRows; ++i ) 00258 { 00259 for( size_t j = 0; j < m_nbCols; ++j ) 00260 { 00261 m( i, j ) = ( *this )( i, j ); 00262 } 00263 } 00264 return m; 00265 } 00266 00267 template< typename T > WMatrix< T >::operator osg::Matrixd() const 00268 { 00269 WAssert( ( getNbRows() == 3 || getNbRows() == 4 ) && ( getNbCols() == 3 || getNbCols() == 4 ), 00270 "Only 3x3 or 4x4 matrices allowed." ); 00271 00272 // handle 4x4 and 3x3 separately 00273 if( getNbRows() == 4 ) 00274 { 00275 return osg::Matrixd( ( *this )[ 0 ], ( *this )[ 4 ], ( *this )[ 8 ], ( *this )[ 12 ], 00276 ( *this )[ 1 ], ( *this )[ 5 ], ( *this )[ 9 ], ( *this )[ 13 ], 00277 ( *this )[ 2 ], ( *this )[ 6 ], ( *this )[ 10 ], ( *this )[ 14 ], 00278 ( *this )[ 3 ], ( *this )[ 7 ], ( *this )[ 11 ], ( *this )[ 15 ] 00279 ); 00280 } 00281 else 00282 { 00283 return osg::Matrixd( ( *this )[ 0 ], ( *this )[ 1 ], ( *this )[ 2 ], 0.0, 00284 ( *this )[ 3 ], ( *this )[ 4 ], ( *this )[ 5 ], 0.0, 00285 ( *this )[ 6 ], ( *this )[ 7 ], ( *this )[ 8 ], 0.0, 00286 ( *this )[ 9 ], ( *this )[ 10 ], ( *this )[ 11 ], 1.0 00287 ); 00288 } 00289 } 00290 00291 template< typename T > WMatrix< T >::operator Eigen::MatrixXd() const 00292 { 00293 Eigen::MatrixXd matrix( this->getNbRows(), this->getNbCols() ); 00294 for( int row = 0; row < matrix.rows(); ++row ) 00295 { 00296 for( int col = 0; col < matrix.cols(); ++col ) 00297 { 00298 matrix( row, col ) = ( *this )( row, col ); 00299 } 00300 } 00301 return matrix; 00302 } 00303 00304 /** 00305 * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal. 00306 */ 00307 template< typename T > WMatrix< T >& WMatrix< T >::makeIdentity() 00308 { 00309 size_t nbRows = this->size() / m_nbCols; 00310 for( size_t i = 0; i < nbRows; ++i ) 00311 { 00312 for( size_t j = 0; j < m_nbCols; ++j ) 00313 { 00314 if( i == j ) 00315 { 00316 (*this)( i, j ) = 1; 00317 } 00318 else 00319 { 00320 (*this)( i, j ) = 0; 00321 } 00322 } 00323 } 00324 return *this; 00325 } 00326 00327 /** 00328 * Get number of rows. 00329 */ 00330 template< typename T > size_t WMatrix< T >::getNbRows() const 00331 { 00332 return this->size() / m_nbCols; 00333 } 00334 00335 /** 00336 * Get number of columns. 00337 */ 00338 template< typename T > size_t WMatrix< T >::getNbCols() const 00339 { 00340 return m_nbCols; 00341 } 00342 00343 /** 00344 * Returns a reference to the component an row i, columns j in order to 00345 * provide access to the component. 00346 * \param i row 00347 * \param j column 00348 */ 00349 template< typename T > T& WMatrix< T >::operator()( size_t i, size_t j ) 00350 { 00351 WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." ); 00352 return (*this)[i * m_nbCols + j]; 00353 } 00354 00355 /** 00356 * Returns a const reference to the component an row i, columns j in order to 00357 * provide read-only access to the component. 00358 * \param i row 00359 * \param j column 00360 */ 00361 template< typename T > const T& WMatrix< T >::operator()( size_t i, size_t j ) const 00362 { 00363 WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." ); 00364 return (*this)[i * m_nbCols + j]; 00365 } 00366 00367 /** 00368 * Compares two matrices and returns true if they are equal. 00369 * \param rhs The right hand side of the comparison 00370 */ 00371 template< typename T > bool WMatrix< T >::operator==( const WMatrix& rhs ) const 00372 { 00373 return WValue< T >::operator==( rhs ) && m_nbCols == rhs.m_nbCols; 00374 } 00375 00376 /** 00377 * Compares two matrices and returns true if they are not equal. 00378 * \param rhs The right hand side of the comparison 00379 */ 00380 template< typename T > bool WMatrix< T >::operator!=( const WMatrix& rhs ) const 00381 { 00382 return WValue< T >::operator!=( rhs ) || m_nbCols != rhs.m_nbCols; 00383 } 00384 00385 /** 00386 * Assigns the argument WMatrix to this WMatrix. 00387 * \param rhs The right hand side of the assignment 00388 */ 00389 template< typename T > WMatrix< T >& WMatrix< T >::operator=( const WMatrix& rhs ) 00390 { 00391 WValue< T >::operator=( rhs ); 00392 m_nbCols = rhs.m_nbCols; 00393 return *this; 00394 } 00395 00396 /** 00397 * Returns the transposed matrix. 00398 */ 00399 template< typename T > WMatrix< T > WMatrix< T >::transposed() const 00400 { 00401 WMatrix result( m_nbCols, getNbRows() ); 00402 00403 for( std::size_t i = 0; i < getNbRows(); i++ ) 00404 for( std::size_t j = 0; j < m_nbCols; j++ ) 00405 result( j, i ) = (*this)( i, j); 00406 return result; 00407 } 00408 00409 template< typename T > WMatrix< T > WMatrix< T >::operator*( const WMatrix< T >& rhs ) const 00410 { 00411 WAssert( rhs.getNbRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." ); 00412 WMatrix< T > result( getNbRows(), rhs.getNbCols() ); 00413 00414 for( size_t r = 0; r < getNbRows(); ++r) 00415 { 00416 for( size_t c = 0; c < rhs.getNbCols(); ++c ) 00417 { 00418 for( size_t i = 0; i < getNbCols(); ++i ) 00419 { 00420 result( r, c ) += ( *this )( r, i ) * rhs( i, c ); 00421 } 00422 } 00423 } 00424 return result; 00425 } 00426 00427 template< typename T > WValue< T > WMatrix< T >::operator*( const WValue< T >& rhs ) const 00428 { 00429 WAssert( rhs.size() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." ); 00430 WValue< T > result( getNbRows() ); 00431 00432 for( size_t r = 0; r < getNbRows(); ++r) 00433 { 00434 for( size_t i = 0; i < getNbCols(); ++i ) 00435 { 00436 result[r] += ( *this )( r, i ) * rhs[i]; 00437 } 00438 } 00439 return result; 00440 } 00441 00442 template< typename T > WVector3d WMatrix< T >::operator*( const WVector3d& rhs ) const 00443 { 00444 WAssert( rhs.getRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." ); 00445 WVector3d result; 00446 00447 for( size_t r = 0; r < getNbRows(); ++r) 00448 { 00449 for( size_t i = 0; i < getNbCols(); ++i ) 00450 { 00451 result[r] += ( *this )( r, i ) * rhs[i]; 00452 } 00453 } 00454 return result; 00455 } 00456 00457 template< typename T > 00458 inline std::ostream& operator<<( std::ostream& os, const WMatrix< T >& m ) 00459 { 00460 os << std::setprecision( 5 ) << std::fixed; 00461 for( size_t i = 0; i < m.getNbRows(); ++i ) 00462 { 00463 if( i == 0 ) 00464 { 00465 os << "[ "; 00466 } 00467 else 00468 { 00469 os << " "; 00470 } 00471 for( size_t j = 0; j < m.getNbCols(); ++j ) 00472 { 00473 os << std::setw( 12 ) << m( i, j ); 00474 if( j < m.getNbCols() - 1 ) 00475 { 00476 os << ", "; 00477 } 00478 else if( i < m.getNbRows() - 1 ) 00479 { 00480 os << " "; 00481 } 00482 else 00483 { 00484 os << " ]"; 00485 } 00486 } 00487 os << std::endl; 00488 } 00489 return os; 00490 } 00491 00492 #endif // WMATRIX_H