33 #include "linearAlgebra/WMatrixFixed.h"
34 #include "linearAlgebra/WVectorFixed.h"
36 #include "../WDefines.h"
61 WMatrix(
size_t nbRows,
size_t nbCols );
81 WMatrix(
const Eigen::MatrixXd& newMatrix );
88 WMatrix(
const Eigen::MatrixXf& newMatrix );
95 WMatrix(
const Eigen::MatrixXi& newMatrix );
131 const T&
operator()(
size_t i,
size_t j )
const;
145 operator osg::Matrixd()
const;
154 template<
typename EigenDataType >
155 operator Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >()
const;
210 for(
size_t i = 0; i < this->
size(); ++i )
212 ( *this )[ i ] = 0.0;
236 template<
typename EigenDataType >
237 void copyFromEigenMatrix(
const Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >& newMatrix );
249 :
WValue< T >( nbRows * nbCols )
259 :
WValue< T >( newMatrix )
268 for(
size_t i = 0; i < 4; ++i )
270 for(
size_t j = 0; j < 4; ++j )
272 ( *this )( i, j ) = newMatrix( i, j );
278 :
WValue< T >( newMatrix.cols() * newMatrix.rows() )
280 copyFromEigenMatrix< double >( newMatrix );
284 :
WValue< T >( newMatrix.cols() * newMatrix.rows() )
286 copyFromEigenMatrix< float >( newMatrix );
290 :
WValue< T >( newMatrix.cols() * newMatrix.rows() )
292 copyFromEigenMatrix< int >( newMatrix );
297 size_t nbRows = this->size() / m_nbCols;
298 WAssert( m_nbCols == 4 && nbRows == 4,
"This is no 4x4 matrix." );
300 for(
size_t i = 0; i < nbRows; ++i )
302 for(
size_t j = 0; j < m_nbCols; ++j )
304 m( i, j ) = ( *this )( i, j );
312 WAssert( ( getNbRows() == 3 || getNbRows() == 4 ) && ( getNbCols() == 3 || getNbCols() == 4 ),
313 "Only 3x3 or 4x4 matrices allowed." );
316 if( getNbRows() == 4 )
318 return osg::Matrixd( ( *
this )[ 0 ], ( *
this )[ 4 ], ( *
this )[ 8 ], ( *
this )[ 12 ],
319 ( *
this )[ 1 ], ( *
this )[ 5 ], ( *
this )[ 9 ], ( *
this )[ 13 ],
320 ( *
this )[ 2 ], ( *
this )[ 6 ], ( *
this )[ 10 ], ( *
this )[ 14 ],
321 ( *
this )[ 3 ], ( *
this )[ 7 ], ( *
this )[ 11 ], ( *
this )[ 15 ]
326 return osg::Matrixd( ( *
this )[ 0 ], ( *
this )[ 1 ], ( *
this )[ 2 ], 0.0,
327 ( *
this )[ 3 ], ( *
this )[ 4 ], ( *
this )[ 5 ], 0.0,
328 ( *
this )[ 6 ], ( *
this )[ 7 ], ( *
this )[ 8 ], 0.0,
329 ( *
this )[ 9 ], ( *
this )[ 10 ], ( *
this )[ 11 ], 1.0
334 template<
typename T >
335 template<
typename EigenDataType >
WMatrix< T >::operator Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >()
const
337 Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic > matrix( this->getNbRows(), this->getNbCols() );
338 for(
int row = 0; row < matrix.rows(); ++row )
340 for(
int col = 0; col < matrix.cols(); ++col )
342 matrix( row, col ) =
static_cast< EigenDataType
>( ( *this )( row, col ) );
354 size_t nbRows = this->size() / m_nbCols;
355 for(
size_t i = 0; i < nbRows; ++i )
357 for(
size_t j = 0; j < m_nbCols; ++j )
377 return this->size() / m_nbCols;
396 WAssert( j < m_nbCols && i * m_nbCols < this->size(),
"Index out of bounds." );
397 return (*
this)[i * m_nbCols + j];
408 WAssert( j < m_nbCols && i * m_nbCols < this->size(),
"Index out of bounds." );
409 return (*
this)[i * m_nbCols + j];
446 WMatrix result( m_nbCols, getNbRows() );
448 for( std::size_t i = 0; i < getNbRows(); i++ )
449 for( std::size_t j = 0; j < m_nbCols; j++ )
450 result( j, i ) = (*this)( i, j);
456 WAssert( rhs.
getNbRows() == getNbCols(),
"Incompatible number of rows of rhs and columns of lhs." );
459 for(
size_t r = 0; r < getNbRows(); ++r)
461 for(
size_t c = 0; c < rhs.
getNbCols(); ++c )
463 for(
size_t i = 0; i < getNbCols(); ++i )
465 result( r, c ) += ( *this )( r, i ) * rhs( i, c );
474 WAssert( rhs.
size() == getNbCols(),
"Incompatible number of rows of rhs and columns of lhs." );
477 for(
size_t r = 0; r < getNbRows(); ++r)
479 for(
size_t i = 0; i < getNbCols(); ++i )
481 result[r] += ( *this )( r, i ) * rhs[i];
489 WAssert( rhs.
getRows() == getNbCols(),
"Incompatible number of rows of rhs and columns of lhs." );
492 for(
size_t r = 0; r < getNbRows(); ++r)
494 for(
size_t i = 0; i < getNbCols(); ++i )
496 result[r] += ( *this )( r, i ) * rhs[i];
504 return getNbRows() == getNbCols();
514 for(
size_t row = 0; row < getNbRows(); row++ )
516 for(
size_t col = 0; col < getNbCols(); col++ )
518 T val = ( *this )( row, col );
519 T expected = ( row == col ? T( 1.0 ) : T( 0.0 ) );
520 if( std::fabs( val - expected ) > delta )
529 template<
typename T >
530 template<
typename EigenDataType >
533 m_nbCols =
static_cast< size_t >( newMatrix.cols() );
534 for(
int row = 0; row < newMatrix.rows(); ++row )
536 for(
int col = 0; col < newMatrix.cols(); ++col )
538 ( *this )( row, col ) = static_cast< T >( newMatrix( row, col ) );
543 template<
typename T >
544 inline std::ostream& operator<<( std::ostream& os, const WMatrix< T >& m )
546 os << std::setprecision( 5 ) << std::fixed;
547 for(
size_t i = 0; i < m.getNbRows(); ++i )
557 for(
size_t j = 0; j < m.getNbCols(); ++j )
559 os << std::setw( 12 ) << m( i, j );
560 if( j < m.getNbCols() - 1 )
564 else if( i < m.getNbRows() - 1 )