OpenWalnut  1.4.0
WMatrix.h
1 //---------------------------------------------------------------------------
2 //
3 // Project: OpenWalnut ( http://www.openwalnut.org )
4 //
5 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
6 // For more information see http://www.openwalnut.org/copying
7 //
8 // This file is part of OpenWalnut.
9 //
10 // OpenWalnut is free software: you can redistribute it and/or modify
11 // it under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 3 of the License, or
13 // (at your option) any later version.
14 //
15 // OpenWalnut is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
22 //
23 //---------------------------------------------------------------------------
24 
25 #ifndef WMATRIX_H
26 #define WMATRIX_H
27 
28 #include <iostream>
29 
30 #include <osg/Matrix>
31 
32 #include "WValue.h"
33 #include "linearAlgebra/WMatrixFixed.h"
34 #include "linearAlgebra/WVectorFixed.h"
35 
36 #include "../WDefines.h"
37 
38 /**
39  * Matrix template class with variable number of rows and columns.
40  * The access function are row-major, which means that the rows
41  * are the first parameter or index.
42  */
43 template< typename T > class WMatrix : public WValue< T >
44 {
45 public:
46  /**
47  * Produces a square matrix with the given number of components.
48  * The components will be set to zero if T is a type representing numbers.
49  *
50  * \param n Number of cols and rows in the matrix
51  */
52  explicit WMatrix( size_t n );
53 
54  /**
55  * Produces a matrix with the given number of components.
56  * The components will be set to zero if T is a type representing numbers.
57  *
58  * \param nbRows number of rows in the matrix
59  * \param nbCols number of columns in the matrix
60  */
61  WMatrix( size_t nbRows, size_t nbCols );
62 
63  /**
64  * Produces a matrix as copy of the one given as parameter.
65  * \param newMatrix The matrix to be copied.
66  */
67  WMatrix( const WMatrix& newMatrix );
68 
69  /**
70  * Copies the specified 4x4 matrix.
71  *
72  * \param newMatrix the matrix to copy
73  */
74  WMatrix( const WMatrix4d& newMatrix ); // NOLINT
75 
76  /**
77  * Copies the specified Eigen::MatrixXd.
78  *
79  * \param newMatrix the Eigen::MatrixXd matrix to copy
80  */
81  WMatrix( const Eigen::MatrixXd& newMatrix ); // NOLINT
82 
83  /**
84  * Copies the specified Eigen::MatrixXf.
85  *
86  * \param newMatrix the Eigen::MatrixXf matrix to copy
87  */
88  WMatrix( const Eigen::MatrixXf& newMatrix ); // NOLINT
89 
90  /**
91  * Copies the specified Eigen::MatrixXi.
92  *
93  * \param newMatrix the Eigen::MatrixXi matrix to copy
94  */
95  WMatrix( const Eigen::MatrixXi& newMatrix ); // NOLINT
96 
97  /**
98  * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal.
99  * \return Reference to the current matrix which is identity matrix now.
100  */
102 
103  /**
104  * Get number of rows.
105  * \return Number of rows of the matrix.
106  */
107  size_t getNbRows() const;
108 
109  /**
110  * Get number of columns.
111  * \return Number of columns of the matrix.
112  */
113  size_t getNbCols() const;
114 
115  /**
116  * Returns a reference to the component an row i, columns j in order to
117  * provide access to the component.
118  * \param i row
119  * \param j column
120  * \return A reference to the component (i,j)
121  */
122  T& operator()( size_t i, size_t j );
123 
124  /**
125  * Returns a const reference to the component an row i, columns j in order to
126  * provide read-only access to the component.
127  * \param i row
128  * \param j column
129  * \return A const reference to the component (i,j)
130  */
131  const T& operator()( size_t i, size_t j ) const;
132 
133  /**
134  * Cast this matrix to an 4x matrix if it is a 4x4 matrix.
135  *
136  * \return casted matrix
137  */
138  operator WMatrix4d() const;
139 
140  /**
141  * Cast this matrix to an 4x4 osg matrix if it is a 4x4 matrix.
142  *
143  * \return casted matrix.
144  */
145  operator osg::Matrixd() const;
146 
147  /**
148  * Cast this matrix to an Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >() matrix.
149  *
150  * \tparam EigenDataType Data type of Eigen matrix.
151  *
152  * \return casted matrix.
153  */
154  template< typename EigenDataType >
155  operator Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >() const;
156 
157  /**
158  * Compares two matrices and returns true if they are equal.
159  * \param rhs The right hand side of the comparison
160  * \return Are the matrices equal?
161  */
162  bool operator==( const WMatrix& rhs ) const;
163 
164  /**
165  * Compares two matrices and returns true if they are not equal.
166  * \param rhs The right hand side of the comparison
167  * \return Are the matrices NOT equal?
168  */
169  bool operator!=( const WMatrix& rhs ) const;
170 
171  /**
172  * Assigns the argument WMatrix to this WMatrix.
173  * \param rhs The right hand side of the assignment
174  * \return A reference to the left hand side of the assignment (i.e. the current object).
175  */
176  WMatrix& operator=( const WMatrix& rhs );
177 
178  /**
179  * Multiplication of the current matrix with andother matrix.
180  * \param rhs The right hand side of the multiplication
181  * \return The product of the two matrices.
182  */
183  WMatrix operator*( const WMatrix& rhs ) const;
184 
185  /**
186  * Multiplication with a vector.
187  * \param rhs The right hand side of the multiplication
188  * \return The product of the matrix and the vector.
189  */
190  WValue< T > operator*( const WValue< T >& rhs ) const;
191 
192  /**
193  * Multiplication with a vector.
194  * \param rhs The right hand side of the multiplication
195  * \return The product of the matrix and the 3D vector.
196  */
197  WVector3d operator*( const WVector3d& rhs ) const;
198 
199  /**
200  * Returns the transposed matrix.
201  * \return Transposed version of the current matrix.
202  */
203  WMatrix transposed() const;
204 
205  /**
206  * Resets the matrix components to zero.
207  */
208  void setZero()
209  {
210  for( size_t i = 0; i < this->size(); ++i )
211  {
212  ( *this )[ i ] = 0.0;
213  }
214  }
215 
216  /**
217  * Returns true if the matrix is a square matrix.
218  * \return true for square matrixes, otherwise false.
219  */
220  bool isSquare() const;
221 
222  /**
223  * Returns true if the matrix is a identity matrix.
224  * \param delta - tolerance parameter when checking the values.
225  * \return true for identity matrixes, otherwise false.
226  */
227  bool isIdentity( T delta = T( 0.0 ) ) const;
228 
229 protected:
230 private:
231  /**
232  * This function is used by the constructors that have the different Eigen::MatrixX types as parameter.
233  * \tparam EigenDataType The data type which is used by the Eigen matrix.
234  * \param newMatrix The source matrix.
235  */
236  template< typename EigenDataType >
237  void copyFromEigenMatrix( const Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >& newMatrix );
238 
239  size_t m_nbCols; //!< Number of columns of the matrix. The number of rows will be computed by (size/m_nbCols).
240 };
241 
242 template< typename T > WMatrix< T >::WMatrix( size_t n )
243  : WValue< T >( n * n )
244 {
245  m_nbCols = n;
246 }
247 
248 template< typename T > WMatrix< T >::WMatrix( size_t nbRows, size_t nbCols )
249  : WValue< T >( nbRows * nbCols )
250 {
251  m_nbCols = nbCols;
252 }
253 
254 /**
255  * Produces a matrix as copy of the one given as parameter.
256  * \param newMatrix The matrix to be copied.
257  */
258 template< typename T > WMatrix< T >::WMatrix( const WMatrix& newMatrix )
259  : WValue< T >( newMatrix )
260 {
261  m_nbCols = newMatrix.m_nbCols;
262 }
263 
264 template< typename T > WMatrix< T >::WMatrix( const WMatrix4d& newMatrix )
265  : WValue< T >( 4 * 4 )
266 {
267  m_nbCols = 4;
268  for( size_t i = 0; i < 4; ++i )
269  {
270  for( size_t j = 0; j < 4; ++j )
271  {
272  ( *this )( i, j ) = newMatrix( i, j );
273  }
274  }
275 }
276 
277 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXd& newMatrix )
278  : WValue< T >( newMatrix.cols() * newMatrix.rows() )
279 {
280  copyFromEigenMatrix< double >( newMatrix );
281 }
282 
283 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXf& newMatrix )
284  : WValue< T >( newMatrix.cols() * newMatrix.rows() )
285 {
286  copyFromEigenMatrix< float >( newMatrix );
287 }
288 
289 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXi& newMatrix )
290  : WValue< T >( newMatrix.cols() * newMatrix.rows() )
291 {
292  copyFromEigenMatrix< int >( newMatrix );
293 }
294 
295 template< typename T > WMatrix< T >::operator WMatrix4d() const
296 {
297  size_t nbRows = this->size() / m_nbCols;
298  WAssert( m_nbCols == 4 && nbRows == 4, "This is no 4x4 matrix." );
299  WMatrix4d m;
300  for( size_t i = 0; i < nbRows; ++i )
301  {
302  for( size_t j = 0; j < m_nbCols; ++j )
303  {
304  m( i, j ) = ( *this )( i, j );
305  }
306  }
307  return m;
308 }
309 
310 template< typename T > WMatrix< T >::operator osg::Matrixd() const
311 {
312  WAssert( ( getNbRows() == 3 || getNbRows() == 4 ) && ( getNbCols() == 3 || getNbCols() == 4 ),
313  "Only 3x3 or 4x4 matrices allowed." );
314 
315  // handle 4x4 and 3x3 separately
316  if( getNbRows() == 4 )
317  {
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 ]
322  );
323  }
324  else
325  {
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
330  );
331  }
332 }
333 
334 template< typename T >
335 template< typename EigenDataType > WMatrix< T >::operator Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >() const
336 {
337  Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic > matrix( this->getNbRows(), this->getNbCols() );
338  for( int row = 0; row < matrix.rows(); ++row )
339  {
340  for( int col = 0; col < matrix.cols(); ++col )
341  {
342  matrix( row, col ) = static_cast< EigenDataType >( ( *this )( row, col ) );
343  }
344  }
345  return matrix;
346 }
347 
348 
349 /**
350  * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal.
351  */
352 template< typename T > WMatrix< T >& WMatrix< T >::makeIdentity()
353 {
354  size_t nbRows = this->size() / m_nbCols;
355  for( size_t i = 0; i < nbRows; ++i )
356  {
357  for( size_t j = 0; j < m_nbCols; ++j )
358  {
359  if( i == j )
360  {
361  (*this)( i, j ) = 1;
362  }
363  else
364  {
365  (*this)( i, j ) = 0;
366  }
367  }
368  }
369  return *this;
370 }
371 
372 /**
373  * Get number of rows.
374  */
375 template< typename T > size_t WMatrix< T >::getNbRows() const
376 {
377  return this->size() / m_nbCols;
378 }
379 
380 /**
381  * Get number of columns.
382  */
383 template< typename T > size_t WMatrix< T >::getNbCols() const
384 {
385  return m_nbCols;
386 }
387 
388 /**
389  * Returns a reference to the component an row i, columns j in order to
390  * provide access to the component.
391  * \param i row
392  * \param j column
393  */
394 template< typename T > T& WMatrix< T >::operator()( size_t i, size_t j )
395 {
396  WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." );
397  return (*this)[i * m_nbCols + j];
398 }
399 
400 /**
401  * Returns a const reference to the component an row i, columns j in order to
402  * provide read-only access to the component.
403  * \param i row
404  * \param j column
405  */
406 template< typename T > const T& WMatrix< T >::operator()( size_t i, size_t j ) const
407 {
408  WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." );
409  return (*this)[i * m_nbCols + j];
410 }
411 
412 /**
413  * Compares two matrices and returns true if they are equal.
414  * \param rhs The right hand side of the comparison
415  */
416 template< typename T > bool WMatrix< T >::operator==( const WMatrix& rhs ) const
417 {
418  return WValue< T >::operator==( rhs ) && m_nbCols == rhs.m_nbCols;
419 }
420 
421 /**
422  * Compares two matrices and returns true if they are not equal.
423  * \param rhs The right hand side of the comparison
424  */
425 template< typename T > bool WMatrix< T >::operator!=( const WMatrix& rhs ) const
426 {
427  return WValue< T >::operator!=( rhs ) || m_nbCols != rhs.m_nbCols;
428 }
429 
430 /**
431  * Assigns the argument WMatrix to this WMatrix.
432  * \param rhs The right hand side of the assignment
433  */
434 template< typename T > WMatrix< T >& WMatrix< T >::operator=( const WMatrix& rhs )
435 {
436  WValue< T >::operator=( rhs );
437  m_nbCols = rhs.m_nbCols;
438  return *this;
439 }
440 
441 /**
442  * Returns the transposed matrix.
443  */
444 template< typename T > WMatrix< T > WMatrix< T >::transposed() const
445 {
446  WMatrix result( m_nbCols, getNbRows() );
447 
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);
451  return result;
452 }
453 
454 template< typename T > WMatrix< T > WMatrix< T >::operator*( const WMatrix< T >& rhs ) const
455 {
456  WAssert( rhs.getNbRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
457  WMatrix< T > result( getNbRows(), rhs.getNbCols() );
458 
459  for( size_t r = 0; r < getNbRows(); ++r)
460  {
461  for( size_t c = 0; c < rhs.getNbCols(); ++c )
462  {
463  for( size_t i = 0; i < getNbCols(); ++i )
464  {
465  result( r, c ) += ( *this )( r, i ) * rhs( i, c );
466  }
467  }
468  }
469  return result;
470 }
471 
472 template< typename T > WValue< T > WMatrix< T >::operator*( const WValue< T >& rhs ) const
473 {
474  WAssert( rhs.size() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
475  WValue< T > result( getNbRows() );
476 
477  for( size_t r = 0; r < getNbRows(); ++r)
478  {
479  for( size_t i = 0; i < getNbCols(); ++i )
480  {
481  result[r] += ( *this )( r, i ) * rhs[i];
482  }
483  }
484  return result;
485 }
486 
487 template< typename T > WVector3d WMatrix< T >::operator*( const WVector3d& rhs ) const
488 {
489  WAssert( rhs.getRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
490  WVector3d result;
491 
492  for( size_t r = 0; r < getNbRows(); ++r)
493  {
494  for( size_t i = 0; i < getNbCols(); ++i )
495  {
496  result[r] += ( *this )( r, i ) * rhs[i];
497  }
498  }
499  return result;
500 }
501 
502 template< typename T > bool WMatrix< T >::isSquare() const
503 {
504  return getNbRows() == getNbCols();
505 }
506 
507 template< typename T > bool WMatrix< T >::isIdentity( T delta ) const
508 {
509  if( !isSquare() )
510  {
511  return false;
512  }
513 
514  for( size_t row = 0; row < getNbRows(); row++ )
515  {
516  for( size_t col = 0; col < getNbCols(); col++ )
517  {
518  T val = ( *this )( row, col );
519  T expected = ( row == col ? T( 1.0 ) : T( 0.0 ) );
520  if( std::fabs( val - expected ) > delta )
521  {
522  return false;
523  }
524  }
525  }
526  return true;
527 }
528 
529 template< typename T >
530 template< typename EigenDataType >
531 void WMatrix< T >::copyFromEigenMatrix( const Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic >& newMatrix )
532 {
533  m_nbCols = static_cast< size_t >( newMatrix.cols() );
534  for( int row = 0; row < newMatrix.rows(); ++row )
535  {
536  for( int col = 0; col < newMatrix.cols(); ++col )
537  {
538  ( *this )( row, col ) = static_cast< T >( newMatrix( row, col ) );
539  }
540  }
541 }
542 
543 template< typename T >
544 inline std::ostream& operator<<( std::ostream& os, const WMatrix< T >& m )
545 {
546  os << std::setprecision( 5 ) << std::fixed;
547  for( size_t i = 0; i < m.getNbRows(); ++i )
548  {
549  if( i == 0 )
550  {
551  os << "[ ";
552  }
553  else
554  {
555  os << " ";
556  }
557  for( size_t j = 0; j < m.getNbCols(); ++j )
558  {
559  os << std::setw( 12 ) << m( i, j );
560  if( j < m.getNbCols() - 1 )
561  {
562  os << ", ";
563  }
564  else if( i < m.getNbRows() - 1 )
565  {
566  os << " ";
567  }
568  else
569  {
570  os << " ]";
571  }
572  }
573  os << std::endl;
574  }
575  return os;
576 }
577 
578 #endif // WMATRIX_H
WMatrix & makeIdentity()
Makes the matrix contain the identity matrix, i.e.
Definition: WMatrix.h:352
WMatrix(size_t n)
Produces a square matrix with the given number of components.
Definition: WMatrix.h:242
size_t getRows() const
The number of rows.
Definition: WMatrixFixed.h:183
bool operator==(const WValue &rhs) const
Compares two WValues and returns true if they contain the same data.
Definition: WValue.h:150
Base class for all higher level values like tensors, vectors, matrices and so on. ...
Definition: WValue.h:40
size_t getNbCols() const
Get number of columns.
Definition: WMatrix.h:383
WMatrix operator*(const WMatrix &rhs) const
Multiplication of the current matrix with andother matrix.
Definition: WMatrix.h:454
WValue & operator=(const WValue &rhs)
Assigns the contents of its argument to the contents of this WValue.
Definition: WValue.h:170
bool operator!=(const WValue &rhs) const
Compares two WValues and returns true if they contain the different data.
Definition: WValue.h:160
T & operator()(size_t i, size_t j)
Returns a reference to the component an row i, columns j in order to provide access to the component...
Definition: WMatrix.h:394
Matrix template class with variable number of rows and columns.
void copyFromEigenMatrix(const Eigen::Matrix< EigenDataType, Eigen::Dynamic, Eigen::Dynamic > &newMatrix)
This function is used by the constructors that have the different Eigen::MatrixX types as parameter...
Definition: WMatrix.h:531
bool operator!=(const WMatrix &rhs) const
Compares two matrices and returns true if they are not equal.
Definition: WMatrix.h:425
bool operator==(const WMatrix &rhs) const
Compares two matrices and returns true if they are equal.
Definition: WMatrix.h:416
bool isSquare() const
Returns true if the matrix is a square matrix.
Definition: WMatrix.h:502
WMatrix transposed() const
Returns the transposed matrix.
Definition: WMatrix.h:444
void setZero()
Resets the matrix components to zero.
Definition: WMatrix.h:208
WMatrix & operator=(const WMatrix &rhs)
Assigns the argument WMatrix to this WMatrix.
Definition: WMatrix.h:434
size_t m_nbCols
Number of columns of the matrix. The number of rows will be computed by (size/m_nbCols).
Definition: WMatrix.h:239
bool isIdentity(T delta=T(0.0)) const
Returns true if the matrix is a identity matrix.
Definition: WMatrix.h:507
size_t size() const
Get number of components the value consists of.
Definition: WValue.h:116
size_t getNbRows() const
Get number of rows.
Definition: WMatrix.h:375