OpenWalnut  1.2.5
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
WMatrixFixed.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 WMATRIXFIXED_H
26 #define WMATRIXFIXED_H
27 
28 #include <string>
29 #include <algorithm>
30 
31 #include <boost/static_assert.hpp>
32 #include <boost/lexical_cast.hpp>
33 #include <boost/tokenizer.hpp>
34 
35 // Needed for conversion: OSG Types
36 #include <osg/Vec3>
37 #include <osg/Vec2d>
38 #include <osg/Vec2f>
39 #include <osg/Vec3d>
40 #include <osg/Vec3f>
41 #include <osg/Vec4d>
42 #include <osg/Vec4f>
43 #include <osg/Matrixd>
44 
45 // Needed for conversion: Eigen3 Types
46 #include <Eigen/Core>
47 #include <Eigen/LU> // needed for the inverse() function
48 
49 #include "../../WDefines.h"
50 #include "../../WStringUtils.h"
51 #include "../../WTypeTraits.h"
52 
53 #include "../../exceptions/WOutOfBounds.h"
54 
55 /**
56  * Macro for handling the value store template.
57  */
58 #define ValueStoreTemplate template< typename, size_t, size_t > class
59 
60 // forward declaration for the test
61 class WMatrixFixedTest;
62 
63 /**
64  * A data store with the specified dimensions and type. The possibilities are endless. This way, you can optimize data storage for certain kinds
65  * of matrices, like sparse or symmetric ones. It even allows the definition of a whole data block containing many matrices.
66  *
67  * \tparam ValueT the integral type
68  * \tparam Rows the number of rows
69  * \tparam Cols the number of cols
70  */
71 template< typename ValueT, size_t Rows, size_t Cols >
73 {
74  //! the test is a friend
75  friend class WMatrixFixedTest;
76 
77 public:
78  /**
79  * Returns a reference to the component of a row and column in order to provide access to the component. It does not check for validity of
80  * the indices.
81  *
82  * \param row the row, staring with 0
83  * \param col the column, starting with 0
84  * \return A reference to the component of a row and column
85  */
86  ValueT& operator()( size_t row, size_t col ) throw()
87  {
88  return m_values[ row * Cols + col ];
89  }
90 
91  /**
92  * Returns a const reference to the component of an row and column in order to provide access to the component.
93  * It does not check for validity of
94  * the indices.
95  *
96  * \param row the row, staring with 0
97  * \param col the column, starting with 0
98  * \return A const reference to the component of an row and column
99  */
100  const ValueT& operator()( size_t row, size_t col ) const throw()
101  {
102  return m_values[ row * Cols + col ];
103  }
104 
105  /**
106  * Replaces the values in this array.
107  *
108  * \tparam RHSValueT the value type. This is casted to ValueT.
109  * \tparam RHSValueStoreT The value store given
110  * \param rhs the values to set.
111  *
112  * \return this
113  */
114  template < typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
115  ValueStore< ValueT, Rows, Cols >& operator=( RHSValueStoreT< RHSValueT, Rows, Cols > const& rhs )
116  {
117  for( size_t row = 0; row < Rows; ++row )
118  {
119  for( size_t col = 0; col < Cols; ++col )
120  {
121  operator()( row, col ) = rhs( row, col );
122  }
123  }
124  }
125 
126 private:
127  /**
128  * The value array. Stored row-major. Never access this directly. Always use operator(). This allows us to later-on use another storing
129  * order.
130  */
131  ValueT m_values[ Rows * Cols ];
132 };
133 
134 /**
135  * A fixed size matrix class. This is the default type in OpenWalnut. You can easily convert this matrix to and from the Eigen3 types and OSG
136  * Types.
137  *
138  * \tparam ValueT The type of the values stored. Most of the operations, if multiple types are involved, use WTypeTraits to guess the better of
139  * both.
140  * \tparam Rows Number of Rows
141  * \tparam Cols Number of Columns
142  * \tparam ValueStoreT The ValueStore handles the values and their access. Use special types here for a fine-grained access control or
143  * data-management
144  */
145 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT = ValueStore >
147 {
148  //! the test is a friend
149  friend class WMatrixFixedTest;
150 
151  // this is needed for access to the storage object of another matrix
152  template< typename ValueTT, size_t Rowss, size_t Colss, ValueStoreTemplate ValueStoreTT >
153  friend class WMatrixFixed;
154 
155 public:
156 
157  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
158  // Types defining this matrix
159  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
160 
161  /**
162  * The integral type used in this matrix.
163  */
164  typedef ValueT ValueType;
165 
166  /**
167  * The storage container.
168  */
169  typedef ValueStoreT< ValueT, Rows, Cols > ValueStoreType;
170 
171  /**
172  * The whole matrix as a type for lazy programmers.
173  */
175 
176  /**
177  * The number of rows.
178  *
179  * \return the number of rows.
180  */
181  size_t getRows() const
182  {
183  return Rows;
184  }
185 
186  /**
187  * The number of columns.
188  *
189  * \return the number of columns.
190  */
191  size_t getColumns() const
192  {
193  return Cols;
194  }
195 
196  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
197  // Construction and Initialization
198  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
199 
200  /**
201  * Default constructor. The values are initialized with 0. Use the static methods \ref zero(), \ref identity() or any of the predefined
202  * transformations if an initialized matrix is wished.
203  */
205  {
206  // initialize to zero
207  for( size_t row = 0; row < Rows; ++row )
208  {
209  for( size_t col = 0; col < Cols; ++col )
210  {
211  operator()( row, col ) = ValueT( 0 );
212  }
213  }
214  }
215 
216  /**
217  * Constructor easing the initialization of vectors. This won't compile if Cols != 1 and Rows != 3.
218  *
219  * \param x x coefficient
220  * \param y y coefficient
221  * \param z z coefficient
222  */
223  WMatrixFixed( const ValueT& x, const ValueT& y, const ValueT& z )
224  {
225  BOOST_STATIC_ASSERT( Rows == 3 );
226  // NOTE: The static Cols == 1 check is done by operator []
227  operator[]( 0 ) = x;
228  operator[]( 1 ) = y;
229  operator[]( 2 ) = z;
230  }
231 
232  /**
233  * Constructor easing the initialization of vectors. This won't compile if Cols != 1 and Rows != 4.
234  *
235  * \param x x coefficient
236  * \param y y coefficient
237  * \param z z coefficient
238  * \param w w coefficient
239  */
240  WMatrixFixed( const ValueT& x, const ValueT& y, const ValueT& z, const ValueT& w )
241  {
242  BOOST_STATIC_ASSERT( Rows == 4 );
243  // NOTE: The static Cols == 1 check is done by operator []
244  operator[]( 0 ) = x;
245  operator[]( 1 ) = y;
246  operator[]( 2 ) = z;
247  operator[]( 3 ) = w;
248  }
249 
250  /**
251  * Copy construction casting the given value type. This is useful to create matrices with matrices using another value type.
252  *
253  * \tparam RHSValueT Value type of the given matrix to copy
254  * \tparam RHSValueStoreT Valuestore type of the given matrix to copy
255  * \param m the matrix to copy
256  */
257  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
258  WMatrixFixed( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& m ) // NOLINT - we do not want it explicit
259  {
260  setValues( m.m_values );
261  }
262 
263  /**
264  * Returns an identity matrix.
265  *
266  * \return the identity matrix.
267  */
269  {
270  MatrixType m = zero();
271  for( size_t i = 0; i < std::min( Rows, Cols ); ++i )
272  {
273  m( i, i ) = ValueT( 1 );
274  }
275  return m;
276  }
277 
278  /**
279  * Returns a zero-initialized matrix.
280  *
281  * \return the matrix.
282  */
283  static MatrixType zero()
284  {
285  MatrixType m;
286  for( size_t row = 0; row < Rows; ++row )
287  {
288  for( size_t col = 0; col < Cols; ++col )
289  {
290  m( row, col ) = ValueT( 0 );
291  }
292  }
293  return m;
294  }
295 
296  /**
297  * Copy construction allowing the creation of a WMatrixFixed by another matrix of different size.
298  * Please see \ref fromMatrices for more details, since this call is equivalent to fromMatrices( zero(), src, rowOffset, colOffset ).
299  *
300  * \see fromMatrices
301  *
302  * \tparam RHSValueT Value type of the given matrix
303  * \tparam RHSRows Number of rows of the given matrix.
304  * \tparam RHSCols Number of cols of the given matrix.
305  * \tparam RHSValueStoreT Value store of the given matrix.
306  *
307  * \param src the matrix to copy
308  * \param rowOffset row offset, defaults to 0
309  * \param colOffset col offset, defaults to 0
310  *
311  * \return The newly created matrix.
312  */
313  template< typename RHSValueT, size_t RHSRows, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
315  size_t colOffset = 0 )
316  {
317  return fromMatrices( zero(), src, rowOffset, colOffset );
318  }
319 
320  /**
321  * Copy construction allowing the creation of a WMatrixFixed by another matrix of different size.
322  * The specified source matrix gets copied into the area specified by its dimensions and the offset. On all other places, the specified
323  * reference matrix is used.
324  *
325  * \tparam RHSValueT Value type of the given matrix
326  * \tparam RHSRows Number of rows of the given matrix.
327  * \tparam RHSCols Number of cols of the given matrix.
328  * \tparam RHSValueStoreT Value store of the given matrix.
329  *
330  * \param m the reference matrix to use where src is not defined or used (due to offset)
331  * \param src the matrix to copy
332  * \param rowOffset row offset, defaults to 0
333  * \param colOffset col offset, defaults to 0
334  *
335  * \return The newly created matrix.
336  */
337  template< typename RHSValueT, size_t RHSRows, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
339  const WMatrixFixed< RHSValueT, RHSRows, RHSCols, RHSValueStoreT >& src, size_t rowOffset = 0,
340  size_t colOffset = 0 )
341  {
342  MatrixType result;
343  for( size_t row = 0; row < Rows; ++row )
344  {
345  for( size_t col = 0; col < Cols; ++col )
346  {
347  if( ( row >= rowOffset ) && ( col >= colOffset ) )
348  {
349  // find the correct index in the src matrix
350  size_t srcRow = row - rowOffset;
351  size_t srcCol = col - colOffset;
352 
353  // is this a valid index?
354  if( ( srcRow < RHSRows ) && ( srcCol < RHSCols ) )
355  {
356  result( row, col ) = src( srcRow, srcCol );
357  }
358  else
359  {
360  result( row, col ) = m( row, col );
361  }
362  }
363  else
364  {
365  result( row, col ) = m( row, col );
366  }
367  }
368  }
369  return result;
370  }
371 
372  /**
373  * Set a row to a specific vector.
374  *
375  * \tparam RHSValueT Value type of the given matrix
376  * \tparam ValueStoreT Value store of the given matrix
377  *
378  * \param index the index of the row you want to set
379  * \param vec the values to set for the row
380  *
381  */
382  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
384  {
385  for( size_t col = 0; col < Cols; col++ )
386  {
387  at( index, col ) = vec( col, 0 );
388  }
389  }
390 
391  /**
392  * Get a vector containing a specific row
393  *
394  * \param index the index of the row
395  *
396  * \return the row as a vector
397  */
399  {
401  for( size_t col = 0; col < Cols; col++ )
402  {
403  result( col, 0 ) = at( index, col );
404  }
405 
406  return result;
407  }
408 
409  /**
410  * Set a column to a specific vector.
411  *
412  * \tparam RHSValueT Value type of the given matrix
413  * \tparam ValueStoreT Value store of the given matrix
414  *
415  * \param index the index of the column you want to set
416  * \param vec the values to set for the column
417  *
418  */
419  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
421  {
422  for( size_t row = 0; row < Rows; row++ )
423  {
424  at( row, index ) = vec( row, 0 );
425  }
426  }
427 
428  /**
429  * Get a vector containing a specific column
430  *
431  * \param index the index of the column
432  *
433  * \return the column as a vector
434  */
436  {
438  for( size_t row = 0; row < Rows; row++ )
439  {
440  result( row, 0 ) = at( row, index );
441  }
442 
443  return result;
444  }
445 
446  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
447  // Conversion
448  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
449 
450  /**
451  * Conversion to a Eigen3 Matrix of same size and type.
452  *
453  * \return eigen3 matrix
454  */
455  operator Eigen::Matrix< ValueT, Rows, Cols >() const
456  {
457  Eigen::Matrix< ValueT, Rows, Cols > m;
458  for( size_t row = 0; row < Rows; ++row )
459  {
460  for( size_t col = 0; col < Cols; ++col )
461  {
462  m( row, col ) = operator()( row, col );
463  }
464  }
465  return m;
466  }
467 
468  /**
469  * Cast to OSG Vector. This will only compile for matrices with only one col and 2 rows.
470  *
471  * \return OSG vector.
472  */
473  operator osg::Vec2d() const
474  {
475  // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
476  BOOST_STATIC_ASSERT( Rows == 2 );
477  return osg::Vec2d( operator[]( 0 ), operator[]( 1 ) );
478  }
479 
480  /**
481  * Cast to OSG Vector. This will only compile for matrices with only one col and 3 or 4 rows.
482  *
483  * \return OSG vector.
484  */
485  operator osg::Vec2f() const
486  {
487  // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
488  BOOST_STATIC_ASSERT( Rows == 2 );
489  return osg::Vec2f( operator[]( 0 ), operator[]( 1 ) );
490  }
491 
492  /**
493  * Cast to OSG Vector. This will only compile for matrices with only one col and 3 or 4 rows.
494  *
495  * \return OSG vector.
496  */
497  operator osg::Vec3d() const
498  {
499  // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
500  BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
501  return osg::Vec3d( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ) );
502  }
503 
504  /**
505  * Cast to OSG Vector. This will only compile for matrices with only one col and 3 or 4 rows.
506  *
507  * \return OSG vector.
508  */
509  operator osg::Vec3f() const
510  {
511  // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
512  BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
513  return osg::Vec3f( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ) );
514  }
515 
516  /**
517  * Cast to OSG Vector. This will only compile for matrices with only one col and 4 rows.
518  *
519  * \return OSG vector.
520  */
521  operator osg::Vec4d() const
522  {
523  // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
524  BOOST_STATIC_ASSERT( Rows == 4 );
525  return osg::Vec4d( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ), operator[]( 3 ) );
526  }
527 
528  /**
529  * Cast to OSG Vector. This will only compile for matrices with only one col and 4 rows.
530  *
531  * \return OSG vector.
532  */
533  operator osg::Vec4f() const
534  {
535  // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
536  BOOST_STATIC_ASSERT( Rows == 4 );
537  return osg::Vec4f( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ), operator[]( 3 ) );
538  }
539 
540  /**
541  * Convert this matrix to a OSG Matrix of size 4x4. This compiles only for 4x4 WMatrix types.
542  *
543  * \return the OSG Matrix
544  */
545  operator osg::Matrixd() const
546  {
547  BOOST_STATIC_ASSERT( Rows == 4 );
548  BOOST_STATIC_ASSERT( Cols == 4 );
549 
550  osg::Matrixd m2;
551  for( size_t row = 0; row < 4; ++row )
552  {
553  for( size_t col = 0; col < 4; ++col )
554  {
555  m2( row, col ) = operator()( row, col );
556  }
557  }
558  return m2;
559  }
560 
561  /**
562  * A convenience function to cast the WMatrixFixed types to arbitrary other vector/matrix types that are supported by WMatrixFixed. This
563  * method is mainly needed for ambiguities during type resolution, if the target methods signature allows several different vec/matrix types.
564  * Example: you have void do( osg::Vec3f v ) and void do( osg::Vec3d v ). If you do WVector3d myV; do( myV ); This is ambiguous since
565  * WVector3d can be casted to either osg::Vec3d AND Vec3f implicitly.
566  *
567  * \tparam TargetType the type needed (to cast to)
568  *
569  * \return the required type
570  */
571  template< typename TargetType >
572  TargetType as() const
573  {
574  return operator TargetType();
575  }
576 
577  /**
578  * Cast to matrix of same size with different value type.
579  *
580  * \tparam ResultValueType resulting value type
581  * \tparam ResultValueStore resulting value store
582  *
583  * \return the converted matrix.
584  */
585  template < typename ResultValueType, ValueStoreTemplate ResultValueStore >
587  {
589  result.setValues( m_values );
590  return result;
591  }
592 
593  /**
594  * Creates a WMatrix from a given Eigen3 Matrix
595  *
596  * \param m the Eigen3 matrix.
597  */
598  WMatrixFixed( const Eigen::Matrix< ValueT, Rows, Cols >& m ) // NOLINT - we do not want it explicit
599  {
600  for( size_t row = 0; row < Rows; ++row )
601  {
602  for( size_t col = 0; col < Cols; ++col )
603  {
604  operator()( row, col ) = m( row, col );
605  }
606  }
607  }
608 
609  /**
610  * Creates a WMatrix from a given OSG 4x4 Matrix. Will not compile if Rows != 4 or Cols != 4.
611  *
612  * \param m the OSG matrix.
613  */
614  WMatrixFixed( const osg::Matrixd& m ) // NOLINT - we do not want it explicit
615  {
616  BOOST_STATIC_ASSERT( Rows == 4 );
617  BOOST_STATIC_ASSERT( Cols == 4 );
618 
619  for( size_t row = 0; row < 4; ++row )
620  {
621  for( size_t col = 0; col < 4; ++col )
622  {
623  operator()( row, col ) = m( row, col );
624  }
625  }
626  }
627 
628  /**
629  * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 3 or Cols != 1.
630  *
631  * \param m the OSG vector.
632  */
633  WMatrixFixed( const osg::Vec3f& m ) // NOLINT - we do not want it explicit
634  {
635  BOOST_STATIC_ASSERT( Rows == 3 );
636  BOOST_STATIC_ASSERT( Cols == 1 );
637 
638  operator[]( 0 ) = m.x();
639  operator[]( 1 ) = m.y();
640  operator[]( 2 ) = m.z();
641  }
642 
643  /**
644  * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 3 or Cols != 1.
645  *
646  * \param m the OSG vector.
647  */
648  WMatrixFixed( const osg::Vec3d& m ) // NOLINT - we do not want it explicit
649  {
650  BOOST_STATIC_ASSERT( Rows == 3 );
651  BOOST_STATIC_ASSERT( Cols == 1 );
652 
653  operator[]( 0 ) = m.x();
654  operator[]( 1 ) = m.y();
655  operator[]( 2 ) = m.z();
656  }
657 
658  /**
659  * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 4 or Cols != 1.
660  *
661  * \param m the OSG vector.
662  */
663  WMatrixFixed( const osg::Vec4f& m ) // NOLINT - we do not want it explicit
664  {
665  BOOST_STATIC_ASSERT( Rows == 4 );
666  BOOST_STATIC_ASSERT( Cols == 1 );
667 
668  operator[]( 0 ) = m[0];
669  operator[]( 1 ) = m[1];
670  operator[]( 2 ) = m[2];
671  operator[]( 3 ) = m[3];
672  }
673 
674  /**
675  * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 4 or Cols != 1.
676  *
677  * \param m the OSG vector.
678  */
679  WMatrixFixed( const osg::Vec4d& m ) // NOLINT - we do not want it explicit
680  {
681  BOOST_STATIC_ASSERT( Rows == 4 );
682  BOOST_STATIC_ASSERT( Cols == 1 );
683 
684  operator[]( 0 ) = m[0];
685  operator[]( 1 ) = m[1];
686  operator[]( 2 ) = m[2];
687  operator[]( 3 ) = m[3];
688  }
689 
690  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
691  // Copy and Assignment
692  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
693 
694  /**
695  * Assigns the given argument matrix to this one. If the types match, a reference is returned.
696  *
697  * \tparam RHSValueT the value type of the source matrix.
698  * \param rhs The right hand side of the assignment
699  *
700  * \return This matrix.
701  */
702  template < typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
704  {
705  setValues( rhs.m_values );
706  return *this;
707  }
708 
709  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
710  // Operators
711  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
712 
713  /**
714  * Matrix-Matrix multiplication. The number of columns of this matrix and the rows of the other need to match.
715  *
716  * \tparam RHSValueT the integral type of the given matrix
717  * \tparam RHSCols the number of columns of the given matrix. The number if rows must match the number of columns in this matrix
718  * \param rhs the matrix
719  *
720  * \return The product of the matrices
721  */
722  template< typename RHSValueT, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
725  {
726  typedef typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result ResultValueType;
727 
728  // NOTE: this is quite a naive implementation.
730  for( std::size_t row = 0; row < Rows; ++row )
731  {
732  for( std::size_t col = 0; col < RHSCols; ++col )
733  {
734  m( row, col ) = ResultValueType();
735  // dot between col and row vector
736  for( std::size_t i = 0; i < Cols; ++i )
737  {
738  m( row, col ) += operator()( row, i ) * rhs( i, col );
739  }
740  }
741  }
742  return m;
743  }
744 
745  /**
746  * Matrix-Matrix multiplication with self-assignment.
747  *
748  * \tparam RHSValueT the integral type of the given matrix
749  * \param rhs the matrix
750  */
751  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
753  {
754  operator=( *this * rhs );
755  }
756 
757  /**
758  * Matrix-Scalar multiplication.
759  *
760  * \tparam RHSValueT the integral type of the given scalar
761  * \param rhs the scalar
762  *
763  * \return The product of this matrix with the given scalar value.
764  */
765  template< typename RHSValueT >
767  operator*( const RHSValueT& rhs ) const
768  {
770  for( size_t row = 0; row < Rows; ++row )
771  {
772  for( size_t col = 0; col < Cols; ++col )
773  {
774  m( row, col ) = operator()( row, col ) * rhs;
775  }
776  }
777  return m;
778  }
779 
780  /**
781  * Matrix-Scalar multiplication with self-assignment.
782  *
783  * \tparam RHSValueT the integral type of the given scalar
784  * \param rhs the scalar
785  */
786  template< typename RHSValueT >
787  void operator*=( const RHSValueT& rhs )
788  {
789  operator=( *this * rhs );
790  }
791 
792  /**
793  * Matrix-Scalar division.
794  *
795  * \tparam RHSValueT the integral type of the given scalar
796  * \param rhs the scalar
797  *
798  * \return The matrix having all components divided by the scalar.
799  */
800  template< typename RHSValueT >
802  operator/( const RHSValueT& rhs ) const
803  {
804  typedef typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result ResultT;
805  return operator*( ResultT( 1 ) / static_cast< ResultT >( rhs ) );
806  }
807 
808  /**
809  * Matrix-Scalar division with self-assignmnet.
810  *
811  * \tparam RHSValueT the integral type of the given scalar
812  * \param rhs the scalar
813  */
814  template< typename RHSValueT >
815  void operator/=( const RHSValueT& rhs )
816  {
817  operator=( ( *this ) / rhs );
818  }
819 
820  /**
821  * Matrix addition. The number of columns and rows must be the same.
822  *
823  * \tparam RHSValueT the integral type of the given matrix
824  * \param rhs the matrix
825  *
826  * \return The sum of the current and the given matrix
827  */
828  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
831  {
833  for( size_t row = 0; row < Rows; ++row )
834  {
835  for( size_t col = 0; col < Cols; ++col )
836  {
837  m( row, col ) = operator()( row, col ) + rhs( row, col );
838  }
839  }
840  return m;
841  }
842 
843  /**
844  * Matrix addition with self-assignment.
845  *
846  * \tparam RHSValueT the integral type of the given matrix
847  * \param rhs the matrix
848  */
849  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
851  {
852  operator=( *this + rhs );
853  }
854 
855  /**
856  * Matrix subtraction. The number of columns and rows must be the same.
857  *
858  * \tparam RHSValueT the integral type of the given matrix
859  * \param rhs the matrix
860  *
861  * \return The difference of the current and the given matrix.
862  */
863  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
866  {
868  for( size_t row = 0; row < Rows; ++row )
869  {
870  for( size_t col = 0; col < Cols; ++col )
871  {
872  m( row, col ) = operator()( row, col ) - rhs( row, col );
873  }
874  }
875  return m;
876  }
877 
878  /**
879  * Matrix subtraction with self-assignment.
880  *
881  * \tparam RHSValueT the integral type of the given matrix
882  * \param rhs the matrix
883  */
884  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
886  {
887  operator=( *this - rhs );
888  }
889 
890  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
891  // Access
892  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
893 
894  /**
895  * Returns a reference to the component of an row and column in order to provide access to the component. It does not check for validity of
896  * the indices. Use \ref at for this.
897  *
898  * \param row the row, staring with 0
899  * \param col the column, starting with 0
900  *
901  * \return A reference to the component of an row and column
902  */
903  ValueT& operator()( size_t row, size_t col ) throw()
904  {
905  return m_values( row, col );
906  }
907 
908  /**
909  * Returns a reference to the component of an row and column in order to provide access to the component. It does not check for validity of
910  * the indices. Use \ref at for this.
911  *
912  * \param row the row, staring with 0
913  * \param col the column, starting with 0
914  *
915  * \return A const reference to the component of an row and column
916  */
917  const ValueT& operator()( size_t row, size_t col ) const throw()
918  {
919  return m_values( row, col );
920  }
921 
922  /**
923  * Returns a reference to the component of the first column to provide access to the component. It does not check for validity of
924  * the indices. Use this for single-column matrices (i.e. vectors). For matrices with cols!=0, this will not compile.
925  *
926  * \param row the row, staring with 0
927  *
928  * \return A reference to the component of the first column
929  */
930  ValueT& operator[]( size_t row ) throw()
931  {
932  BOOST_STATIC_ASSERT( Cols == 1 );
933  return m_values( row, 0 );
934  }
935 
936  /**
937  * Returns a reference to the component of the first column to provide access to the component. It does not check for validity of
938  * the indices. Use this for single-column matrices (i.e. vectors). For matrices with cols!=0, this will not compile.
939  *
940  * \param row the row, staring with 0
941  *
942  * \return A const reference to the component of the first column
943  */
944  const ValueT& operator[]( size_t row ) const throw()
945  {
946  BOOST_STATIC_ASSERT( Cols == 1 );
947  return m_values( row, 0 );
948  }
949 
950  /**
951  * Returns a reference to the component of an row and column in order to provide access to the component. It does check for validity of
952  * the indices. Use operator() for avoiding this check.
953  *
954  * \param row the row, staring with 0
955  * \param col the column, starting with 0
956  *
957  * \return A reference to the component of an row and column
958  *
959  * \throw WOutOfBounds if the specified index is invalid
960  */
961  ValueT& at( size_t row, size_t col ) throw( WOutOfBounds )
962  {
963  if( ( row >= Rows ) || ( col >= Cols ) )
964  {
965  throw WOutOfBounds( "Index pair (" + boost::lexical_cast< std::string >( row ) + ", " + boost::lexical_cast< std::string >( col ) +
966  ") is invalid for " + boost::lexical_cast< std::string >( Rows ) + "x" + boost::lexical_cast< std::string >( Cols ) +
967  " matrix." );
968  }
969  return operator()( row, col );
970  }
971 
972  /**
973  * Returns a const reference to the component of an row and column in order to provide access to the component.
974  * It does check for validity of
975  * the indices. Use operator() for avoiding this check.
976  *
977  * \param row the row, staring with 0
978  * \param col the column, starting with 0
979  *
980  * \return A const reference to the component of an row and column.
981  *
982  * \throw WOutOfBounds if the specified index is invalid
983  */
984  const ValueT& at( size_t row, size_t col ) const throw( WOutOfBounds )
985  {
986  if( ( row >= Rows ) || ( col >= Cols ) )
987  {
988  throw WOutOfBounds( "Index pair (" + boost::lexical_cast< std::string >( row ) + ", " + boost::lexical_cast< std::string >( col ) +
989  ") is invalid for " + boost::lexical_cast< std::string >( Rows ) + "x" + boost::lexical_cast< std::string >( Cols ) +
990  " matrix." );
991  }
992  return operator()( row, col );
993  }
994 
995  /**
996  * Access x element of vector. Works only for matrices with Cols == 1.
997  *
998  * \return x element
999  */
1000  ValueT& x() throw()
1001  {
1002  BOOST_STATIC_ASSERT( Rows >= 1 );
1003  BOOST_STATIC_ASSERT( Cols == 1 );
1004  return operator[]( 0 );
1005  }
1006 
1007  /**
1008  * Access x element of vector. Works only for matrices with Cols == 1.
1009  *
1010  * \return x element
1011  */
1012  const ValueT& x() const throw()
1013  {
1014  BOOST_STATIC_ASSERT( Rows >= 1 );
1015  BOOST_STATIC_ASSERT( Cols == 1 );
1016  return operator[]( 0 );
1017  }
1018 
1019  /**
1020  * Access y element of vector. Works only for matrices with Cols == 1.
1021  *
1022  * \return y element
1023  */
1024  ValueT& y() throw()
1025  {
1026  BOOST_STATIC_ASSERT( Rows >= 2 );
1027  BOOST_STATIC_ASSERT( Cols == 1 );
1028  return operator[]( 1 );
1029  }
1030 
1031  /**
1032  * Access y element of vector. Works only for matrices with Cols == 1.
1033  *
1034  * \return y element
1035  */
1036  const ValueT& y() const throw()
1037  {
1038  BOOST_STATIC_ASSERT( Rows >= 2 );
1039  BOOST_STATIC_ASSERT( Cols == 1 );
1040  return operator[]( 1 );
1041  }
1042 
1043  /**
1044  * Access z element of vector. Works only for matrices with Cols == 1.
1045  *
1046  * \return z element
1047  */
1048  ValueT& z() throw()
1049  {
1050  BOOST_STATIC_ASSERT( Rows >= 3 );
1051  BOOST_STATIC_ASSERT( Cols == 1 );
1052  return operator[]( 2 );
1053  }
1054 
1055  /**
1056  * Access z element of vector. Works only for matrices with Cols == 1.
1057  *
1058  * \return z element
1059  */
1060  const ValueT& z() const throw()
1061  {
1062  BOOST_STATIC_ASSERT( Rows >= 3 );
1063  BOOST_STATIC_ASSERT( Cols == 1 );
1064  return operator[]( 2 );
1065  }
1066 
1067  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1068  // Comparison
1069  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1070 
1071  /**
1072  * Compares two matrices and returns true if they are equal (component-wise).
1073  *
1074  * \tparam RHSValueT the value type of the argument
1075  * \param rhs The right hand side of the comparison
1076  *
1077  * \return true if equal
1078  */
1079  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1081  {
1082  bool eq = true;
1083  for( size_t row = 0; eq && ( row < Rows ); ++row )
1084  {
1085  for( size_t col = 0; eq && ( col < Cols ); ++col )
1086  {
1087  eq = eq && ( operator()( row, col ) == rhs( row, col ) );
1088  }
1089  }
1090  return eq;
1091  }
1092 
1093  /**
1094  * Compares two matrices and returns true if this is smaller than the specified one (component-wise).
1095  *
1096  * \tparam RHSValueT the value type of the argument
1097  * \param rhs The right hand side of the comparison
1098  *
1099  * \return true if this is less
1100  */
1101  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1102  bool operator<( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) const throw()
1103  {
1104  bool eq = true;
1105  bool result = true;
1106  for( size_t row = 0; eq && ( row < Rows ); ++row )
1107  {
1108  for( size_t col = 0; eq && ( col < Cols ); ++col )
1109  {
1110  eq = eq && ( operator()( row, col ) == rhs( row, col ) );
1111  result = ( operator()( row, col ) < rhs( row, col ) );
1112  }
1113  }
1114  return result;
1115  }
1116 
1117  /**
1118  * Compares two matrices and returns true if they are not equal.
1119  *
1120  * \tparam RHSValueT the value type of the argument
1121  * \param rhs The right hand side of the comparison
1122  * \return true if not equal.
1123  */
1124  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1126  {
1127  return !operator==( rhs );
1128  }
1129 
1130 private:
1131  /**
1132  * The value array. Stored row-major. Never access this directly. Always use operator(). This allows us to later-on use another storing
1133  * order.
1134  */
1136 
1137  /**
1138  * Sets the new values. Always use this method for replacing values in this matrix.
1139  *
1140  * \param values
1141  */
1142  template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1143  void setValues( const RHSValueStoreT< RHSValueT, Rows, Cols >& values )
1144  {
1145  for( std::size_t i = 0; i < Rows; ++i )
1146  {
1147  for( std::size_t j = 0; j < Cols; ++j )
1148  {
1149  m_values( i, j ) = static_cast< ValueT >( values( i, j ) );
1150  }
1151  }
1152  }
1153 };
1154 
1155 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1156 // Some standard matrices and vectors
1157 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1158 
1163 
1164 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1165 // Commutative Operators
1166 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1167 
1168 /**
1169  * Cale the given matrix by the value. This is needed for having * to be commutative. For more details, see \ref WMatrixFixed::operator*.
1170  *
1171  * \tparam ScalarT Integral type of scaler
1172  * \tparam MatrixValueT Value type of matrix
1173  * \tparam MatrixRows Rows of matrix
1174  * \tparam MatrixCols Columns of matrix
1175  * \tparam MatrixValueStoreT matrix value store type
1176  * \param n the scalar multiplier
1177  * \param mat the matrix to scale
1178  *
1179  * \return scaled matrix.
1180  */
1181 template < typename ScalarT,
1182  typename RHSValueT, size_t RHSRows, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
1185 {
1186  return mat * n;
1187 }
1188 
1189 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1190 // Non-friend Non-Member functions
1191 // * they implement most of the common algebra
1192 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
1193 
1194 /**
1195  * Calculate dot product between two vectors.
1196  *
1197  * \tparam AValueT Value type of the first vector
1198  * \tparam AValueStoreT ValueStore type of the first vector
1199  * \tparam BValueT Value type of the second vector
1200  * \tparam BValueStoreT ValueStore type of the second vector
1201  * \tparam Rows Number of rows for the vectors
1202  * \param a the first vector
1203  * \param b the second vector
1204  *
1205  * \return dot product
1206  */
1207 template< typename AValueT, ValueStoreTemplate AValueStoreT,
1208  typename BValueT, ValueStoreTemplate BValueStoreT,
1209  size_t Rows >
1210 typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result dot( const WMatrixFixed< AValueT, Rows, 1, AValueStoreT >& a,
1212 {
1213  typedef typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result ResultType;
1214  ResultType r = ResultType();
1215  for( size_t i = 0; i < Rows; ++i )
1216  {
1217  r += a( i, 0 ) * b( i, 0 );
1218  }
1219  return r;
1220 }
1221 
1222 /**
1223  * Calculate cross product between two 3D vectors.
1224  *
1225  * \tparam AValueT Value type of the first vector
1226  * \tparam AValueStoreT ValueStore type of the first vector
1227  * \tparam BValueT Value type of the second vector
1228  * \tparam BValueStoreT ValueStore type of the second vector
1229  * \tparam ResultValueStoreT resulting valuestore
1230  * \param a the first vector
1231  * \param b the second vector
1232  *
1233  * \return cross product
1234  */
1235 template< typename AValueT, ValueStoreTemplate AValueStoreT,
1236  typename BValueT, ValueStoreTemplate BValueStoreT >
1239 {
1240  typedef WMatrixFixed< typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result, 3, 1 > ResultT;
1241 
1242  // NOTE: to implement a general cross product for arbitrary row counts, the implementation is more complex and requires the implementation of
1243  // the Levi Civita symbol.
1244  ResultT v;
1245  v[0] = a[1] * b[2] - a[2] * b[1];
1246  v[1] = a[2] * b[0] - a[0] * b[2];
1247  v[2] = a[0] * b[1] - a[1] * b[0];
1248  return v;
1249 }
1250 
1251 /**
1252  * Calculates the <b>squared</b> length of a specified vector.
1253  *
1254  * \tparam ValueT Value type
1255  * \tparam ValueStoreT Value store to use
1256  * \tparam Rows number of rows in this colums-vector
1257  * \param a the vector
1258  *
1259  * \return the length of the vector
1260  */
1261 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Rows >
1262 ValueT length2( const WMatrixFixed< ValueT, Rows, 1, ValueStoreT >& a )
1263 {
1264  ValueT r = ValueT();
1265  for( size_t i = 0; i < Rows; ++i )
1266  {
1267  r += a( i, 0 ) * a( i, 0 );
1268  }
1269  return r;
1270 }
1271 
1272 /**
1273  * Calculates the <b>squared</b> length of a specified vector.
1274  *
1275  * \tparam ValueT Value type
1276  * \tparam ValueStoreT Value store to use
1277  * \tparam Cols number of columns in this row-vector
1278  * \param a the vector
1279  *
1280  * \return length of the vector
1281  */
1282 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Cols >
1283 ValueT length2( const WMatrixFixed< ValueT, 1, Cols, ValueStoreT >& a )
1284 {
1285  ValueT r = ValueT();
1286  for( size_t i = 0; i < Cols; ++i )
1287  {
1288  r += a( 0, i ) * a( 0, i );
1289  }
1290  return r;
1291 }
1292 
1293 /**
1294  * Calculates the length of a specified vector.
1295  *
1296  * \tparam ValueT Value type
1297  * \tparam ValueStoreT Value store to use
1298  * \tparam Rows number of rows in this colums-vector
1299  * \param a the vector
1300  *
1301  * \return the length of the vector
1302  */
1303 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Rows >
1304 ValueT length( const WMatrixFixed< ValueT, Rows, 1, ValueStoreT >& a )
1305 {
1306  return sqrt( length2( a ) );
1307 }
1308 
1309 /**
1310  * Calculates the length of a specified vector.
1311  *
1312  * \tparam ValueT Value type
1313  * \tparam ValueStoreT Value store to use
1314  * \tparam Cols number of columns in this row-vector
1315  * \param a the vector
1316  *
1317  * \return length of the vector
1318  */
1319 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Cols >
1320 ValueT length( const WMatrixFixed< ValueT, 1, Cols, ValueStoreT >& a )
1321 {
1322  return sqrt( length2( a ) );
1323 }
1324 
1325 /**
1326  * Normalizes the given vector.
1327  *
1328  * \tparam RHSValueT given matrix value type
1329  * \tparam Rows given row number
1330  * \tparam Cols given col number
1331  * \tparam RHSValueStoreT given matrix' valuestore
1332  * \param m the vector
1333  *
1334  * \return normalized vector
1335  */
1336 template< typename RHSValueT, size_t Rows, size_t Cols, ValueStoreTemplate RHSValueStoreT >
1338 {
1339  // NOTE: the static cast ensures that the returned matrix value type is the same as the input one.
1340  return m * static_cast< RHSValueT >( 1.0 / length( m ) );
1341 }
1342 
1343 /**
1344  * Inverts the specified matrix.
1345  *
1346  * \tparam ValueT The type of the values stored. Most of the operations, if multiple types are involved, use WTypeTraits to guess the better of
1347  * both.
1348  * \tparam Size Number of rows and columns
1349  * \tparam ValueStoreT The ValueStore handles the values and their access. Use special types here for a fine-grained access control or
1350  * data-management
1351  * \param m the matrix to invert
1352  *
1353  * \return inverted matrix
1354  */
1355 template< typename ValueT, std::size_t Size, template< typename, std::size_t, std::size_t > class ValueStoreT >
1357 {
1358  // this is a standard implementation
1359  return WMatrixFixed< ValueT, Size, Size, ValueStoreT >( static_cast< Eigen::Matrix< ValueT, Size, Size > >( m ).inverse() );
1360 }
1361 
1362 /**
1363  * Transposes a given matrix.
1364  *
1365  * \tparam ValueT The type of the values stored. Most of the operations, if multiple types are involved, use WTypeTraits to guess the better of
1366  * both.
1367  * \tparam Rows Number of Rows
1368  * \tparam Cols Number of Columns
1369  * \tparam ValueStoreT The ValueStore handles the values and their access. Use special types here for a fine-grained access control or
1370  * data-management
1371  *
1372  * \param mat the matrix to transpose
1373  *
1374  * \return transposed matrix
1375  */
1376 template< typename ValueT, std::size_t Rows, std::size_t Cols, template< typename, std::size_t, std::size_t > class ValueStoreT >
1378 {
1380  for( size_t row = 0; row < mat.getRows(); ++row )
1381  {
1382  for( size_t col = 0; col < mat.getColumns(); ++col )
1383  {
1384  res( col, row ) = mat( row, col );
1385  }
1386  }
1387  return res;
1388 }
1389 
1390 /**
1391  * Write a matrix in string representation to the given output stream.
1392  *
1393  * \tparam ValueT the integral type for the matrix coefficients
1394  * \tparam Rows The number of rows
1395  * \tparam Cols The number of columns
1396  * \tparam ValueStoreT the ValueStore type used for storing the values
1397  *
1398  * \param out the output stream to print the value to
1399  * \param m the matrix
1400  *
1401  * \return the output stream extended by the trigger value.
1402  */
1403 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT >
1404 std::ostream& operator<<( std::ostream& out, const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& m )
1405 {
1406  // NOTE if you change this operator, also change operator >>
1407  for( size_t row = 0; row < m.getRows(); ++row )
1408  {
1409  for( size_t col = 0; col < m.getColumns(); ++col )
1410  {
1411  out << m( row, col ) << ";";
1412  }
1413  }
1414  return out;
1415 }
1416 
1417 /**
1418  * Read a matrix in string representation from the given input stream.
1419  *
1420  * \param in the input stream to read the value from
1421  * \param m set the values red to this
1422  *
1423  * \tparam ValueT the integral type for the matrix coefficients
1424  * \tparam Rows The number of rows
1425  * \tparam Cols The number of columns
1426  * \tparam ValueStoreT the ValueStore type used for storing the values
1427  *
1428  * \return the input stream.
1429  */
1430 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT >
1431 std::istream& operator>>( std::istream& in, WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& m ) throw()
1432 {
1433  // NOTE if you change this operator, also change operator <<
1434  typedef boost::tokenizer< boost::char_separator< char > > Tokenizer;
1435 
1436  std::string s;
1437  in >> s;
1438  boost::char_separator< char > separators( " ;" );
1439  Tokenizer t( s, separators );
1440 
1441  Tokenizer::iterator it = t.begin();
1442  for( std::size_t row = 0; row < Rows; ++row )
1443  {
1444  for( std::size_t col = 0; col < Cols; ++col )
1445  {
1446  if( it == t.end() )
1447  {
1448  return in;
1449  }
1450  m( row, col ) = boost::lexical_cast< ValueT >( *it );
1451  ++it;
1452  }
1453  }
1454 
1455  return in;
1456 }
1457 
1458 #endif // WMATRIXFIXED_H
1459