00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00039
00040
00041
00042 template< typename T > class WMatrix : public WValue< T >
00043 {
00044 public:
00045
00046
00047
00048
00049
00050
00051 explicit WMatrix( size_t n );
00052
00053
00054
00055
00056
00057
00058
00059
00060 WMatrix( size_t nbRows, size_t nbCols );
00061
00062
00063
00064
00065
00066 WMatrix( const WMatrix& newMatrix );
00067
00068
00069
00070
00071
00072
00073 WMatrix( const WMatrix4d& newMatrix );
00074
00075
00076
00077
00078
00079
00080 WMatrix( const Eigen::MatrixXd& newMatrix );
00081
00082
00083
00084
00085
00086 WMatrix& makeIdentity();
00087
00088
00089
00090
00091
00092 size_t getNbRows() const;
00093
00094
00095
00096
00097
00098 size_t getNbCols() const;
00099
00100
00101
00102
00103
00104
00105
00106
00107 T& operator()( size_t i, size_t j );
00108
00109
00110
00111
00112
00113
00114
00115
00116 const T& operator()( size_t i, size_t j ) const;
00117
00118
00119
00120
00121
00122
00123 operator WMatrix4d() const;
00124
00125
00126
00127
00128
00129
00130 operator osg::Matrixd() const;
00131
00132
00133
00134
00135
00136
00137 operator Eigen::MatrixXd() const;
00138
00139
00140
00141
00142
00143
00144 bool operator==( const WMatrix& rhs ) const;
00145
00146
00147
00148
00149
00150
00151 bool operator!=( const WMatrix& rhs ) const;
00152
00153
00154
00155
00156
00157
00158 WMatrix& operator=( const WMatrix& rhs );
00159
00160
00161
00162
00163
00164
00165 WMatrix operator*( const WMatrix& rhs ) const;
00166
00167
00168
00169
00170
00171
00172 WValue< T > operator*( const WValue< T >& rhs ) const;
00173
00174
00175
00176
00177
00178
00179 WVector3d operator*( const WVector3d& rhs ) const;
00180
00181
00182
00183
00184
00185 WMatrix transposed() const;
00186
00187
00188
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;
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
00217
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
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
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
00329
00330 template< typename T > size_t WMatrix< T >::getNbRows() const
00331 {
00332 return this->size() / m_nbCols;
00333 }
00334
00335
00336
00337
00338 template< typename T > size_t WMatrix< T >::getNbCols() const
00339 {
00340 return m_nbCols;
00341 }
00342
00343
00344
00345
00346
00347
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
00357
00358
00359
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
00369
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
00378
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
00387
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
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