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