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