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