00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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
00046 #include <Eigen/Core>
00047 #include <Eigen/LU>
00048
00049 #include "../../WDefines.h"
00050 #include "../../WStringUtils.h"
00051 #include "../../WTypeTraits.h"
00052
00053 #include "../../exceptions/WOutOfBounds.h"
00054
00055
00056
00057
00058 #define ValueStoreTemplate template< typename, size_t, size_t > class
00059
00060
00061 class WMatrixFixedTest;
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 template< typename ValueT, size_t Rows, size_t Cols >
00072 class ValueStore
00073 {
00074
00075 friend class WMatrixFixedTest;
00076
00077 public:
00078
00079
00080
00081
00082
00083
00084
00085
00086 ValueT& operator()( size_t row, size_t col ) throw()
00087 {
00088 return m_values[ row * Cols + col ];
00089 }
00090
00091
00092
00093
00094
00095
00096
00097
00098
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
00107
00108
00109
00110
00111
00112
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
00129
00130
00131 ValueT m_values[ Rows * Cols ];
00132 };
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 template< typename ValueT, size_t Rows, size_t Cols, ValueStoreTemplate ValueStoreT = ValueStore >
00146 class WMatrixFixed
00147 {
00148
00149 friend class WMatrixFixedTest;
00150
00151
00152 template< typename ValueTT, size_t Rowss, size_t Colss, ValueStoreTemplate ValueStoreTT >
00153 friend class WMatrixFixed;
00154
00155 public:
00156
00157
00158
00159
00160
00161
00162
00163
00164 typedef ValueT ValueType;
00165
00166
00167
00168
00169 typedef ValueStoreT< ValueT, Rows, Cols > ValueStoreType;
00170
00171
00172
00173
00174 typedef WMatrixFixed< ValueT, Rows, Cols, ValueStoreT > MatrixType;
00175
00176
00177
00178
00179
00180
00181 size_t getRows() const
00182 {
00183 return Rows;
00184 }
00185
00186
00187
00188
00189
00190
00191 size_t getColumns() const
00192 {
00193 return Cols;
00194 }
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204 WMatrixFixed()
00205 {
00206
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
00218
00219
00220
00221
00222
00223 WMatrixFixed( const ValueT& x, const ValueT& y, const ValueT& z )
00224 {
00225 BOOST_STATIC_ASSERT( Rows == 3 );
00226
00227 operator[]( 0 ) = x;
00228 operator[]( 1 ) = y;
00229 operator[]( 2 ) = z;
00230 }
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240 WMatrixFixed( const ValueT& x, const ValueT& y, const ValueT& z, const ValueT& w )
00241 {
00242 BOOST_STATIC_ASSERT( Rows == 4 );
00243
00244 operator[]( 0 ) = x;
00245 operator[]( 1 ) = y;
00246 operator[]( 2 ) = z;
00247 operator[]( 3 ) = w;
00248 }
00249
00250
00251
00252
00253
00254
00255
00256
00257 template< typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
00258 WMatrixFixed( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& m )
00259 {
00260 setValues( m.m_values );
00261 }
00262
00263
00264
00265
00266
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
00280
00281
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
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
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
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
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
00350 size_t srcRow = row - rowOffset;
00351 size_t srcCol = col - colOffset;
00352
00353
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
00374
00375
00376
00377
00378
00379
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
00393
00394
00395
00396
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
00411
00412
00413
00414
00415
00416
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
00430
00431
00432
00433
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
00448
00449
00450
00451
00452
00453
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
00470
00471
00472
00473 operator osg::Vec2d() const
00474 {
00475
00476 BOOST_STATIC_ASSERT( Rows == 2 );
00477 return osg::Vec2d( operator[]( 0 ), operator[]( 1 ) );
00478 }
00479
00480
00481
00482
00483
00484
00485 operator osg::Vec2f() const
00486 {
00487
00488 BOOST_STATIC_ASSERT( Rows == 2 );
00489 return osg::Vec2f( operator[]( 0 ), operator[]( 1 ) );
00490 }
00491
00492
00493
00494
00495
00496
00497 operator osg::Vec3d() const
00498 {
00499
00500 BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
00501 return osg::Vec3d( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ) );
00502 }
00503
00504
00505
00506
00507
00508
00509 operator osg::Vec3f() const
00510 {
00511
00512 BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
00513 return osg::Vec3f( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ) );
00514 }
00515
00516
00517
00518
00519
00520
00521 operator osg::Vec4d() const
00522 {
00523
00524 BOOST_STATIC_ASSERT( Rows == 4 );
00525 return osg::Vec4d( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ), operator[]( 3 ) );
00526 }
00527
00528
00529
00530
00531
00532
00533 operator osg::Vec4f() const
00534 {
00535
00536 BOOST_STATIC_ASSERT( Rows == 4 );
00537 return osg::Vec4f( operator[]( 0 ), operator[]( 1 ), operator[]( 2 ), operator[]( 3 ) );
00538 }
00539
00540
00541
00542
00543
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
00563
00564
00565
00566
00567
00568
00569
00570
00571 template< typename TargetType >
00572 TargetType as() const
00573 {
00574 return operator TargetType();
00575 }
00576
00577
00578
00579
00580
00581
00582
00583
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
00595
00596
00597
00598 WMatrixFixed( const Eigen::Matrix< ValueT, Rows, Cols >& m )
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
00611
00612
00613
00614 WMatrixFixed( const osg::Matrixd& m )
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
00630
00631
00632
00633 WMatrixFixed( const osg::Vec3f& m )
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
00645
00646
00647
00648 WMatrixFixed( const osg::Vec3d& m )
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
00660
00661
00662
00663 WMatrixFixed( const osg::Vec4f& m )
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
00676
00677
00678
00679 WMatrixFixed( const osg::Vec4d& m )
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
00692
00693
00694
00695
00696
00697
00698
00699
00700
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
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
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
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
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
00747
00748
00749
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
00759
00760
00761
00762
00763
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
00782
00783
00784
00785
00786 template< typename RHSValueT >
00787 void operator*=( const RHSValueT& rhs )
00788 {
00789 operator=( *this * rhs );
00790 }
00791
00792
00793
00794
00795
00796
00797
00798
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
00810
00811
00812
00813
00814 template< typename RHSValueT >
00815 void operator/=( const RHSValueT& rhs )
00816 {
00817 operator=( ( *this ) / rhs );
00818 }
00819
00820
00821
00822
00823
00824
00825
00826
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
00845
00846
00847
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
00857
00858
00859
00860
00861
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
00880
00881
00882
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
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903 ValueT& operator()( size_t row, size_t col ) throw()
00904 {
00905 return m_values( row, col );
00906 }
00907
00908
00909
00910
00911
00912
00913
00914
00915
00916
00917 const ValueT& operator()( size_t row, size_t col ) const throw()
00918 {
00919 return m_values( row, col );
00920 }
00921
00922
00923
00924
00925
00926
00927
00928
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
00938
00939
00940
00941
00942
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
00952
00953
00954
00955
00956
00957
00958
00959
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
00974
00975
00976
00977
00978
00979
00980
00981
00982
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
00997
00998
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
01009
01010
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
01021
01022
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
01033
01034
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
01045
01046
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
01057
01058
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
01069
01070
01071
01072
01073
01074
01075
01076
01077
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
01095
01096
01097
01098
01099
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
01119
01120
01121
01122
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
01133
01134
01135 ValueStoreType m_values;
01136
01137
01138
01139
01140
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
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
01166
01167
01168
01169
01170
01171
01172
01173
01174
01175
01176
01177
01178
01179
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
01191
01192
01193
01194
01195
01196
01197
01198
01199
01200
01201
01202
01203
01204
01205
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
01224
01225
01226
01227
01228
01229
01230
01231
01232
01233
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
01243
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
01253
01254
01255
01256
01257
01258
01259
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
01274
01275
01276
01277
01278
01279
01280
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
01295
01296
01297
01298
01299
01300
01301
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
01311
01312
01313
01314
01315
01316
01317
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
01327
01328
01329
01330
01331
01332
01333
01334
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
01340 return m * static_cast< RHSValueT >( 1.0 / length( m ) );
01341 }
01342
01343
01344
01345
01346
01347
01348
01349
01350
01351
01352
01353
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
01359 return WMatrixFixed< ValueT, Size, Size, ValueStoreT >( static_cast< Eigen::Matrix< ValueT, Size, Size > >( m ).inverse() );
01360 }
01361
01362
01363
01364
01365
01366
01367
01368
01369
01370
01371
01372
01373
01374
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
01392
01393
01394
01395
01396
01397
01398
01399
01400
01401
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
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
01419
01420
01421
01422
01423
01424
01425
01426
01427
01428
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
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