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