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