OpenWalnut  1.3.1
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/WLinearAlgebra.h"
34 
35 #include "../WDefines.h"
36 
37 /**
38  * Matrix template class with variable number of rows and columns.
39  * The access function are row-major, which means that the rows
40  * are the first parameter or index.
41  */
42 template< typename T > class WMatrix : public WValue< T >
43 {
44 public:
45  /**
46  * Produces a square matrix with the given number of components.
47  * The components will be set to zero if T is a type representing numbers.
48  *
49  * \param n Number of cols and rows in the matrix
50  */
51  explicit WMatrix( size_t n );
52 
53  /**
54  * Produces a matrix with the given number of components.
55  * The components will be set to zero if T is a type representing numbers.
56  *
57  * \param nbRows number of rows in the matrix
58  * \param nbCols number of columns in the matrix
59  */
60  WMatrix( size_t nbRows, size_t nbCols );
61 
62  /**
63  * Produces a matrix as copy of the one given as parameter.
64  * \param newMatrix The matrix to be copied.
65  */
66  WMatrix( const WMatrix& newMatrix );
67 
68  /**
69  * Copies the specified 4x4 matrix.
70  *
71  * \param newMatrix the matrix to copy
72  */
73  WMatrix( const WMatrix4d& newMatrix ); // NOLINT
74 
75  /**
76  * Copies the specified Eigen::MatrixXd.
77  *
78  * \param newMatrix the Eigen::MatrixXd matrix to copy
79  */
80  WMatrix( const Eigen::MatrixXd& newMatrix ); // NOLINT
81 
82  /**
83  * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal.
84  * \return Reference to the current matrix which is identity matrix now.
85  */
87 
88  /**
89  * Get number of rows.
90  * \return Number of rows of the matrix.
91  */
92  size_t getNbRows() const;
93 
94  /**
95  * Get number of columns.
96  * \return Number of columns of the matrix.
97  */
98  size_t getNbCols() const;
99 
100  /**
101  * Returns a reference to the component an row i, columns j in order to
102  * provide access to the component.
103  * \param i row
104  * \param j column
105  * \return A reference to the component (i,j)
106  */
107  T& operator()( size_t i, size_t j );
108 
109  /**
110  * Returns a const reference to the component an row i, columns j in order to
111  * provide read-only access to the component.
112  * \param i row
113  * \param j column
114  * \return A const reference to the component (i,j)
115  */
116  const T& operator()( size_t i, size_t j ) const;
117 
118  /**
119  * Cast this matrix to an 4x matrix if it is a 4x4 matrix.
120  *
121  * \return casted matrix
122  */
123  operator WMatrix4d() const;
124 
125  /**
126  * Cast this matrix to an 4x4 osg matrix if it is a 4x4 matrix.
127  *
128  * \return casted matrix.
129  */
130  operator osg::Matrixd() const;
131 
132  /**
133  * Cast this matrix to an Eigen::MatrixXd matrix.
134  *
135  * \return casted matrix.
136  */
137  operator Eigen::MatrixXd() const;
138 
139  /**
140  * Compares two matrices and returns true if they are equal.
141  * \param rhs The right hand side of the comparison
142  * \return Are the matrices equal?
143  */
144  bool operator==( const WMatrix& rhs ) const;
145 
146  /**
147  * Compares two matrices and returns true if they are not equal.
148  * \param rhs The right hand side of the comparison
149  * \return Are the matrices NOT equal?
150  */
151  bool operator!=( const WMatrix& rhs ) const;
152 
153  /**
154  * Assigns the argument WMatrix to this WMatrix.
155  * \param rhs The right hand side of the assignment
156  * \return A reference to the left hand side of the assignment (i.e. the current object).
157  */
158  WMatrix& operator=( const WMatrix& rhs );
159 
160  /**
161  * Multiplication of the current matrix with andother matrix.
162  * \param rhs The right hand side of the multiplication
163  * \return The product of the two matrices.
164  */
165  WMatrix operator*( const WMatrix& rhs ) const;
166 
167  /**
168  * Multiplication with a vector.
169  * \param rhs The right hand side of the multiplication
170  * \return The product of the matrix and the vector.
171  */
172  WValue< T > operator*( const WValue< T >& rhs ) const;
173 
174  /**
175  * Multiplication with a vector.
176  * \param rhs The right hand side of the multiplication
177  * \return The product of the matrix and the 3D vector.
178  */
179  WVector3d operator*( const WVector3d& rhs ) const;
180 
181  /**
182  * Returns the transposed matrix.
183  * \return Transposed version of the current matrix.
184  */
185  WMatrix transposed() const;
186 
187  /**
188  * Resets the matrix components to zero.
189  */
190  void setZero()
191  {
192  for( size_t i = 0; i < this->size(); ++i )
193  {
194  ( *this )[ i ] = 0.0;
195  }
196  }
197 
198 protected:
199 private:
200  size_t m_nbCols; //!< Number of columns of the matrix. The number of rows will be computed by (size/m_nbCols).
201 };
202 
203 template< typename T > WMatrix< T >::WMatrix( size_t n )
204  : WValue< T >( n * n )
205 {
206  m_nbCols = n;
207 }
208 
209 template< typename T > WMatrix< T >::WMatrix( size_t nbRows, size_t nbCols )
210  : WValue< T >( nbRows * nbCols )
211 {
212  m_nbCols = nbCols;
213 }
214 
215 /**
216  * Produces a matrix as copy of the one given as parameter.
217  * \param newMatrix The matrix to be copied.
218  */
219 template< typename T > WMatrix< T >::WMatrix( const WMatrix& newMatrix )
220  : WValue< T >( newMatrix )
221 {
222  m_nbCols = newMatrix.m_nbCols;
223 }
224 
225 template< typename T > WMatrix< T >::WMatrix( const WMatrix4d& newMatrix )
226  : WValue< T >( 4 * 4 )
227 {
228  m_nbCols = 4;
229  for( size_t i = 0; i < 4; ++i )
230  {
231  for( size_t j = 0; j < 4; ++j )
232  {
233  ( *this )( i, j ) = newMatrix( i, j );
234  }
235  }
236 }
237 
238 template< typename T > WMatrix< T >::WMatrix( const Eigen::MatrixXd& newMatrix )
239  : WValue< T >( newMatrix.cols() * newMatrix.rows() )
240 {
241  m_nbCols = static_cast< size_t >( newMatrix.cols() );
242  for( int row = 0; row < newMatrix.rows(); ++row )
243  {
244  for( int col = 0; col < newMatrix.cols(); ++col )
245  {
246  ( *this )( row, col ) = static_cast< T >( newMatrix( row, col ) );
247  }
248  }
249 }
250 
251 
252 template< typename T > WMatrix< T >::operator WMatrix4d() const
253 {
254  size_t nbRows = this->size() / m_nbCols;
255  WAssert( m_nbCols == 4 && nbRows == 4, "This is no 4x4 matrix." );
256  WMatrix4d m;
257  for( size_t i = 0; i < nbRows; ++i )
258  {
259  for( size_t j = 0; j < m_nbCols; ++j )
260  {
261  m( i, j ) = ( *this )( i, j );
262  }
263  }
264  return m;
265 }
266 
267 template< typename T > WMatrix< T >::operator osg::Matrixd() const
268 {
269  WAssert( ( getNbRows() == 3 || getNbRows() == 4 ) && ( getNbCols() == 3 || getNbCols() == 4 ),
270  "Only 3x3 or 4x4 matrices allowed." );
271 
272  // handle 4x4 and 3x3 separately
273  if( getNbRows() == 4 )
274  {
275  return osg::Matrixd( ( *this )[ 0 ], ( *this )[ 4 ], ( *this )[ 8 ], ( *this )[ 12 ],
276  ( *this )[ 1 ], ( *this )[ 5 ], ( *this )[ 9 ], ( *this )[ 13 ],
277  ( *this )[ 2 ], ( *this )[ 6 ], ( *this )[ 10 ], ( *this )[ 14 ],
278  ( *this )[ 3 ], ( *this )[ 7 ], ( *this )[ 11 ], ( *this )[ 15 ]
279  );
280  }
281  else
282  {
283  return osg::Matrixd( ( *this )[ 0 ], ( *this )[ 1 ], ( *this )[ 2 ], 0.0,
284  ( *this )[ 3 ], ( *this )[ 4 ], ( *this )[ 5 ], 0.0,
285  ( *this )[ 6 ], ( *this )[ 7 ], ( *this )[ 8 ], 0.0,
286  ( *this )[ 9 ], ( *this )[ 10 ], ( *this )[ 11 ], 1.0
287  );
288  }
289 }
290 
291 template< typename T > WMatrix< T >::operator Eigen::MatrixXd() const
292 {
293  Eigen::MatrixXd matrix( this->getNbRows(), this->getNbCols() );
294  for( int row = 0; row < matrix.rows(); ++row )
295  {
296  for( int col = 0; col < matrix.cols(); ++col )
297  {
298  matrix( row, col ) = ( *this )( row, col );
299  }
300  }
301  return matrix;
302 }
303 
304 /**
305  * Makes the matrix contain the identity matrix, i.e. 1 on the diagonal.
306  */
307 template< typename T > WMatrix< T >& WMatrix< T >::makeIdentity()
308 {
309  size_t nbRows = this->size() / m_nbCols;
310  for( size_t i = 0; i < nbRows; ++i )
311  {
312  for( size_t j = 0; j < m_nbCols; ++j )
313  {
314  if( i == j )
315  {
316  (*this)( i, j ) = 1;
317  }
318  else
319  {
320  (*this)( i, j ) = 0;
321  }
322  }
323  }
324  return *this;
325 }
326 
327 /**
328  * Get number of rows.
329  */
330 template< typename T > size_t WMatrix< T >::getNbRows() const
331 {
332  return this->size() / m_nbCols;
333 }
334 
335 /**
336  * Get number of columns.
337  */
338 template< typename T > size_t WMatrix< T >::getNbCols() const
339 {
340  return m_nbCols;
341 }
342 
343 /**
344  * Returns a reference to the component an row i, columns j in order to
345  * provide access to the component.
346  * \param i row
347  * \param j column
348  */
349 template< typename T > T& WMatrix< T >::operator()( size_t i, size_t j )
350 {
351  WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." );
352  return (*this)[i * m_nbCols + j];
353 }
354 
355 /**
356  * Returns a const reference to the component an row i, columns j in order to
357  * provide read-only access to the component.
358  * \param i row
359  * \param j column
360  */
361 template< typename T > const T& WMatrix< T >::operator()( size_t i, size_t j ) const
362 {
363  WAssert( j < m_nbCols && i * m_nbCols < this->size(), "Index out of bounds." );
364  return (*this)[i * m_nbCols + j];
365 }
366 
367 /**
368  * Compares two matrices and returns true if they are equal.
369  * \param rhs The right hand side of the comparison
370  */
371 template< typename T > bool WMatrix< T >::operator==( const WMatrix& rhs ) const
372 {
373  return WValue< T >::operator==( rhs ) && m_nbCols == rhs.m_nbCols;
374 }
375 
376 /**
377  * Compares two matrices and returns true if they are not equal.
378  * \param rhs The right hand side of the comparison
379  */
380 template< typename T > bool WMatrix< T >::operator!=( const WMatrix& rhs ) const
381 {
382  return WValue< T >::operator!=( rhs ) || m_nbCols != rhs.m_nbCols;
383 }
384 
385 /**
386  * Assigns the argument WMatrix to this WMatrix.
387  * \param rhs The right hand side of the assignment
388  */
389 template< typename T > WMatrix< T >& WMatrix< T >::operator=( const WMatrix& rhs )
390 {
391  WValue< T >::operator=( rhs );
392  m_nbCols = rhs.m_nbCols;
393  return *this;
394 }
395 
396 /**
397  * Returns the transposed matrix.
398  */
399 template< typename T > WMatrix< T > WMatrix< T >::transposed() const
400 {
401  WMatrix result( m_nbCols, getNbRows() );
402 
403  for( std::size_t i = 0; i < getNbRows(); i++ )
404  for( std::size_t j = 0; j < m_nbCols; j++ )
405  result( j, i ) = (*this)( i, j);
406  return result;
407 }
408 
409 template< typename T > WMatrix< T > WMatrix< T >::operator*( const WMatrix< T >& rhs ) const
410 {
411  WAssert( rhs.getNbRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
412  WMatrix< T > result( getNbRows(), rhs.getNbCols() );
413 
414  for( size_t r = 0; r < getNbRows(); ++r)
415  {
416  for( size_t c = 0; c < rhs.getNbCols(); ++c )
417  {
418  for( size_t i = 0; i < getNbCols(); ++i )
419  {
420  result( r, c ) += ( *this )( r, i ) * rhs( i, c );
421  }
422  }
423  }
424  return result;
425 }
426 
427 template< typename T > WValue< T > WMatrix< T >::operator*( const WValue< T >& rhs ) const
428 {
429  WAssert( rhs.size() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
430  WValue< T > result( getNbRows() );
431 
432  for( size_t r = 0; r < getNbRows(); ++r)
433  {
434  for( size_t i = 0; i < getNbCols(); ++i )
435  {
436  result[r] += ( *this )( r, i ) * rhs[i];
437  }
438  }
439  return result;
440 }
441 
442 template< typename T > WVector3d WMatrix< T >::operator*( const WVector3d& rhs ) const
443 {
444  WAssert( rhs.getRows() == getNbCols(), "Incompatible number of rows of rhs and columns of lhs." );
445  WVector3d result;
446 
447  for( size_t r = 0; r < getNbRows(); ++r)
448  {
449  for( size_t i = 0; i < getNbCols(); ++i )
450  {
451  result[r] += ( *this )( r, i ) * rhs[i];
452  }
453  }
454  return result;
455 }
456 
457 template< typename T >
458 inline std::ostream& operator<<( std::ostream& os, const WMatrix< T >& m )
459 {
460  os << std::setprecision( 5 ) << std::fixed;
461  for( size_t i = 0; i < m.getNbRows(); ++i )
462  {
463  if( i == 0 )
464  {
465  os << "[ ";
466  }
467  else
468  {
469  os << " ";
470  }
471  for( size_t j = 0; j < m.getNbCols(); ++j )
472  {
473  os << std::setw( 12 ) << m( i, j );
474  if( j < m.getNbCols() - 1 )
475  {
476  os << ", ";
477  }
478  else if( i < m.getNbRows() - 1 )
479  {
480  os << " ";
481  }
482  else
483  {
484  os << " ]";
485  }
486  }
487  os << std::endl;
488  }
489  return os;
490 }
491 
492 #endif // WMATRIX_H