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