OpenWalnut 1.3.1
WMatrixFixed.h
00001 //---------------------------------------------------------------------------
00002 //
00003 // Project: OpenWalnut ( http://www.openwalnut.org )
00004 //
00005 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
00006 // For more information see http://www.openwalnut.org/copying
00007 //
00008 // This file is part of OpenWalnut.
00009 //
00010 // OpenWalnut is free software: you can redistribute it and/or modify
00011 // it under the terms of the GNU Lesser General Public License as published by
00012 // the Free Software Foundation, either version 3 of the License, or
00013 // (at your option) any later version.
00014 //
00015 // OpenWalnut is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 // GNU Lesser General Public License for more details.
00019 //
00020 // You should have received a copy of the GNU Lesser General Public License
00021 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
00022 //
00023 //---------------------------------------------------------------------------
00024 
00025 #ifndef WMATRIXFIXED_H
00026 #define WMATRIXFIXED_H
00027 
00028 #include <string>
00029 #include <algorithm>
00030 
00031 #include <boost/static_assert.hpp>
00032 #include <boost/tokenizer.hpp>
00033 
00034 // Needed for conversion: OSG Types
00035 #include <osg/Vec3>
00036 #include <osg/Vec2d>
00037 #include <osg/Vec2f>
00038 #include <osg/Vec3d>
00039 #include <osg/Vec3f>
00040 #include <osg/Vec4d>
00041 #include <osg/Vec4f>
00042 #include <osg/Matrixd>
00043 
00044 // Needed for conversion: Eigen3 Types
00045 #include <Eigen/Core>
00046 #include <Eigen/LU>  // needed for the inverse() function
00047 
00048 #include "../../WDefines.h"
00049 #include "../../WStringUtils.h"
00050 #include "../../WTypeTraits.h"
00051 
00052 #include "../../exceptions/WOutOfBounds.h"
00053 
00054 /**
00055  * Macro for handling the value store template.
00056  */
00057 #define ValueStoreTemplate template< typename, size_t, size_t > class
00058 
00059 // forward declaration for the test
00060 class WMatrixFixedTest;
00061 
00062 /**
00063  * A data store with the specified dimensions and type. The possibilities are endless. This way, you can optimize data storage for certain kinds
00064  * of matrices, like sparse or symmetric ones. It even allows the definition of a whole data block containing many matrices.
00065  *
00066  * \note storage is done row-major
00067  *
00068  * \tparam ValueT   the integral type
00069  * \tparam Rows     the number of rows
00070  * \tparam Cols     the number of cols
00071  */
00072 template< typename ValueT, size_t Rows, size_t Cols >
00073 class ValueStore
00074 {
00075     //! the test is a friend
00076     friend class WMatrixFixedTest;
00077 
00078 public:
00079     /**
00080      * 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
00081      * the indices.
00082      *
00083      * \param row the row, staring with 0
00084      * \param col the column, starting with 0
00085      * \return A reference to the component of a row and column
00086      */
00087     ValueT& operator()( size_t row, size_t col ) throw()
00088     {
00089         return m_values[ row * Cols + col ];
00090     }
00091 
00092     /**
00093      * Returns a const reference to the component of an row and column in order to provide access to the component.
00094      * It does not check for validity of
00095      * the indices.
00096      *
00097      * \param row the row, staring with 0
00098      * \param col the column, starting with 0
00099      * \return A const reference to the component of an row and column
00100      */
00101     const ValueT& operator()( size_t row, size_t col ) const throw()
00102     {
00103         return m_values[ row * Cols + col ];
00104     }
00105 
00106     /**
00107      * Replaces the values in this array.
00108      *
00109      * \tparam RHSValueT the value type. This is casted to ValueT.
00110      * \tparam RHSValueStoreT The value store given
00111      * \param rhs the values to set.
00112      *
00113      * \return this
00114      */
00115     template < typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00116     ValueStore< ValueT, Rows, Cols >& operator=( RHSValueStoreT< RHSValueT, Rows, Cols > const& rhs )
00117     {
00118         for( size_t row = 0; row < Rows; ++row )
00119         {
00120             for( size_t col = 0; col < Cols; ++col )
00121             {
00122                 operator()( row, col ) = rhs( row, col );
00123             }
00124         }
00125     }
00126 
00127 private:
00128     /**
00129      * The value array. Stored row-major. Never access this directly. Always use operator(). This allows us to later-on use another storing
00130      * order.
00131      */
00132     ValueT m_values[ Rows * Cols ];
00133 };
00134 
00135 /**
00136  * 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
00137  * Types.
00138  *
00139  * \tparam ValueT The type of the values stored. Most of the operations, if multiple types are involved, use WTypeTraits to guess the better of
00140  *         both.
00141  * \tparam Rows Number of Rows
00142  * \tparam Cols Number of Columns
00143  * \tparam ValueStoreT The ValueStore handles the values and their access. Use special types here for a fine-grained access control or
00144  *         data-management
00145  */
00146 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT = ValueStore >
00147 class WMatrixFixed
00148 {
00149     //! the test is a friend
00150     friend class WMatrixFixedTest;
00151 
00152     // this is needed for access to the storage object of another matrix
00153     template< typename ValueTT, size_t Rowss, size_t Colss, ValueStoreTemplate ValueStoreTT >
00154     friend class WMatrixFixed;
00155 
00156 public:
00157     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00158     // Types defining this matrix
00159     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00160 
00161     /**
00162      * The integral type used in this matrix.
00163      */
00164     typedef ValueT ValueType;
00165 
00166     /**
00167      * The storage container.
00168      */
00169     typedef ValueStoreT< ValueT, Rows, Cols > ValueStoreType;
00170 
00171     /**
00172      * The whole matrix as a type for lazy programmers.
00173      */
00174     typedef WMatrixFixed< ValueT, Rows, Cols, ValueStoreT > MatrixType;
00175 
00176     /**
00177      * The number of rows.
00178      *
00179      * \return the number of rows.
00180      */
00181     size_t getRows() const
00182     {
00183         return Rows;
00184     }
00185 
00186     /**
00187      * The number of columns.
00188      *
00189      * \return the number of columns.
00190      */
00191     size_t getColumns() const
00192     {
00193         return Cols;
00194     }
00195 
00196     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00197     // Construction and Initialization
00198     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00199 
00200     /**
00201      * Default constructor. The values are initialized with 0. Use the static methods \ref zero(), \ref identity() or any of the predefined
00202      * transformations if an initialized matrix is wished.
00203      */
00204     WMatrixFixed()
00205     {
00206         // initialize to zero
00207         for( size_t row = 0; row < Rows; ++row )
00208         {
00209             for( size_t col = 0; col < Cols; ++col )
00210             {
00211                 operator()( row, col ) = ValueT( 0 );
00212             }
00213         }
00214     }
00215 
00216     /**
00217      * Constructor easing the initialization of vectors. This won't compile if Cols != 1 and Rows != 2.
00218      *
00219      * \param x x coefficient
00220      * \param y y coefficient
00221      */
00222     WMatrixFixed( const ValueT& x, const ValueT& y )
00223     {
00224         BOOST_STATIC_ASSERT( Rows == 2 );
00225         // NOTE: The static Cols == 1 check is done by operator []
00226         operator[]( 0 ) = x;
00227         operator[]( 1 ) = y;
00228     }
00229 
00230     /**
00231      * Constructor easing the initialization of vectors. This won't compile if Cols != 1 and Rows != 3.
00232      *
00233      * \param x x coefficient
00234      * \param y y coefficient
00235      * \param z z coefficient
00236      */
00237     WMatrixFixed( const ValueT& x, const ValueT& y, const ValueT& z )
00238     {
00239         BOOST_STATIC_ASSERT( Rows == 3 );
00240         // NOTE: The static Cols == 1 check is done by operator []
00241         operator[]( 0 ) = x;
00242         operator[]( 1 ) = y;
00243         operator[]( 2 ) = z;
00244     }
00245 
00246     /**
00247      * Constructor easing the initialization of vectors. This won't compile if Cols != 1 and Rows != 4.
00248      *
00249      * \param x x coefficient
00250      * \param y y coefficient
00251      * \param z z coefficient
00252      * \param w w coefficient
00253      */
00254     WMatrixFixed( const ValueT& x, const ValueT& y, const ValueT& z, const ValueT& w )
00255     {
00256         BOOST_STATIC_ASSERT( Rows == 4 );
00257         // NOTE: The static Cols == 1 check is done by operator []
00258         operator[]( 0 ) = x;
00259         operator[]( 1 ) = y;
00260         operator[]( 2 ) = z;
00261         operator[]( 3 ) = w;
00262     }
00263 
00264     /**
00265      * Copy construction casting the given value type. This is useful to create matrices with matrices using another value type.
00266      *
00267      * \tparam RHSValueT  Value type of the given matrix to copy
00268      * \tparam RHSValueStoreT Valuestore type of the given matrix to copy
00269      * \param m the matrix to copy
00270      */
00271     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00272     WMatrixFixed( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& m )     // NOLINT - we do not want it explicit
00273     {
00274         setValues( m.m_values );
00275     }
00276 
00277     /**
00278      * Returns an identity matrix.
00279      *
00280      * \return the identity matrix.
00281      */
00282     static MatrixType identity()
00283     {
00284         MatrixType m = zero();
00285         for( size_t i = 0; i < std::min( Rows, Cols ); ++i )
00286         {
00287             m( i, i ) = ValueT( 1 );
00288         }
00289         return m;
00290     }
00291 
00292     /**
00293      * Returns a zero-initialized matrix.
00294      *
00295      * \return the matrix.
00296      */
00297     static MatrixType zero()
00298     {
00299         MatrixType m;
00300         for( size_t row = 0; row < Rows; ++row )
00301         {
00302             for( size_t col = 0; col < Cols; ++col )
00303             {
00304                 m( row, col ) = ValueT( 0 );
00305             }
00306         }
00307         return m;
00308     }
00309 
00310     /**
00311      * Copy construction allowing the creation of a WMatrixFixed by another matrix of different size.
00312      * Please see \ref fromMatrices for more details, since this call is equivalent to fromMatrices( zero(), src, rowOffset, colOffset ).
00313      *
00314      * \see fromMatrices
00315      *
00316      * \tparam RHSValueT Value type of the given matrix
00317      * \tparam RHSRows Number of rows of the given matrix.
00318      * \tparam RHSCols Number of cols of the given matrix.
00319      * \tparam RHSValueStoreT Value store of the given matrix.
00320      *
00321      * \param src the matrix to copy
00322      * \param rowOffset row offset, defaults to 0
00323      * \param colOffset col offset, defaults to 0
00324      *
00325      * \return The newly created matrix.
00326      */
00327     template< typename RHSValueT, size_t RHSRows, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
00328     static MatrixType fromMatrix( const WMatrixFixed< RHSValueT, RHSRows, RHSCols, RHSValueStoreT >& src, size_t rowOffset = 0,
00329                                                                                                           size_t colOffset = 0 )
00330     {
00331         return fromMatrices( zero(), src, rowOffset, colOffset );
00332     }
00333 
00334     /**
00335      * Copy construction allowing the creation of a WMatrixFixed by another matrix of different size.
00336      * The specified source matrix gets copied into the area specified by its dimensions and the offset. On all other places, the specified
00337      * reference matrix is used.
00338      *
00339      * \tparam RHSValueT Value type of the given matrix
00340      * \tparam RHSRows Number of rows of the given matrix.
00341      * \tparam RHSCols Number of cols of the given matrix.
00342      * \tparam RHSValueStoreT Value store of the given matrix.
00343      *
00344      * \param m the reference matrix to use where src is not defined or used (due to offset)
00345      * \param src the matrix to copy
00346      * \param rowOffset row offset, defaults to 0
00347      * \param colOffset col offset, defaults to 0
00348      *
00349      * \return The newly created matrix.
00350      */
00351     template< typename RHSValueT, size_t RHSRows, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
00352     static MatrixType fromMatrices( const MatrixType& m,
00353                                     const WMatrixFixed< RHSValueT, RHSRows, RHSCols, RHSValueStoreT >& src, size_t rowOffset = 0,
00354                                                                                                             size_t colOffset = 0 )
00355     {
00356         MatrixType result;
00357         for( size_t row = 0; row < Rows; ++row )
00358         {
00359             for( size_t col = 0; col < Cols; ++col )
00360             {
00361                 if( ( row >= rowOffset ) && ( col >= colOffset ) )
00362                 {
00363                     // find the correct index in the src matrix
00364                     size_t srcRow = row - rowOffset;
00365                     size_t srcCol = col - colOffset;
00366 
00367                     // is this a valid index?
00368                     if( ( srcRow < RHSRows ) && ( srcCol < RHSCols ) )
00369                     {
00370                         result( row, col ) = src( srcRow, srcCol );
00371                     }
00372                     else
00373                     {
00374                         result( row, col ) = m( row, col );
00375                     }
00376                 }
00377                 else
00378                 {
00379                     result( row, col ) = m( row, col );
00380                 }
00381             }
00382         }
00383         return result;
00384     }
00385 
00386     /**
00387       * Set a row to a specific vector.
00388       *
00389       * \tparam RHSValueT Value type of the given matrix
00390       * \tparam ValueStoreT Value store of the given matrix
00391       *
00392       * \param index the index of the row you want to set
00393       * \param vec the values to set for the row
00394       *
00395       */
00396     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00397     void  setRowVector( size_t index, const WMatrixFixed< RHSValueT, Rows, 1, RHSValueStoreT >& vec )
00398     {
00399         for( size_t col = 0; col < Cols; col++ )
00400         {
00401             at( index, col ) = vec( col, 0 );
00402         }
00403     }
00404 
00405     /**
00406       * Get a vector containing a specific row
00407       *
00408       * \param index the index of the row
00409       *
00410       * \return the row as a vector
00411       */
00412     WMatrixFixed< ValueT, Cols, 1, ValueStoreT > getRowVector( size_t index ) const
00413     {
00414         WMatrixFixed< ValueT, Cols, 1 > result;
00415         for( size_t col = 0; col < Cols; col++ )
00416         {
00417             result( col, 0 ) = at( index, col );
00418         }
00419 
00420         return result;
00421     }
00422 
00423     /**
00424       * Set a column to a specific vector.
00425       *
00426       * \tparam RHSValueT Value type of the given matrix
00427       * \tparam ValueStoreT Value store of the given matrix
00428       *
00429       * \param index the index of the column you want to set
00430       * \param vec the values to set for the column
00431       *
00432       */
00433     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00434     void  setColumnVector( size_t index, const WMatrixFixed< RHSValueT, Rows, 1, RHSValueStoreT >& vec )
00435     {
00436         for( size_t row = 0; row < Rows; row++ )
00437         {
00438             at( row, index ) = vec( row, 0 );
00439         }
00440     }
00441 
00442     /**
00443       * Get a vector containing a specific column
00444       *
00445       * \param index the index of the column
00446       *
00447       * \return the column as a vector
00448       */
00449     WMatrixFixed< ValueT, Rows, 1 > getColumnVector( size_t index ) const
00450     {
00451         WMatrixFixed< ValueT, Rows, 1 > result;
00452         for( size_t row = 0; row < Rows; row++ )
00453         {
00454             result( row, 0 ) = at( row, index );
00455         }
00456 
00457         return result;
00458     }
00459 
00460     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00461     // Conversion
00462     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00463 
00464     /**
00465      * Conversion to a Eigen3 Matrix of same size and type.
00466      *
00467      * \return eigen3 matrix
00468      */
00469     operator Eigen::Matrix< ValueT, Rows, Cols >() const
00470     {
00471         Eigen::Matrix< ValueT, Rows, Cols > m;
00472         for( size_t row = 0; row < Rows; ++row )
00473         {
00474             for( size_t col = 0; col < Cols; ++col )
00475             {
00476                 m( row, col ) = operator()( row, col );
00477             }
00478         }
00479         return m;
00480     }
00481 
00482     /**
00483      * Cast to OSG Vector. This will only compile for matrices with only one col and 2 rows.
00484      *
00485      * \return OSG vector.
00486      */
00487     operator osg::Vec2d() const
00488     {
00489         // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
00490         BOOST_STATIC_ASSERT( Rows == 2 );
00491         return osg::Vec2d( operator[]( 0 ), operator[]( 1 ) );
00492     }
00493 
00494     /**
00495      * Cast to OSG Vector. This will only compile for matrices with only one col and 3 or 4 rows.
00496      *
00497      * \return OSG vector.
00498      */
00499     operator osg::Vec2f() const
00500     {
00501         // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
00502         BOOST_STATIC_ASSERT( Rows == 2 );
00503         return osg::Vec2f( operator[]( 0 ), operator[]( 1 ) );
00504     }
00505 
00506     /**
00507      * Cast to OSG Vector. This will only compile for matrices with only one col and 3 or 4 rows.
00508      *
00509      * \return OSG vector.
00510      */
00511     operator osg::Vec3d() const
00512     {
00513         // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
00514         BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
00515         return osg::Vec3d( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ) );
00516     }
00517 
00518     /**
00519      * Cast to OSG Vector. This will only compile for matrices with only one col and 3 or 4 rows.
00520      *
00521      * \return OSG vector.
00522      */
00523     operator osg::Vec3f() const
00524     {
00525         // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
00526         BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
00527         return osg::Vec3f( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ) );
00528     }
00529 
00530     /**
00531      * Cast to OSG Vector. This will only compile for matrices with only one col and 4 rows.
00532      *
00533      * \return OSG vector.
00534      */
00535     operator osg::Vec4d() const
00536     {
00537         // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
00538         BOOST_STATIC_ASSERT( Rows == 4 );
00539         return osg::Vec4d( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ), operator[]( 3 ) );
00540     }
00541 
00542     /**
00543      * Cast to OSG Vector. This will only compile for matrices with only one col and 4 rows.
00544      *
00545      * \return OSG vector.
00546      */
00547     operator osg::Vec4f() const
00548     {
00549         // NOTE: operator[] already manages Cols=0 assert and casting is done implicitly
00550         BOOST_STATIC_ASSERT( Rows == 4 );
00551         return osg::Vec4f( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ), operator[]( 3 ) );
00552     }
00553 
00554     /**
00555      * Convert this matrix to a OSG Matrix of size 4x4. This compiles only for 4x4 WMatrix types.
00556      *
00557      * \return the OSG Matrix
00558      */
00559     operator osg::Matrixd() const
00560     {
00561         BOOST_STATIC_ASSERT( Rows == 4 );
00562         BOOST_STATIC_ASSERT( Cols == 4 );
00563 
00564         osg::Matrixd m2;
00565         for( size_t row = 0; row < 4; ++row )
00566         {
00567             for( size_t col = 0; col < 4; ++col )
00568             {
00569                 m2( row, col ) = operator()( row, col );
00570             }
00571         }
00572         return m2;
00573     }
00574 
00575     /**
00576      * A convenience function to cast the WMatrixFixed types to arbitrary other vector/matrix types that are supported by WMatrixFixed. This
00577      * method is mainly needed for ambiguities during type resolution, if the target methods signature allows several different vec/matrix types.
00578      * 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
00579      * WVector3d can be casted to either osg::Vec3d AND Vec3f implicitly.
00580      *
00581      * \tparam TargetType the type needed (to cast to)
00582      *
00583      * \return the required type
00584      */
00585     template< typename TargetType >
00586     TargetType as() const
00587     {
00588         return operator TargetType();
00589     }
00590 
00591     /**
00592      * Cast to matrix of same size with different value type.
00593      *
00594      * \tparam ResultValueType resulting value type
00595      * \tparam ResultValueStore resulting value store
00596      *
00597      * \return the converted matrix.
00598      */
00599     template < typename ResultValueType, ValueStoreTemplate ResultValueStore >
00600     operator WMatrixFixed< ResultValueType, Rows, Cols, ResultValueStore >() const
00601     {
00602         WMatrixFixed< ResultValueType, Rows, Cols, ResultValueStore > result;
00603         result.setValues( m_values );
00604         return result;
00605     }
00606 
00607     /**
00608      * Creates a WMatrix from a given Eigen3 Matrix
00609      *
00610      * \param m the Eigen3 matrix.
00611      */
00612     WMatrixFixed( const Eigen::Matrix< ValueT, Rows, Cols >& m )    // NOLINT - we do not want it explicit
00613     {
00614         for( size_t row = 0; row < Rows; ++row )
00615         {
00616             for( size_t col = 0; col < Cols; ++col )
00617             {
00618                 operator()( row, col ) = m( row, col );
00619             }
00620         }
00621     }
00622 
00623     /**
00624      * Creates a WMatrix from a given OSG 4x4 Matrix. Will not compile if Rows != 4 or Cols != 4.
00625      *
00626      * \param m the OSG matrix.
00627      */
00628     WMatrixFixed( const osg::Matrixd& m )     // NOLINT - we do not want it explicit
00629     {
00630         BOOST_STATIC_ASSERT( Rows == 4 );
00631         BOOST_STATIC_ASSERT( Cols == 4 );
00632 
00633         for( size_t row = 0; row < 4; ++row )
00634         {
00635             for( size_t col = 0; col < 4; ++col )
00636             {
00637                 operator()( row, col ) = m( row, col );
00638             }
00639         }
00640     }
00641 
00642     /**
00643      * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 3 or Cols != 1.
00644      *
00645      * \param m the OSG vector.
00646      */
00647     WMatrixFixed( const osg::Vec3f& m )     // NOLINT - we do not want it explicit
00648     {
00649         BOOST_STATIC_ASSERT( Rows == 3 );
00650         BOOST_STATIC_ASSERT( Cols == 1 );
00651 
00652         operator[]( 0 ) = m.x();
00653         operator[]( 1 ) = m.y();
00654         operator[]( 2 ) = m.z();
00655     }
00656 
00657     /**
00658      * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 3 or Cols != 1.
00659      *
00660      * \param m the OSG vector.
00661      */
00662     WMatrixFixed( const osg::Vec3d& m )     // NOLINT - we do not want it explicit
00663     {
00664         BOOST_STATIC_ASSERT( Rows == 3 );
00665         BOOST_STATIC_ASSERT( Cols == 1 );
00666 
00667         operator[]( 0 ) = m.x();
00668         operator[]( 1 ) = m.y();
00669         operator[]( 2 ) = m.z();
00670     }
00671 
00672     /**
00673      * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 4 or Cols != 1.
00674      *
00675      * \param m the OSG vector.
00676      */
00677     WMatrixFixed( const osg::Vec4f& m )     // NOLINT - we do not want it explicit
00678     {
00679         BOOST_STATIC_ASSERT( Rows == 4 );
00680         BOOST_STATIC_ASSERT( Cols == 1 );
00681 
00682         operator[]( 0 ) = m[0];
00683         operator[]( 1 ) = m[1];
00684         operator[]( 2 ) = m[2];
00685         operator[]( 3 ) = m[3];
00686     }
00687 
00688     /**
00689      * Creates a WMatrix from a given OSG Vector. Will not compile if Rows != 4 or Cols != 1.
00690      *
00691      * \param m the OSG vector.
00692      */
00693     WMatrixFixed( const osg::Vec4d& m )     // NOLINT - we do not want it explicit
00694     {
00695         BOOST_STATIC_ASSERT( Rows == 4 );
00696         BOOST_STATIC_ASSERT( Cols == 1 );
00697 
00698         operator[]( 0 ) = m[0];
00699         operator[]( 1 ) = m[1];
00700         operator[]( 2 ) = m[2];
00701         operator[]( 3 ) = m[3];
00702     }
00703 
00704     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00705     // Copy and Assignment
00706     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00707 
00708     /**
00709      * Assigns the given argument matrix to this one. If the types match, a reference is returned.
00710      *
00711      * \tparam RHSValueT the value type of the source matrix.
00712      * \param rhs The right hand side of the assignment
00713      *
00714      * \return This matrix.
00715      */
00716     template < typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00717     MatrixType& operator=( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs )
00718     {
00719         setValues( rhs.m_values );
00720         return *this;
00721     }
00722 
00723     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00724     // Operators
00725     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00726 
00727     /**
00728      * Matrix-Matrix multiplication. The number of columns of this matrix and the rows of the other need to match.
00729      *
00730      * \tparam RHSValueT the integral type of the given matrix
00731      * \tparam RHSCols the number of columns of the given matrix. The number if rows must match the number of columns in this matrix
00732      * \param rhs the matrix
00733      *
00734      * \return The product of the matrices
00735      */
00736     template< typename RHSValueT, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
00737     WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, RHSCols, ValueStoreT >
00738         operator*( const WMatrixFixed< RHSValueT, Cols, RHSCols, RHSValueStoreT >& rhs ) const
00739     {
00740         typedef typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result ResultValueType;
00741 
00742         // NOTE: this is quite a naive implementation.
00743         WMatrixFixed< ResultValueType, Rows, RHSCols, ValueStoreT > m;
00744         for( std::size_t row = 0; row < Rows; ++row )
00745         {
00746             for( std::size_t col = 0; col < RHSCols; ++col )
00747             {
00748                 m( row, col ) = ResultValueType();
00749                 // dot between col and row vector
00750                 for( std::size_t i = 0; i < Cols; ++i )
00751                 {
00752                     m( row, col ) += operator()( row, i ) * rhs( i, col );
00753                 }
00754             }
00755         }
00756         return m;
00757     }
00758 
00759     /**
00760      * Matrix-Matrix multiplication with self-assignment.
00761      *
00762      * \tparam RHSValueT the integral type of the given matrix
00763      * \param rhs the matrix
00764      */
00765     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00766     void operator*=( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs )
00767     {
00768         operator=( *this * rhs );
00769     }
00770 
00771     /**
00772      * Matrix-Scalar multiplication.
00773      *
00774      * \tparam RHSValueT the integral type of the given scalar
00775      * \param rhs the scalar
00776      *
00777      * \return The product of this matrix with the given scalar value.
00778      */
00779     template< typename RHSValueT >
00780     WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT >
00781     operator*( const RHSValueT& rhs ) const
00782     {
00783         WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT > m;
00784         for( size_t row = 0; row < Rows; ++row )
00785         {
00786             for( size_t col = 0; col < Cols; ++col )
00787             {
00788                 m( row, col ) = operator()( row, col ) * rhs;
00789             }
00790         }
00791         return m;
00792     }
00793 
00794     /**
00795      * Matrix-Scalar multiplication with self-assignment.
00796      *
00797      * \tparam RHSValueT the integral type of the given scalar
00798      * \param rhs the scalar
00799      */
00800     template< typename RHSValueT >
00801     void operator*=( const RHSValueT& rhs )
00802     {
00803         operator=( *this * rhs );
00804     }
00805 
00806     /**
00807      * Matrix-Scalar division.
00808      *
00809      * \tparam RHSValueT the integral type of the given scalar
00810      * \param rhs the scalar
00811      *
00812      * \return The matrix having all components divided by the scalar.
00813      */
00814     template< typename RHSValueT >
00815     WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT >
00816     operator/( const RHSValueT& rhs ) const
00817     {
00818         typedef typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result ResultT;
00819         return operator*( ResultT( 1 ) / static_cast< ResultT >( rhs ) );
00820     }
00821 
00822     /**
00823      * Matrix-Scalar division with self-assignmnet.
00824      *
00825      * \tparam RHSValueT the integral type of the given scalar
00826      * \param rhs the scalar
00827      */
00828     template< typename RHSValueT >
00829     void operator/=( const RHSValueT& rhs )
00830     {
00831         operator=( ( *this ) / rhs );
00832     }
00833 
00834     /**
00835      * Matrix addition. The number of columns and rows must be the same.
00836      *
00837      * \tparam RHSValueT the integral type of the given matrix
00838      * \param rhs the matrix
00839      *
00840      * \return The sum of the current and the given matrix
00841      */
00842     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00843     WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT >
00844         operator+( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) const
00845     {
00846         WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT > m;
00847         for( size_t row = 0; row < Rows; ++row )
00848         {
00849             for( size_t col = 0; col < Cols; ++col )
00850             {
00851                 m( row, col ) = operator()( row, col ) + rhs( row, col );
00852             }
00853         }
00854         return m;
00855     }
00856 
00857     /**
00858      * Matrix addition with self-assignment.
00859      *
00860      * \tparam RHSValueT the integral type of the given matrix
00861      * \param rhs the matrix
00862      */
00863     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00864     void operator+=( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs )
00865     {
00866         operator=( *this + rhs );
00867     }
00868 
00869     /**
00870      * Matrix subtraction. The number of columns and rows must be the same.
00871      *
00872      * \tparam RHSValueT the integral type of the given matrix
00873      * \param rhs the matrix
00874      *
00875      * \return The difference of the current and the given matrix.
00876      */
00877     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00878     WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT >
00879         operator-( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) const
00880     {
00881         WMatrixFixed< typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result, Rows, Cols, ValueStoreT > m;
00882         for( size_t row = 0; row < Rows; ++row )
00883         {
00884             for( size_t col = 0; col < Cols; ++col )
00885             {
00886                 m( row, col ) = operator()( row, col ) - rhs( row, col );
00887             }
00888         }
00889         return m;
00890     }
00891 
00892     /**
00893      * Matrix subtraction with self-assignment.
00894      *
00895      * \tparam RHSValueT the integral type of the given matrix
00896      * \param rhs the matrix
00897      */
00898     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00899     void operator-=( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) throw()
00900     {
00901         operator=( *this - rhs );
00902     }
00903 
00904     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00905     // Access
00906     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00907 
00908     /**
00909      * 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
00910      * the indices. Use \ref at for this.
00911      *
00912      * \param row the row, staring with 0
00913      * \param col the column, starting with 0
00914      *
00915      * \return A reference to the component of an row and column
00916      */
00917     ValueT& operator()( size_t row, size_t col ) throw()
00918     {
00919         return m_values( row, col );
00920     }
00921 
00922     /**
00923      * 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
00924      * the indices. Use \ref at for this.
00925      *
00926      * \param row the row, staring with 0
00927      * \param col the column, starting with 0
00928      *
00929      * \return A const reference to the component of an row and column
00930      */
00931     const ValueT& operator()( size_t row, size_t col ) const throw()
00932     {
00933         return m_values( row, col );
00934     }
00935 
00936     /**
00937      * Returns a reference to the component of the first column to provide access to the component. It does not check for validity of
00938      * the indices. Use this for single-column matrices (i.e. vectors). For matrices with cols!=0, this will not compile.
00939      *
00940      * \param row the row, staring with 0
00941      *
00942      * \return A reference to the component of the first column
00943      */
00944     ValueT& operator[]( size_t row ) throw()
00945     {
00946         BOOST_STATIC_ASSERT( Cols == 1 );
00947         return m_values( row, 0 );
00948     }
00949 
00950     /**
00951      * Returns a reference to the component of the first column to provide access to the component. It does not check for validity of
00952      * the indices. Use this for single-column matrices (i.e. vectors). For matrices with cols!=0, this will not compile.
00953      *
00954      * \param row the row, staring with 0
00955      *
00956      * \return A const reference to the component of the first column
00957      */
00958     const ValueT& operator[]( size_t row ) const throw()
00959     {
00960         BOOST_STATIC_ASSERT( Cols == 1 );
00961         return m_values( row, 0 );
00962     }
00963 
00964     /**
00965      * 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
00966      * the indices. Use operator() for avoiding this check.
00967      *
00968      * \param row the row, staring with 0
00969      * \param col the column, starting with 0
00970      *
00971      * \return A reference to the component of an row and column
00972      *
00973      * \throw WOutOfBounds if the specified index is invalid
00974      */
00975     ValueT& at( size_t row, size_t col ) throw( WOutOfBounds )
00976     {
00977         if( ( row >= Rows ) || ( col >= Cols ) )
00978         {
00979             throw WOutOfBounds( "Index pair (" + string_utils::toString( row ) + ", " + string_utils::toString( col ) +
00980                                 ") is invalid for " + string_utils::toString( Rows ) + "x" + string_utils::toString( Cols ) +
00981                                 " matrix." );
00982         }
00983         return operator()( row, col );
00984     }
00985 
00986     /**
00987      * Returns a const reference to the component of an row and column in order to provide access to the component.
00988      * It does check for validity of
00989      * the indices. Use operator() for avoiding this check.
00990      *
00991      * \param row the row, staring with 0
00992      * \param col the column, starting with 0
00993      *
00994      * \return A const reference to the component of an row and column.
00995      *
00996      * \throw WOutOfBounds if the specified index is invalid
00997      */
00998     const ValueT& at( size_t row, size_t col ) const throw( WOutOfBounds )
00999     {
01000         if( ( row >= Rows ) || ( col >= Cols ) )
01001         {
01002             throw WOutOfBounds( "Index pair (" + string_utils::toString( row ) + ", " + string_utils::toString( col ) +
01003                                 ") is invalid for " + string_utils::toString( Rows ) + "x" + string_utils::toString( Cols ) +
01004                                 " matrix." );
01005         }
01006         return operator()( row, col );
01007     }
01008 
01009     /**
01010      * Access x element of vector. Works only for matrices with Cols == 1.
01011      *
01012      * \return x element
01013      */
01014     ValueT& x() throw()
01015     {
01016         BOOST_STATIC_ASSERT( Rows >= 1 );
01017         BOOST_STATIC_ASSERT( Cols == 1 );
01018         return operator[]( 0 );
01019     }
01020 
01021     /**
01022      * Access x element of vector. Works only for matrices with Cols == 1.
01023      *
01024      * \return x element
01025      */
01026     const ValueT& x() const throw()
01027     {
01028         BOOST_STATIC_ASSERT( Rows >= 1 );
01029         BOOST_STATIC_ASSERT( Cols == 1 );
01030         return operator[]( 0 );
01031     }
01032 
01033     /**
01034      * Access y element of vector. Works only for matrices with Cols == 1.
01035      *
01036      * \return y element
01037      */
01038     ValueT& y() throw()
01039     {
01040         BOOST_STATIC_ASSERT( Rows >= 2 );
01041         BOOST_STATIC_ASSERT( Cols == 1 );
01042         return operator[]( 1 );
01043     }
01044 
01045     /**
01046      * Access y element of vector. Works only for matrices with Cols == 1.
01047      *
01048      * \return y element
01049      */
01050     const ValueT& y() const throw()
01051     {
01052         BOOST_STATIC_ASSERT( Rows >= 2 );
01053         BOOST_STATIC_ASSERT( Cols == 1 );
01054         return operator[]( 1 );
01055     }
01056 
01057     /**
01058      * Access z element of vector. Works only for matrices with Cols == 1.
01059      *
01060      * \return z element
01061      */
01062     ValueT& z() throw()
01063     {
01064         BOOST_STATIC_ASSERT( Rows >= 3 );
01065         BOOST_STATIC_ASSERT( Cols == 1 );
01066         return operator[]( 2 );
01067     }
01068 
01069     /**
01070      * Access z element of vector. Works only for matrices with Cols == 1.
01071      *
01072      * \return z element
01073      */
01074     const ValueT& z() const throw()
01075     {
01076         BOOST_STATIC_ASSERT( Rows >= 3 );
01077         BOOST_STATIC_ASSERT( Cols == 1 );
01078         return operator[]( 2 );
01079     }
01080 
01081     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01082     // Comparison
01083     /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01084 
01085     /**
01086      * Compares two matrices and returns true if they are equal (component-wise).
01087      *
01088      * \tparam RHSValueT the value type of the argument
01089      * \param rhs The right hand side of the comparison
01090      *
01091      * \return true if equal
01092      */
01093     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
01094     bool operator==( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) const throw()
01095     {
01096         bool eq = true;
01097         for( size_t row = 0; eq && ( row < Rows ); ++row )
01098         {
01099             for( size_t col = 0; eq && ( col < Cols ); ++col )
01100             {
01101                 eq = eq && ( operator()( row, col ) == rhs( row, col ) );
01102             }
01103         }
01104         return eq;
01105     }
01106 
01107     /**
01108      * Compares two matrices and returns true if this is smaller than the specified one (component-wise).
01109      *
01110      * \tparam RHSValueT the value type of the argument
01111      * \param rhs The right hand side of the comparison
01112      *
01113      * \return true if this is less
01114      */
01115     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
01116     bool operator<( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) const throw()
01117     {
01118         bool eq = true;
01119         bool result = true;
01120         for( size_t row = 0; eq && ( row < Rows ); ++row )
01121         {
01122             for( size_t col = 0; eq && ( col < Cols ); ++col )
01123             {
01124                 eq = eq && ( operator()( row, col ) == rhs( row, col ) );
01125                 result = ( operator()( row, col ) < rhs( row, col ) );
01126             }
01127         }
01128         return result;
01129     }
01130 
01131     /**
01132      * Compares two matrices and returns true if they are not equal.
01133      *
01134      * \tparam RHSValueT the value type of the argument
01135      * \param rhs The right hand side of the comparison
01136      * \return true if not equal.
01137      */
01138     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
01139     bool operator!=( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs ) const throw()
01140     {
01141         return !operator==( rhs );
01142     }
01143 
01144 private:
01145     /**
01146      * The value array. Stored row-major. Never access this directly. Always use operator(). This allows us to later-on use another storing
01147      * order.
01148      */
01149     ValueStoreType m_values;
01150 
01151     /**
01152      * Sets the new values. Always use this method for replacing values in this matrix.
01153      *
01154      * \param values
01155      */
01156     template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
01157     void setValues( const RHSValueStoreT< RHSValueT, Rows, Cols >& values )
01158     {
01159         for( std::size_t i = 0; i < Rows; ++i )
01160         {
01161             for( std::size_t j = 0; j < Cols; ++j )
01162             {
01163                 m_values( i, j ) = static_cast< ValueT >( values( i, j ) );
01164             }
01165         }
01166     }
01167 };
01168 
01169 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01170 // Some standard matrices and vectors
01171 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01172 
01173 typedef WMatrixFixed< double, 3, 3 > WMatrix3d;
01174 typedef WMatrixFixed< double, 4, 4 > WMatrix4d;
01175 typedef WMatrixFixed< float, 3, 3 > WMatrix3f;
01176 typedef WMatrixFixed< float, 4, 4 > WMatrix4f;
01177 
01178 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01179 // Commutative Operators
01180 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01181 
01182 /**
01183  * Scale the given matrix by the value. This is needed for having * to be commutative. For more details, see  \ref WMatrixFixed::operator*.
01184  *
01185  * \tparam ScalarT Integral type of scaler
01186  * \tparam MatrixValueT Value type of matrix
01187  * \tparam MatrixRows Rows of matrix
01188  * \tparam MatrixCols Columns of matrix
01189  * \tparam MatrixValueStoreT matrix value store type
01190  * \param n the scalar multiplier
01191  * \param mat the matrix to scale
01192  *
01193  * \return scaled matrix.
01194  */
01195 template < typename ScalarT,
01196            typename RHSValueT, size_t RHSRows, size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
01197 WMatrixFixed< typename WTypeTraits::TypePromotion< ScalarT, RHSValueT >::Result, RHSRows, RHSCols, RHSValueStoreT >
01198     operator*( const ScalarT n, const WMatrixFixed< RHSValueT, RHSRows, RHSCols, RHSValueStoreT >& mat )
01199 {
01200     return mat * n;
01201 }
01202 
01203 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01204 // Non-friend Non-Member functions
01205 //  * they implement most of the common algebra
01206 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
01207 
01208 /**
01209  * Calculate dot product between two vectors.
01210  *
01211  * \tparam AValueT Value type of the first vector
01212  * \tparam AValueStoreT ValueStore type of the first vector
01213  * \tparam BValueT Value type of the second vector
01214  * \tparam BValueStoreT ValueStore type of the second vector
01215  * \tparam Rows Number of rows for the vectors
01216  * \param a the first vector
01217  * \param b the second vector
01218  *
01219  * \return dot product
01220  */
01221 template< typename AValueT, ValueStoreTemplate AValueStoreT,
01222           typename BValueT, ValueStoreTemplate BValueStoreT,
01223           size_t Rows >
01224 typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result dot( const WMatrixFixed< AValueT, Rows, 1, AValueStoreT >& a,
01225                                                                      const WMatrixFixed< BValueT, Rows, 1, BValueStoreT >& b )
01226 {
01227     typedef typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result ResultType;
01228     ResultType r = ResultType();
01229     for( size_t i = 0; i < Rows; ++i )
01230     {
01231         r += a( i, 0 ) * b( i, 0 );
01232     }
01233     return r;
01234 }
01235 
01236 /**
01237  * Calculate cross product between two 3D vectors.
01238  *
01239  * \tparam AValueT Value type of the first vector
01240  * \tparam AValueStoreT ValueStore type of the first vector
01241  * \tparam BValueT Value type of the second vector
01242  * \tparam BValueStoreT ValueStore type of the second vector
01243  * \tparam ResultValueStoreT resulting valuestore
01244  * \param a the first vector
01245  * \param b the second vector
01246  *
01247  * \return cross product
01248  */
01249 template< typename AValueT, ValueStoreTemplate AValueStoreT,
01250           typename BValueT, ValueStoreTemplate BValueStoreT >
01251 WMatrixFixed< typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result, 3, 1 >
01252     cross( const WMatrixFixed< AValueT, 3, 1, AValueStoreT >& a, const WMatrixFixed< BValueT, 3, 1, BValueStoreT >& b )
01253 {
01254     typedef WMatrixFixed< typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result, 3, 1 > ResultT;
01255 
01256     // NOTE: to implement a general cross product for arbitrary row counts, the implementation is more complex and requires the implementation of
01257     // the Levi Civita symbol.
01258     ResultT v;
01259     v[0] = a[1] * b[2] - a[2] * b[1];
01260     v[1] = a[2] * b[0] - a[0] * b[2];
01261     v[2] = a[0] * b[1] - a[1] * b[0];
01262     return v;
01263 }
01264 
01265 /**
01266  * Calculates the <b>squared</b> length of a specified vector.
01267  *
01268  * \tparam ValueT Value type
01269  * \tparam ValueStoreT Value store to use
01270  * \tparam Rows number of rows in this colums-vector
01271  * \param a the vector
01272  *
01273  * \return the length of the vector
01274  */
01275 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Rows >
01276 ValueT length2( const WMatrixFixed< ValueT, Rows, 1, ValueStoreT >& a )
01277 {
01278     ValueT r = ValueT();
01279     for( size_t i = 0; i < Rows; ++i )
01280     {
01281         r += a( i, 0 ) * a( i, 0 );
01282     }
01283     return r;
01284 }
01285 
01286 /**
01287  * Calculates the <b>squared</b> length of a specified vector.
01288  *
01289  * \tparam ValueT Value type
01290  * \tparam ValueStoreT Value store to use
01291  * \tparam Cols number of columns in this row-vector
01292  * \param a the vector
01293  *
01294  * \return length of the vector
01295  */
01296 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Cols >
01297 ValueT length2( const WMatrixFixed< ValueT, 1, Cols, ValueStoreT >& a )
01298 {
01299     ValueT r = ValueT();
01300     for( size_t i = 0; i < Cols; ++i )
01301     {
01302         r += a( 0, i ) * a( 0, i );
01303     }
01304     return r;
01305 }
01306 
01307 /**
01308  * Calculates the length of a specified vector.
01309  *
01310  * \tparam ValueT Value type
01311  * \tparam ValueStoreT Value store to use
01312  * \tparam Rows number of rows in this colums-vector
01313  * \param a the vector
01314  *
01315  * \return the length of the vector
01316  */
01317 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Rows >
01318 ValueT length( const WMatrixFixed< ValueT, Rows, 1, ValueStoreT >& a )
01319 {
01320     return sqrt( length2( a ) );
01321 }
01322 
01323 /**
01324  * Calculates the length of a specified vector.
01325  *
01326  * \tparam ValueT Value type
01327  * \tparam ValueStoreT Value store to use
01328  * \tparam Cols number of columns in this row-vector
01329  * \param a the vector
01330  *
01331  * \return length of the vector
01332  */
01333 template< typename ValueT, ValueStoreTemplate ValueStoreT, size_t Cols >
01334 ValueT length( const WMatrixFixed< ValueT, 1, Cols, ValueStoreT >& a )
01335 {
01336     return sqrt( length2( a ) );
01337 }
01338 
01339 /**
01340  * Normalizes the given vector.
01341  *
01342  * \tparam RHSValueT given matrix value type
01343  * \tparam Rows given row number
01344  * \tparam Cols given col number
01345  * \tparam RHSValueStoreT given matrix' valuestore
01346  * \param m the vector
01347  *
01348  * \return normalized vector
01349  */
01350 template< typename RHSValueT, size_t Rows, size_t Cols, ValueStoreTemplate RHSValueStoreT >
01351 WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT > normalize( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& m )
01352 {
01353     // NOTE: the static cast ensures that the returned matrix value type is the same as the input one.
01354     return m * static_cast< RHSValueT >( 1.0 / length( m ) );
01355 }
01356 
01357 /**
01358  * Inverts the specified matrix.
01359  *
01360  * \tparam ValueT The type of the values stored. Most of the operations, if multiple types are involved, use WTypeTraits to guess the better of
01361  *         both.
01362  * \tparam Size Number of rows and columns
01363  * \tparam ValueStoreT The ValueStore handles the values and their access. Use special types here for a fine-grained access control or
01364  *         data-management
01365  * \param m the matrix to invert
01366  *
01367  * \return inverted matrix
01368  */
01369 template< typename ValueT, std::size_t Size, template< typename, std::size_t, std::size_t > class ValueStoreT >
01370 WMatrixFixed< ValueT, Size, Size, ValueStoreT > invert( WMatrixFixed< ValueT, Size, Size, ValueStoreT > const& m )
01371 {
01372     // this is a standard implementation
01373     return WMatrixFixed< ValueT, Size, Size, ValueStoreT >( static_cast< Eigen::Matrix< ValueT, Size, Size > >( m ).inverse() );
01374 }
01375 
01376 /**
01377  * Transposes a given matrix.
01378  *
01379  * \tparam ValueT The type of the values stored. Most of the operations, if multiple types are involved, use WTypeTraits to guess the better of
01380  *         both.
01381  * \tparam Rows Number of Rows
01382  * \tparam Cols Number of Columns
01383  * \tparam ValueStoreT The ValueStore handles the values and their access. Use special types here for a fine-grained access control or
01384  *         data-management
01385  *
01386  * \param mat the matrix to transpose
01387  *
01388  * \return transposed matrix
01389  */
01390 template< typename ValueT, std::size_t Rows, std::size_t Cols, template< typename, std::size_t, std::size_t > class ValueStoreT >
01391 WMatrixFixed< ValueT, Cols, Rows, ValueStoreT > transpose( WMatrixFixed< ValueT, Rows, Cols, ValueStoreT > const& mat )
01392 {
01393     WMatrixFixed< ValueT, Cols, Rows, ValueStoreT > res;
01394     for( size_t row = 0; row < mat.getRows(); ++row )
01395     {
01396         for( size_t col = 0; col < mat.getColumns(); ++col )
01397         {
01398             res( col, row ) = mat( row, col );
01399         }
01400     }
01401     return res;
01402 }
01403 
01404 /**
01405  * Write a matrix in string representation to the given output stream.
01406  *
01407  * \tparam ValueT the integral type for the matrix coefficients
01408  * \tparam Rows The number of rows
01409  * \tparam Cols The number of columns
01410  * \tparam ValueStoreT the ValueStore type used for storing the values
01411  *
01412  * \param out the output stream to print the value to
01413  * \param m the matrix
01414  *
01415  * \return the output stream extended by the trigger value.
01416  */
01417 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT >
01418 std::ostream& operator<<( std::ostream& out, const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& m )
01419 {
01420     // NOTE if you change this operator, also change operator >>
01421     for( size_t row = 0; row < m.getRows(); ++row )
01422     {
01423         for( size_t col = 0; col < m.getColumns(); ++col )
01424         {
01425             out << m( row, col ) << ";";
01426         }
01427     }
01428     return out;
01429 }
01430 
01431 /**
01432  * Read a matrix in string representation from the given input stream.
01433  *
01434  * \param in the input stream to read the value from
01435  * \param m  set the values red to this
01436  *
01437  * \tparam ValueT the integral type for the matrix coefficients
01438  * \tparam Rows The number of rows
01439  * \tparam Cols The number of columns
01440  * \tparam ValueStoreT the ValueStore type used for storing the values
01441  *
01442  * \return the input stream.
01443  */
01444 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT >
01445 std::istream& operator>>( std::istream& in, WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& m ) throw()
01446 {
01447     // NOTE if you change this operator, also change operator <<
01448     typedef boost::tokenizer< boost::char_separator< char > > Tokenizer;
01449 
01450     std::string s;
01451     in >> s;
01452     boost::char_separator< char > separators( " ;" );
01453     Tokenizer t( s, separators );
01454 
01455     Tokenizer::iterator it = t.begin();
01456     for( std::size_t row = 0; row < Rows; ++row )
01457     {
01458         for( std::size_t col = 0; col < Cols; ++col )
01459         {
01460             if( it == t.end() )
01461             {
01462                 return in;
01463             }
01464             m( row, col ) = string_utils::fromString< ValueT >( *it );
01465             ++it;
01466         }
01467     }
01468 
01469     return in;
01470 }
01471 
01472 #endif  // WMATRIXFIXED_H
01473