00001 #ifndef __VMML__MATRIX__HPP__
00002 #define __VMML__MATRIX__HPP__
00003
00004 #include <vmmlib/vmmlib_config.hpp>
00005 #include <vmmlib/exception.hpp>
00006 #include <vmmlib/vector.hpp>
00007 #include <vmmlib/details.hpp>
00008 #include <vmmlib/stringUtils.hpp>
00009 #include <vmmlib/matrix_functors.hpp>
00010
00011 #include <iostream>
00012 #include <iomanip>
00013 #include <vector>
00014 #include <string>
00015
00016 namespace vmml
00017 {
00018
00019
00020 template< size_t M, size_t N, typename float_t = double >
00021 class matrix
00022 {
00023 public:
00024 typedef float_t float_type;
00025 typedef float_t value_type;
00026 typedef matrix< M, N, float_t > matrix_type;
00027
00028 typedef float_t* iterator;
00029 typedef const float_t* const_iterator;
00030
00031
00032 iterator begin();
00033 iterator end();
00034 const_iterator begin() const;
00035 const_iterator end() const;
00036
00037
00038 inline float_t& operator()( size_t rowIndex, size_t colIndex );
00039 inline const float_t& operator()( size_t rowIndex, size_t colIndex ) const;
00040 inline float_t& at( size_t rowIndex, size_t colIndex );
00041 inline const float_t& at( size_t rowIndex, size_t colIndex ) const;
00042
00043
00044 matrix();
00045 matrix( const matrix_functor< matrix< M, N, float_t > >& functor );
00046
00047 void zero();
00048 void identity();
00049
00050 bool operator==( const matrix& other ) const;
00051 bool operator!=( const matrix& other ) const;
00052
00053
00054 bool isEqualTo( const matrix& other, float_t tolerance );
00055
00056
00057 template< size_t P >
00058 void multiply(
00059 const matrix< M, P, float_t >& left,
00060 const matrix< P, N, float_t >& right
00061 );
00062
00063
00064
00065 matrix operator*( matrix& other );
00066
00067
00068
00069
00070 template< size_t P >
00071 matrix< M, P, float_t > operator*( matrix< N, P, float_t >& other );
00072
00073
00074 void operator*=( const matrix< M, M, float_t >& right );
00075
00076 inline matrix operator+( const matrix& other ) const;
00077 inline matrix operator-( const matrix& other ) const;
00078
00079 void operator+=( const matrix& other );
00080 void operator-=( const matrix& other );
00081
00082 template< size_t P, size_t Q >
00083 void directSum(
00084 const matrix< P, Q, float_t >& other,
00085 matrix< M + P, N + Q, float_t >& result
00086 );
00087
00088 template< size_t P, size_t Q >
00089 matrix< M + P, N + Q, float_t >
00090 directSum( const matrix< P, Q, float_t >& other );
00091
00092
00093
00094
00095 matrix operator*( float_t scalar );
00096 void operator*=( float_t scalar );
00097
00098
00099
00100
00101
00102 vector< M, float_t > operator*( const vector< N, float_t >& other ) const;
00103
00104
00105
00106 vector< M-1, float_t > operator*( const vector< N-1, float_t >& other ) const;
00107
00108 inline matrix< M, N, float_t > operator-() const;
00109 matrix< M, N, float_t > negate() const;
00110
00111
00112 void tensor( const vector< M, float_t >& u, const vector< N, float_t >& v );
00113
00114
00115 void tensor( const vector< M-1, float_t >& u, const vector< N-1, float_t >& v );
00116
00117 template< size_t Mret, size_t Nret >
00118 matrix< Mret, Nret, float_t >
00119 getSubMatrix( size_t rowOffset, size_t colOffset ) const;
00120
00121 template< size_t Mret, size_t Nret >
00122 void getSubMatrix( matrix< Mret, Nret, float_t >& result,
00123 size_t rowOffset = 0, size_t colOffset = 0 ) const;
00124
00125 template< size_t Mret, size_t Nret >
00126 void setSubMatrix( const matrix< Mret, Nret, float_t >& subMatrix,
00127 size_t rowOffset = 0, size_t colOffset = 0 );
00128
00129
00130 void transposeTo( matrix<N, M, float_t >& transposedMatrix ) const;
00131
00132 matrix< N, M, float_t > getTransposed() const;
00133
00134
00135
00136
00137
00138 void operator=( const float_t* data_array );
00139 void operator=( const float_t old_fashioned_matrix[ M ][ N ] );
00140 void operator=( const std::vector< float_t >& data );
00141
00142
00143 void operator=( float_t fill_value );
00144 void fill( float_t fill_value );
00145
00146 bool set( const std::vector< std::string >& values );
00147 bool set( const std::string& values, char delimiter );
00148
00149
00150 void set(
00151 float_t v00, float_t v01, float_t v02,
00152 float_t v10, float_t v11, float_t v12,
00153 float_t v20, float_t v21, float_t v22
00154 );
00155
00156
00157 void set(
00158 float_t v00, float_t v01, float_t v02, float_t v03,
00159 float_t v10, float_t v11, float_t v12, float_t v13,
00160 float_t v20, float_t v21, float_t v22, float_t v23,
00161 float_t v30, float_t v31, float_t v32, float_t v33
00162 );
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182 void copyFrom1DimCArray( const float_t* c_array,
00183 bool row_by_row_layout = true );
00184
00185 template< typename different_float_t >
00186 void copyFrom1DimCArray( const different_float_t* c_array,
00187 bool row_by_row_layout = true );
00188
00189
00190 vector< M, float_t > getColumn( size_t columnNumber ) const;
00191 void getColumn( size_t columnNumber, vector< M, float_t >& column ) const;
00192 void setColumn( size_t columnNumber, const vector< M, float_t >& column );
00193
00194 void getColumn( size_t columnNumber, matrix< M, 1, float_t >& column ) const;
00195 void setColumn( size_t columnNumber, const matrix< M, 1, float_t >& column );
00196
00197 vector< N, float_t > getRow( size_t rowNumber ) const;
00198 void getRow( size_t rowNumber, vector< N, float_t >& row ) const;
00199 void setRow( size_t rowNumber, const vector< N, float_t >& row );
00200
00201 void getRow( size_t rowNumber, matrix< 1, N, float_t >& row ) const;
00202 void setRow( size_t rowNumber, const matrix< 1, N, float_t >& row );
00203
00204 size_t size() const;
00205
00206 size_t getM() const;
00207 size_t getNumberOfRows() const;
00208
00209 size_t getN() const;
00210 size_t getNumberOfColumns() const;
00211
00212
00213 float_t getDeterminant() const;
00214 void getAdjugate( matrix< M, M, float_t >& adjugate ) const;
00215 void getCofactors( matrix< M, N, float_t >& cofactors ) const;
00216
00217
00218
00219
00220 bool getInverse( matrix< M, M, float_t >& inverse, float_t tolerance = 1e-9 ) const;
00221
00222
00223 float_t getMinor(
00224 matrix< M-1, N-1 >& other,
00225 size_t row_to_cut,
00226 size_t col_to_cut
00227 );
00228
00229 bool isPositiveDefinite( const float_t limit = -1e12 ) const;
00230
00231
00232
00233
00238 void rotate( const float_t angle, const vector< M-1, float_t >& axis );
00239
00240 void rotateX( const float_t angle );
00241 void rotateY( const float_t angle );
00242 void rotateZ( const float_t angle );
00243 void preRotateX( const float_t angle );
00244 void preRotateY( const float_t angle );
00245 void preRotateZ( const float_t angle );
00246 void scale( const float_t scale[3] );
00247 void scale( const float_t x, const float_t y, const float_t z );
00248 inline void scale( const vector< 3, float_t >& scale_ );
00249 void scaleTranslation( const float_t scale_[3] );
00250 inline void scaleTranslation( const vector< 3, float_t >& scale_ );
00251 void setTranslation( const float_t x, const float_t y, const float_t z );
00252 void setTranslation( const float_t trans[3] );
00253 inline void setTranslation( const vector< 3, float_t >& trans );
00254 vector< 3, float_t > getTranslation() const;
00255
00256
00257
00258
00259 bool getString( std::string& result, const std::string& delimiter = " " ) const;
00260
00261
00262 template< typename init_functor_t >
00263 static const matrix getInitializedMatrix();
00264
00265
00266 struct row_accessor
00267 {
00268 row_accessor( float_t* array_ ) : array( array_ ) {}
00269 float_t&
00270 operator[]( size_t colIndex )
00271 {
00272 #ifdef VMMLIB_SAFE_ACCESSORS
00273 if ( colIndex >= N )
00274 VMMLIB_ERROR( "column index out of bounds", VMMLIB_HERE );
00275 #endif
00276 return array[ colIndex * M ];
00277 }
00278
00279 const float_t&
00280 operator[]( size_t colIndex ) const
00281 {
00282 #ifdef VMMLIB_SAFE_ACCESSORS
00283 if ( colIndex >= N )
00284 VMMLIB_ERROR( "column index out of bounds", VMMLIB_HERE );
00285 #endif
00286 return array[ colIndex * M ];
00287 }
00288
00289 float_t* array;
00290 private: row_accessor() {}
00291 };
00292
00293
00294 inline row_accessor operator[]( size_t rowIndex )
00295 {
00296 #ifdef VMMLIB_SAFE_ACCESSORS
00297 if ( rowIndex > M )
00298 VMMLIB_ERROR( "row index out of bounds", VMMLIB_HERE );
00299 #endif
00300 return row_accessor( array + rowIndex );
00301 }
00302
00303 friend std::ostream& operator << ( std::ostream& os,
00304 const matrix< M, N, float_t >& matrix )
00305 {
00306 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00307 {
00308 os << "(";
00309 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00310 {
00311 os << matrix.at( rowIndex, colIndex ) << ", ";
00312 }
00313 os << ")\n";
00314 }
00315 return os;
00316 };
00317
00318
00319
00320 float_t array[ M * N ]
00321 #ifndef VMMLIB_DONT_FORCE_ALIGNMENT
00322 #ifdef __GNUC__
00323 __attribute__((aligned(16)))
00324 #endif
00325 #endif
00326 ;
00327
00328
00329 static const matrix< M, N, float_t > IDENTITY;
00330 static const matrix< M, N, float_t > ZERO;
00331 #if 0
00332 static const set_to_identity< matrix< M, N, float_t > > IDENTITY_FUNCTOR;
00333 static const set_to_zero< matrix< M, N, float_t > > ZERO_FUNCTOR;
00334 #endif
00335
00336 };
00337
00338
00339 #ifndef VMMLIB_NO_TYPEDEFS
00340 typedef matrix< 3, 3, float > mat3f;
00341 typedef matrix< 3, 3, double > mat3d;
00342 typedef matrix< 4, 4, float > mat4f;
00343 typedef matrix< 4, 4, double > mat4d;
00344 #endif
00345
00346
00347
00348
00349
00350
00351 #if 0
00352 template< size_t M, size_t N, typename float_t >
00353 const set_to_identity< matrix< M, N, float_t > >
00354 matrix< M, N, float_t >::IDENTITY_FUNCTOR;
00355
00356 template< size_t M, size_t N, typename float_t >
00357 const set_to_zero< matrix< M, N, float_t > >
00358 matrix< M, N, float_t >::ZERO_FUNCTOR;
00359
00360 #endif
00361
00362
00363
00364 template< size_t M, size_t N, typename float_t >
00365 const matrix< M, N, float_t >
00366 matrix< M, N, float_t >::IDENTITY( matrix< M, N, float_t >::
00367 getInitializedMatrix< set_to_identity< matrix< M, N, float_t > > >() );
00368
00369
00370 template< size_t M, size_t N, typename float_t >
00371 const matrix< M, N, float_t >
00372 matrix< M, N, float_t >::ZERO( matrix< M, N, float_t >::
00373 getInitializedMatrix< set_to_zero< matrix< M, N, float_t > > >() );
00374
00375
00376 template< size_t M, size_t N, typename float_t >
00377 matrix< M, N, float_t >::matrix()
00378 {
00379
00380 }
00381
00382
00383
00384 template< size_t M, size_t N, typename float_t >
00385 matrix< M, N, float_t >::matrix( const matrix_functor< matrix< M, N, float_t > >& functor )
00386 {
00387 functor( *this );
00388 }
00389
00390
00391
00392 template< size_t M, size_t N, typename float_t >
00393 inline float_t&
00394 matrix< M, N, float_t >::at( size_t rowIndex, size_t colIndex )
00395 {
00396 #ifdef VMMLIB_SAFE_ACCESSORS
00397 if ( rowIndex >= M || colIndex >= N )
00398 VMMLIB_ERROR( "at( row, col ) - index out of bounds", VMMLIB_HERE );
00399 #endif
00400 return array[ colIndex * M + rowIndex ];
00401 }
00402
00403
00404
00405 template< size_t M, size_t N, typename float_t >
00406 const inline float_t&
00407 matrix< M, N, float_t >::at( size_t rowIndex, size_t colIndex ) const
00408 {
00409 #ifdef VMMLIB_SAFE_ACCESSORS
00410 if ( rowIndex >= M || colIndex >= N )
00411 VMMLIB_ERROR( "at( row, col ) - index out of bounds", VMMLIB_HERE );
00412 #endif
00413 return array[ colIndex * M + rowIndex ];
00414 }
00415
00416
00417 template< size_t M, size_t N, typename float_t >
00418 inline float_t&
00419 matrix< M, N, float_t >::operator()( size_t rowIndex, size_t colIndex )
00420 {
00421 return at( rowIndex, colIndex );
00422 }
00423
00424
00425
00426 template< size_t M, size_t N, typename float_t >
00427 const inline float_t&
00428 matrix< M, N, float_t >::operator()( size_t rowIndex, size_t colIndex ) const
00429 {
00430 return at( rowIndex, colIndex );
00431 }
00432
00433
00434 #if 0
00435 template< size_t M, size_t N, typename float_t >
00436 matrix< M, N, float_t >::matrix()
00437 {
00438 }
00439
00440
00441
00442 template< size_t M, size_t N, typename float_t >
00443 matrix< M, N, float_t >::matrix( const matrix< M, N >& original )
00444 {
00445
00446 }
00447 #endif
00448
00449
00450 template< size_t M, size_t N, typename float_t >
00451 bool
00452 matrix< M, N, float_t >::
00453 operator==( const matrix< M, N, float_t >& other ) const
00454 {
00455 bool ok = true;
00456 for( size_t i = 0; ok && i < M * N; ++i )
00457 {
00458 ok = array[ i ] == other.array[ i ];
00459 }
00460 return ok;
00461 }
00462
00463
00464
00465
00466 template< size_t M, size_t N, typename float_t >
00467 bool
00468 matrix< M, N, float_t >::
00469 operator!=( const matrix< M, N, float_t >& other ) const
00470 {
00471 return ! operator==( other );
00472 }
00473
00474
00475
00476 template< size_t M, size_t N, typename float_t >
00477 bool
00478 matrix< M, N, float_t >::
00479 isEqualTo( const matrix< M, N, float_t >& other, float_t tolerance )
00480 {
00481 bool ok = true;
00482 for( size_t rowIndex = 0; ok && rowIndex < M; rowIndex++)
00483 {
00484 for( size_t colIndex = 0; ok && colIndex < N; colIndex++)
00485 {
00486 if ( at( rowIndex, colIndex ) > other.at( rowIndex, colIndex ) )
00487 ok = fabs( at( rowIndex, colIndex ) - other.at( rowIndex, colIndex ) ) < tolerance;
00488 else
00489 ok = fabs( other.at( rowIndex, colIndex ) - at( rowIndex, colIndex ) ) < tolerance;
00490 }
00491 }
00492 return ok;
00493 }
00494
00495
00496
00497 template< size_t M, size_t N, typename float_t >
00498 void
00499 matrix< M, N, float_t >::operator=( const float_t matrix_[ M ][ N ] )
00500 {
00501 for( size_t rowIndex = 0; rowIndex < M; rowIndex++)
00502 {
00503 for( size_t colIndex = 0; colIndex < N; colIndex++)
00504 {
00505 at( rowIndex, colIndex ) = matrix_[ rowIndex ][ colIndex ];
00506 }
00507 }
00508
00509 }
00510
00511
00512
00513
00514
00515
00516 template< size_t M, size_t N, typename float_t >
00517 void
00518 matrix< M, N, float_t >::operator=( const float_t* data_array )
00519 {
00520 copyFrom1DimCArray( data_array, true );
00521 }
00522
00523
00524
00525 template< size_t M, size_t N, typename float_t >
00526 void
00527 matrix< M, N, float_t >::operator=( const std::vector< float_t >& data )
00528 {
00529
00530 if ( data.size() >= M * N )
00531 VMMLIB_ERROR( "input data vector too small", VMMLIB_HERE );
00532 copyFrom1DimCArray( &data[ 0 ], true );
00533 }
00534
00535
00536
00537 template< size_t M, size_t N, typename float_t >
00538 template< size_t P >
00539 void
00540 matrix< M, N, float_t >::multiply(
00541 const matrix< M, P, float_t >& left,
00542 const matrix< P, N, float_t >& right
00543 )
00544 {
00545 float_t tmp;
00546 for( size_t rowIndex = 0; rowIndex < M; rowIndex++)
00547 {
00548 for( size_t colIndex = 0; colIndex < N; colIndex++)
00549 {
00550 tmp = static_cast< float_t >( 0.0 );
00551 for( size_t p = 0; p < P; p++)
00552 {
00553 tmp += left.at( rowIndex, p ) * right.at( p, colIndex );
00554 }
00555 at( rowIndex, colIndex ) = tmp;
00556 }
00557 }
00558 }
00559
00560
00561
00562 template< size_t M, size_t N, typename float_t >
00563 matrix< M, N, float_t >
00564 matrix< M, N, float_t >::operator*( matrix< M, N, float_t >& other )
00565 {
00566
00567
00568 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
00569
00570 matrix< M, N, float_t > result;
00571 result.multiply( *this, other );
00572 return result;
00573 }
00574
00575
00576
00577 template< size_t M, size_t N, typename float_t >
00578 template< size_t P >
00579 matrix< M, P, float_t >
00580 matrix< M, N, float_t >::operator*( matrix< N, P, float_t >& other )
00581 {
00582 matrix< M, P, float_t > result;
00583 result.multiply( *this, other );
00584 return result;
00585 }
00586
00587
00588
00589
00590 template< size_t M, size_t N, typename float_t >
00591 void
00592 matrix< M, N, float_t >::operator*=( const matrix< M, M, float_t >& right )
00593 {
00594 matrix< M, M, float_t > copy( *this );
00595 multiply( copy, right );
00596 }
00597
00598
00599
00600
00601 template< size_t M, size_t N, typename float_t >
00602 matrix< M, N, float_t >
00603 matrix< M, N, float_t >::operator*( float_t scalar )
00604 {
00605 matrix< M, N, float_t > result;
00606
00607 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00608 {
00609 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00610 {
00611 result.at( rowIndex, colIndex ) = at( rowIndex, colIndex ) * scalar;
00612 }
00613 }
00614 return result;
00615 }
00616
00617
00618
00619 template< size_t M, size_t N, typename float_t >
00620 void
00621 matrix< M, N, float_t >::operator*=( float_t scalar )
00622 {
00623 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00624 {
00625 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00626 {
00627 at( rowIndex, colIndex ) *= scalar;
00628 }
00629 }
00630 }
00631
00632
00633
00634 template< size_t M, size_t N, typename float_t >
00635 vector< M, float_t >
00636 matrix< M, N, float_t >::
00637 operator*( const vector< N, float_t >& other ) const
00638 {
00639 vector< M, float_t > result;
00640
00641
00642 float_t tmp;
00643 for( size_t rowIndex = 0; rowIndex < M; rowIndex++)
00644 {
00645 tmp = static_cast< float_t >( 0.0 );
00646 for( size_t p = 0; p < N; p++)
00647 {
00648 tmp += at( rowIndex, p ) * other.at( p );
00649 }
00650 result.at( rowIndex ) = tmp;
00651 }
00652 return result;
00653 }
00654
00655
00656
00657
00658
00659 template< size_t M, size_t N, typename float_t >
00660 vector< M-1, float_t >
00661 matrix< M, N, float_t >::
00662 operator*( const vector< N-1, float_t >& other ) const
00663 {
00664
00665
00666 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
00667
00668 vector< M-1, float_t > result;
00669 float tmp;
00670 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00671 {
00672 tmp = 0.0;
00673 for( size_t colIndex = 0; colIndex < N-1; ++colIndex )
00674 {
00675 tmp += other( colIndex ) * at( rowIndex, colIndex );
00676 }
00677 if ( rowIndex < N - 1 )
00678 result( rowIndex ) = tmp + at( rowIndex, N-1 );
00679 else
00680 {
00681 tmp += at( rowIndex, N - 1 );
00682 for( size_t colIndex = 0; colIndex < N - 1; ++colIndex )
00683 {
00684 result( colIndex ) /= tmp;
00685 }
00686 }
00687 }
00688 return result;
00689 }
00690
00691
00692
00693 template< size_t M, size_t N, typename float_t >
00694 inline matrix< M, N, float_t >
00695 matrix< M, N, float_t >::operator-() const
00696 {
00697 return negate();
00698 }
00699
00700
00701
00702 template< size_t M, size_t N, typename float_t >
00703 matrix< M, N, float_t >
00704 matrix< M, N, float_t >::negate() const
00705 {
00706 matrix< M, N, float_t > result;
00707 result *= -1.0;
00708 return result;
00709 }
00710
00711
00712
00713 template< size_t M, size_t N, typename float_t >
00714 void
00715 matrix< M, N, float_t >::tensor(
00716 const vector< M, float_t >& u,
00717 const vector< N, float_t >& v
00718 )
00719 {
00720 for ( size_t colIndex = 0; colIndex < N; ++colIndex )
00721 for ( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00722 at( rowIndex, colIndex ) = u.array[ colIndex ] * v.array[ rowIndex ];
00723
00724 }
00725
00726
00727
00728 template< size_t M, size_t N, typename float_t >
00729 void
00730 matrix< M, N, float_t >::tensor(
00731 const vector< M-1, float_t >& u,
00732 const vector< N-1, float_t >& v
00733 )
00734 {
00735
00736
00737 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
00738
00739 int i, j;
00740 for ( size_t colIndex = 0; colIndex < 3; ++colIndex )
00741 {
00742 for ( size_t rowIndex = 0; rowIndex < 3; ++rowIndex )
00743 at( rowIndex, colIndex ) = u.array[ colIndex ] * v.array[ rowIndex ];
00744
00745 at( 3, colIndex ) = u.array[ colIndex ];
00746 }
00747
00748 for ( size_t rowIndex = 0; rowIndex < 3; ++rowIndex )
00749 at( rowIndex, 3 ) = v.array[ rowIndex ];
00750
00751 at( 3, 3 ) = 1.0;
00752
00753 }
00754
00755
00756
00757 template< size_t M, size_t N, typename float_t >
00758 void
00759 matrix< M, N, float_t >::
00760 transposeTo( matrix< N, M, float_t >& tM ) const
00761 {
00762 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00763 {
00764 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00765 {
00766 tM.at( colIndex, rowIndex ) = at( rowIndex, colIndex );
00767 }
00768 }
00769 }
00770
00771
00772
00773 template< size_t M, size_t N, typename float_t >
00774 matrix< N, M, float_t >
00775 matrix< M, N, float_t >::getTransposed() const
00776 {
00777 matrix< N, M, float_t > result;
00778 transposeTo( result );
00779 return result;
00780 }
00781
00782
00783
00784 template< size_t M, size_t N, typename float_t >
00785 void
00786 matrix< M, N, float_t >::
00787 copyFrom1DimCArray( const float_t* c_array, bool row_by_row_layout )
00788 {
00789 if ( row_by_row_layout )
00790 {
00791 for( size_t index = 0, rowIndex = 0; rowIndex < M; ++rowIndex )
00792 {
00793 for( size_t colIndex = 0; colIndex < N; ++colIndex, ++index )
00794 {
00795 at( rowIndex, colIndex ) = c_array[ index ];
00796 }
00797 }
00798 }
00799 else
00800 {
00801 memcpy( array, c_array, M * N * sizeof( float_t ) );
00802 }
00803 }
00804
00805
00806 template< size_t M, size_t N, typename float_t >
00807 template< typename different_float_t >
00808 void
00809 matrix< M, N, float_t >::
00810 copyFrom1DimCArray( const different_float_t* c_array, bool row_by_row_layout )
00811 {
00812 if ( row_by_row_layout )
00813 {
00814 for( size_t index = 0, rowIndex = 0; rowIndex < M; ++rowIndex )
00815 {
00816 for( size_t colIndex = 0; colIndex < N; ++colIndex, ++index )
00817 {
00818 at( rowIndex, colIndex ) = static_cast< float_t >( c_array[ index ] );
00819 }
00820 }
00821 }
00822 else
00823 {
00824 for( size_t index = 0, colIndex = 0; colIndex < N; ++colIndex )
00825 {
00826 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex, ++index )
00827 {
00828 at( rowIndex, colIndex ) = static_cast< float_t >( c_array[ index ] );
00829 }
00830 }
00831 }
00832 }
00833
00834
00835
00836 template< size_t M, size_t N, typename float_t >
00837 vector< M, float_t >
00838 matrix< M, N, float_t >::
00839 getColumn( size_t columnNumber ) const
00840 {
00841 vector< M, float_t > column;
00842 getColumn( columnNumber, column );
00843 return column;
00844 }
00845
00846
00847
00848 template< size_t M, size_t N, typename float_t >
00849 void
00850 matrix< M, N, float_t >::
00851 getColumn( size_t columnNumber, vector< M, float_t >& column ) const
00852 {
00853 #ifdef VMMLIB_SAFE_ACCESSORS
00854 if ( columnNumber >= N )
00855 VMMLIB_ERROR( "getColumn() - index out of bounds.", VMMLIB_HERE );
00856 #endif
00857
00858 memcpy( &column.array[0], &array[ M * columnNumber ], M * sizeof( float_t ) );
00859 }
00860
00861
00862
00863 template< size_t M, size_t N, typename float_t >
00864 void
00865 matrix< M, N, float_t >::
00866 setColumn( size_t columnNumber, const vector< M, float_t >& column )
00867 {
00868 #ifdef VMMLIB_SAFE_ACCESSORS
00869 if ( columnNumber >= N )
00870 VMMLIB_ERROR( "setColumn() - index out of bounds.", VMMLIB_HERE );
00871 #endif
00872
00873 memcpy( &array[ M * columnNumber ], &column.array[0], M * sizeof( float_t ) );
00874 }
00875
00876
00877
00878 template< size_t M, size_t N, typename float_t >
00879 void
00880 matrix< M, N, float_t >::
00881 getColumn( size_t columnNumber, matrix< M, 1, float_t >& column ) const
00882 {
00883 #if 0
00884 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00885 {
00886
00887 column.at( rowIndex, 1 ) = at( rowIndex, columnNumber );
00888 }
00889 #endif
00890
00891 #ifdef VMMLIB_SAFE_ACCESSORS
00892 if ( columnNumber >= N )
00893 VMMLIB_ERROR( "getColumn() - index out of bounds.", VMMLIB_HERE );
00894 #endif
00895
00896 memcpy( &column.array[0], &array[ M * columnNumber ], M * sizeof( float_t ) );
00897 }
00898
00899
00900 template< size_t M, size_t N, typename float_t >
00901 void
00902 matrix< M, N, float_t >::
00903 setColumn( size_t columnNumber, const matrix< M, 1, float_t >& column )
00904 {
00905 #if 0
00906 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
00907 {
00908 at( rowIndex, columnNumber ) = column.at( rowIndex, 0 );
00909 }
00910 #else
00911
00912 #ifdef VMMLIB_SAFE_ACCESSORS
00913 if ( columnNumber >= N )
00914 VMMLIB_ERROR( "setColumn() - index out of bounds.", VMMLIB_HERE );
00915 #endif
00916
00917 memcpy( &array[ M * columnNumber ], &column.array[0], M * sizeof( float_t ) );
00918
00919 #endif
00920 }
00921
00922
00923 template< size_t M, size_t N, typename float_t >
00924 vector< N, float_t >
00925 matrix< M, N, float_t >::
00926 getRow( size_t rowIndex ) const
00927 {
00928 vector< N, float_t > row;
00929 getRow( rowIndex, row );
00930 return row;
00931 }
00932
00933
00934 template< size_t M, size_t N, typename float_t >
00935 void
00936 matrix< M, N, float_t >::
00937 getRow( size_t rowIndex, vector< N, float_t >& row ) const
00938 {
00939 #ifdef VMMLIB_SAFE_ACCESSORS
00940 if ( rowIndex >= M )
00941 VMMLIB_ERROR( "getRow() - index out of bounds.", VMMLIB_HERE );
00942 #endif
00943
00944 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00945 {
00946 row.at( colIndex ) = at( rowIndex, colIndex );
00947 }
00948 }
00949
00950
00951
00952 template< size_t M, size_t N, typename float_t >
00953 void
00954 matrix< M, N, float_t >::
00955 setRow( size_t rowIndex, const vector< N, float_t >& row )
00956 {
00957 #ifdef VMMLIB_SAFE_ACCESSORS
00958 if ( rowIndex >= M )
00959 VMMLIB_ERROR( "setRow() - index out of bounds.", VMMLIB_HERE );
00960 #endif
00961
00962 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00963 {
00964 at( rowIndex, colIndex ) = row.at( colIndex );
00965 }
00966 }
00967
00968
00969
00970 template< size_t M, size_t N, typename float_t >
00971 void
00972 matrix< M, N, float_t >::
00973 getRow( size_t rowIndex, matrix< 1, N, float_t >& row ) const
00974 {
00975 #ifdef VMMLIB_SAFE_ACCESSORS
00976 if ( rowIndex >= M )
00977 VMMLIB_ERROR( "getRow() - index out of bounds.", VMMLIB_HERE );
00978 #endif
00979
00980 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00981 {
00982 row.at( 0, colIndex ) = at( rowIndex, colIndex );
00983 }
00984 }
00985
00986
00987
00988 template< size_t M, size_t N, typename float_t >
00989 void
00990 matrix< M, N, float_t >::
00991 setRow( size_t rowIndex, const matrix< 1, N, float_t >& row )
00992 {
00993 #ifdef VMMLIB_SAFE_ACCESSORS
00994 if ( rowIndex >= M )
00995 VMMLIB_ERROR( "getRow() - index out of bounds.", VMMLIB_HERE );
00996 #endif
00997
00998 for( size_t colIndex = 0; colIndex < N; ++colIndex )
00999 {
01000 at( rowIndex, colIndex ) = row.at( 0, colIndex );
01001 }
01002 }
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031 template< size_t M, size_t N, typename float_t >
01032 size_t
01033 matrix< M, N, float_t >::
01034 getM() const
01035 {
01036 return M;
01037 }
01038
01039
01040
01041 template< size_t M, size_t N, typename float_t >
01042 size_t
01043 matrix< M, N, float_t >::
01044 getN() const
01045 {
01046 return N;
01047 }
01048
01049
01050
01051 template< size_t M, size_t N, typename float_t >
01052 size_t
01053 matrix< M, N, float_t >::
01054 getNumberOfRows() const
01055 {
01056 return M;
01057 }
01058
01059
01060
01061 template< size_t M, size_t N, typename float_t >
01062 size_t
01063 matrix< M, N, float_t >::
01064 getNumberOfColumns() const
01065 {
01066 return N;
01067 }
01068
01069
01070
01071 template< size_t M, size_t N, typename float_t >
01072 void
01073 matrix< M, N, float_t >::
01074 fill( float_t fillValue )
01075 {
01076 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01077 {
01078 for( size_t colIndex = 0; colIndex < N; ++colIndex )
01079 {
01080 at( rowIndex, colIndex ) = fillValue;
01081 }
01082 }
01083 }
01084
01085
01086
01087
01088
01089
01090
01091 template< size_t M, size_t N, typename float_t >
01092 bool
01093 matrix< M, N, float_t >::set( const std::string& values, char delimiter )
01094 {
01095 std::vector< std::string > tokens;
01096 stringUtil::split( values, tokens, delimiter );
01097 return set( tokens );
01098 }
01099
01100
01101
01102
01103
01104
01105
01106 template< size_t M, size_t N, typename float_t >
01107 bool
01108 matrix< M, N, float_t >::set( const std::vector< std::string >& values )
01109 {
01110 bool ok = true;
01111
01112 if ( values.size() < M * N )
01113 return false;
01114
01115 std::vector< std::string >::const_iterator it = values.begin();
01116
01117 for( size_t row = 0; row < M; ++row )
01118 {
01119 for( size_t col = 0; ok && col < N; ++col, ++it )
01120 {
01121
01122 ok = stringUtil::fromString< float_t >( *it, at( row, col ) );
01123 }
01124 }
01125
01126 return ok;
01127 }
01128
01129
01130
01131
01132 template< size_t M, size_t N, typename float_t >
01133 void
01134 matrix< M, N, float_t >::set(
01135 float_t v00, float_t v01, float_t v02,
01136 float_t v10, float_t v11, float_t v12,
01137 float_t v20, float_t v21, float_t v22
01138 )
01139 {
01140
01141
01142 details::matrix_is_3x3< M, N, matrix< M, N, float_t > >();
01143
01144 array[ 0 ] = v00;
01145 array[ 1 ] = v10;
01146 array[ 2 ] = v20;
01147 array[ 3 ] = v01;
01148 array[ 4 ] = v11;
01149 array[ 5 ] = v21;
01150 array[ 6 ] = v02;
01151 array[ 7 ] = v12;
01152 array[ 8 ] = v22;
01153 }
01154
01155
01156
01157
01158 template< size_t M, size_t N, typename float_t >
01159 void
01160 matrix< M, N, float_t >::set(
01161 float_t v00, float_t v01, float_t v02, float_t v03,
01162 float_t v10, float_t v11, float_t v12, float_t v13,
01163 float_t v20, float_t v21, float_t v22, float_t v23,
01164 float_t v30, float_t v31, float_t v32, float_t v33
01165 )
01166 {
01167
01168
01169 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
01170
01171 array[ 0 ] = v00;
01172 array[ 1 ] = v10;
01173 array[ 2 ] = v20;
01174 array[ 3 ] = v30;
01175 array[ 4 ] = v01;
01176 array[ 5 ] = v11;
01177 array[ 6 ] = v21;
01178 array[ 7 ] = v31;
01179 array[ 8 ] = v02;
01180 array[ 9 ] = v12;
01181 array[ 10 ] = v22;
01182 array[ 11 ] = v32;
01183 array[ 12 ] = v03;
01184 array[ 13 ] = v13;
01185 array[ 14 ] = v23;
01186 array[ 15 ] = v33;
01187
01188 }
01189
01190
01191
01192
01193 template< size_t M, size_t N, typename float_t >
01194 void
01195 matrix< M, N, float_t >::
01196 identity()
01197 {
01198 (*this) = IDENTITY;
01199 }
01200
01201
01202 template< size_t M, size_t N, typename float_t >
01203 void
01204 matrix< M, N, float_t >::
01205 zero()
01206 {
01207 (*this) = ZERO;
01208 }
01209
01210
01211 template< size_t M, size_t N, typename float_t >
01212 void
01213 matrix< M, N, float_t >::
01214 operator=( float_t fillValue )
01215 {
01216 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01217 {
01218 for( size_t colIndex = 0; colIndex < N; ++colIndex )
01219 {
01220 at( rowIndex, colIndex ) = fillValue;
01221 }
01222 }
01223 }
01224
01225
01226
01227 template< size_t M, size_t N, typename float_t >
01228 inline matrix< M, N, float_t >
01229 matrix< M, N, float_t >::
01230 operator+( const matrix< M, N, float_t >& other ) const
01231 {
01232 matrix< M, N, float_t > result( *this );
01233 result += other;
01234 return result;
01235 }
01236
01237
01238
01239 template< size_t M, size_t N, typename float_t >
01240 void
01241 matrix< M, N, float_t >::
01242 operator+=( const matrix< M, N, float_t >& other )
01243 {
01244 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01245 {
01246 for( size_t colIndex = 0; colIndex < N; ++colIndex )
01247 {
01248 at( rowIndex, colIndex ) += other.at( rowIndex, colIndex );
01249 }
01250 }
01251 }
01252
01253
01254
01255 template< size_t M, size_t N, typename float_t >
01256 inline matrix< M, N, float_t >
01257 matrix< M, N, float_t >::
01258 operator-( const matrix< M, N, float_t >& other ) const
01259 {
01260 matrix< M, N, float_t > result( *this );
01261 result -= other;
01262 return result;
01263 }
01264
01265
01266
01267 template< size_t M, size_t N, typename float_t >
01268 void
01269 matrix< M, N, float_t >::
01270 operator-=( const matrix< M, N, float_t >& other )
01271 {
01272 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01273 {
01274 for( size_t colIndex = 0; colIndex < N; ++colIndex )
01275 {
01276 at( rowIndex, colIndex ) -= other.at( rowIndex, colIndex );
01277 }
01278 }
01279 }
01280
01281
01282
01283 template< size_t M, size_t N, typename float_t >
01284 template< size_t P, size_t Q >
01285 void
01286 matrix< M, N, float_t >::
01287 directSum( const matrix< P, Q, float_t >& other, matrix< M + P, N + Q, float_t >& result )
01288 {
01289 result.fill( 0.0 );
01290
01291
01292 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01293 {
01294 for( size_t colIndex = 0; colIndex < N; ++colIndex )
01295 {
01296 result.at( rowIndex, colIndex ) = at( rowIndex, colIndex );
01297 }
01298 }
01299
01300 for( size_t rowIndex = 0; rowIndex < P; ++rowIndex )
01301 {
01302 for( size_t colIndex = 0; colIndex < Q; ++colIndex )
01303 {
01304 result.at( M + rowIndex, N + colIndex ) = other.at( rowIndex, colIndex );
01305 }
01306
01307 }
01308
01309 }
01310
01311
01312 template< size_t M, size_t N, typename float_t >
01313 template< size_t P, size_t Q >
01314 matrix< M + P, N + Q, float_t >
01315 matrix< M, N, float_t >::
01316 directSum( const matrix< P, Q, float_t >& other )
01317 {
01318 matrix< M + P, N + Q, float_t > result;
01319 directSum( other, result );
01320 return result;
01321 }
01322
01323
01324 template< size_t M, size_t N, typename float_t >
01325 template< size_t Mret, size_t Nret >
01326 matrix< Mret, Nret, float_t >
01327 matrix< M, N, float_t >::getSubMatrix( size_t rowOffset,
01328 size_t colOffset ) const
01329 {
01330 matrix< Mret, Nret, float_t > result;
01331 getSubMatrix( result, rowOffset, colOffset );
01332 return result;
01333 }
01334
01335
01336
01337 template< size_t M, size_t N, typename float_t >
01338 template< size_t Mret, size_t Nret >
01339 void
01340 matrix< M, N, float_t >::getSubMatrix( matrix< Mret, Nret, float_t >& result,
01341 size_t rowOffset, size_t colOffset ) const
01342 {
01343 for( size_t rowIndex = 0; rowIndex < Mret; ++rowIndex )
01344 {
01345 for( size_t colIndex = 0; colIndex < Nret; ++colIndex )
01346 {
01347 result.at( rowIndex, colIndex )
01348 = at( rowOffset + rowIndex, colOffset + colIndex );
01349 }
01350 }
01351 }
01352
01353
01354
01355 template< size_t M, size_t N, typename float_t >
01356 template< size_t Mret, size_t Nret >
01357 void
01358 matrix< M, N, float_t >::
01359 setSubMatrix( const matrix< Mret, Nret, float_t >& subMatrix,
01360 size_t rowOffset, size_t colOffset )
01361 {
01362 for( size_t rowIndex = 0; rowIndex < Mret; ++rowIndex )
01363 {
01364 for( size_t colIndex = 0; colIndex < Nret; ++colIndex )
01365 {
01366 at( rowOffset + rowIndex, colOffset + colIndex )
01367 = subMatrix.at( rowIndex, colIndex );
01368 }
01369 }
01370 }
01371
01372
01373
01374 template< size_t M, size_t N, typename float_t >
01375 inline float_t
01376 matrix< M, N, float_t >::getDeterminant() const
01377 {
01378
01379
01380 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
01381
01382 throw VMMLIB_ERROR( "not implemented yet.", VMMLIB_HERE );
01383
01384 return 0.0;
01385 }
01386
01387
01388
01389 template<>
01390 inline float
01391 matrix< 2, 2, float >::getDeterminant() const
01392 {
01393 const float& a = at( 0, 0 );
01394 const float& b = at( 0, 1 );
01395 const float& c = at( 1, 0 );
01396 const float& d = at( 1, 1 );
01397 return a * d - b * c;
01398 }
01399
01400
01401 template<>
01402 inline double
01403 matrix< 2, 2, double >::getDeterminant() const
01404 {
01405 const double& a = at( 0, 0 );
01406 const double& b = at( 0, 1 );
01407 const double& c = at( 1, 0 );
01408 const double& d = at( 1, 1 );
01409 return a * d - b * c;
01410 }
01411
01412
01413
01414
01415 template<>
01416 inline float
01417 matrix< 3, 3, float >::getDeterminant() const
01418 {
01419 const vector< 3, float > cof(
01420 at( 1,1 ) * at( 2,2 ) - at( 1,2 ) * at( 2,1 ),
01421 at( 1,2 ) * at( 2,0 ) - at( 1,0 ) * at( 2,2 ),
01422 at( 1,0 ) * at( 2,1 ) - at( 1,1 ) * at( 2,0 )
01423 );
01424
01425 return at( 0,0 ) * cof( 0 ) + at( 0,1 ) * cof( 1 ) + at( 0,2 ) * cof( 2 );
01426 }
01427
01428
01429
01430 template<>
01431 inline double
01432 matrix< 3, 3, double >::getDeterminant() const
01433 {
01434 const vector< 3, double > cof(
01435 at( 1,1 ) * at( 2,2 ) - at( 1,2 ) * at( 2,1 ),
01436 at( 1,2 ) * at( 2,0 ) - at( 1,0 ) * at( 2,2 ),
01437 at( 1,0 ) * at( 2,1 ) - at( 1,1 ) * at( 2,0 )
01438 );
01439
01440 return at( 0,0 ) * cof( 0 ) + at( 0,1 ) * cof( 1 ) + at( 0,2 ) * cof( 2 );
01441 }
01442
01443
01444
01445
01446 template< size_t M, size_t N, typename float_t >
01447 inline void
01448 matrix< M, N, float_t >::getAdjugate( matrix< M, M, float_t >& adjugate ) const
01449 {
01450
01451
01452 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
01453
01454 getCofactors( adjugate );
01455 adjugate = adjugate.getTransposed();
01456 }
01457
01458
01459
01460 template<>
01461 inline void
01462 matrix< 2, 2, float >::getAdjugate( matrix< 2, 2, float >& adjugate ) const
01463 {
01464 const float& a = at( 0, 0 );
01465 const float& b = at( 0, 1 );
01466 const float& c = at( 1, 0 );
01467 const float& d = at( 1, 1 );
01468
01469 adjugate( 0, 0 ) = d;
01470 adjugate( 0, 1 ) = -b;
01471 adjugate( 1, 0 ) = -c;
01472 adjugate( 1, 1 ) = a;
01473 }
01474
01475
01476
01477 template<>
01478 inline void
01479 matrix< 2, 2, double >::getAdjugate( matrix< 2, 2, double >& adjugate ) const
01480 {
01481 const double& a = at( 0, 0 );
01482 const double& b = at( 0, 1 );
01483 const double& c = at( 1, 0 );
01484 const double& d = at( 1, 1 );
01485
01486 adjugate( 0, 0 ) = d;
01487 adjugate( 0, 1 ) = -b;
01488 adjugate( 1, 0 ) = -c;
01489 adjugate( 1, 1 ) = a;
01490 }
01491
01492
01493
01494 template< size_t M, size_t N, typename float_t >
01495 inline void
01496 matrix< M, N, float_t >::
01497 getCofactors( matrix< M, N, float_t >& cofactors ) const
01498 {
01499 size_t negate = 1u;
01500 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01501 {
01502 for( size_t colIndex = 0; colIndex < N; ++colIndex )
01503 {
01504 if ( ( rowIndex + colIndex ) & negate )
01505 cofactors( rowIndex, colIndex ) = -getMinor( rowIndex, colIndex );
01506 else
01507 cofactors( rowIndex, colIndex ) = getMinor( rowIndex, colIndex );
01508 }
01509 }
01510 }
01511
01512
01513
01514
01515 template< size_t M, size_t N, typename float_t >
01516 inline bool
01517 matrix< M, N, float_t >::getInverse( matrix< M, M, float_t >& Minverse, float_t tolerance ) const
01518 {
01519
01520
01521 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
01522
01523 VMMLIB_ERROR( "not implemented yet.", VMMLIB_HERE );
01524
01525 return false;
01526 }
01527
01528
01529
01530 template<>
01531 inline bool
01532 matrix< 2, 2, float >::getInverse( matrix< 2, 2, float >& Minverse, float tolerance ) const
01533 {
01534 float det = getDeterminant();
01535 if ( fabs( det ) < tolerance )
01536 {
01537 return false;
01538 }
01539 float reciprocal_of_determinant = 1.0f / det;
01540
01541
01542 getAdjugate( Minverse );
01543
01544 Minverse *= reciprocal_of_determinant;
01545
01546 return true;
01547 }
01548
01549
01550 template<>
01551 inline bool
01552 matrix< 2, 2, double >::getInverse( matrix< 2, 2, double >& Minverse, double tolerance ) const
01553 {
01554 double det = getDeterminant();
01555 if ( fabs( det ) < tolerance )
01556 {
01557 return false;
01558 }
01559 double reciprocal_of_determinant = 1.0 / det;
01560
01561
01562 getAdjugate( Minverse );
01563
01564 Minverse *= reciprocal_of_determinant;
01565
01566 return true;
01567 }
01568
01569
01570
01571 template<>
01572 inline bool
01573 matrix< 3, 3, float >::getInverse( matrix< 3, 3, float >& result, float tolerance ) const
01574 {
01575
01576
01577
01578 result.at( 0, 0 ) = at( 1, 1 ) * at( 2, 2 ) - at( 1, 2 ) * at( 2, 1 );
01579 result.at( 0, 1 ) = at( 0, 2 ) * at( 2, 1 ) - at( 0, 1 ) * at( 2, 2 );
01580 result.at( 0, 2 ) = at( 0, 1 ) * at( 1, 2 ) - at( 0, 2 ) * at( 1, 1 );
01581 result.at( 1, 0 ) = at( 1, 2 ) * at( 2, 0 ) - at( 1, 0 ) * at( 2, 2 );
01582 result.at( 1, 1 ) = at( 0, 0 ) * at( 2, 2 ) - at( 0, 2 ) * at( 2, 0 );
01583 result.at( 1, 2 ) = at( 0, 2 ) * at( 1, 0 ) - at( 0, 0 ) * at( 1, 2 );
01584 result.at( 2, 0 ) = at( 1, 0 ) * at( 2, 1 ) - at( 1, 1 ) * at( 2, 0 );
01585 result.at( 2, 1 ) = at( 0, 1 ) * at( 2, 0 ) - at( 0, 0 ) * at( 2, 1 );
01586 result.at( 2, 2 ) = at( 0, 0 ) * at( 1, 1 ) - at( 0, 1 ) * at( 1, 0 );
01587
01588 const float determinant = at( 0, 0 ) * result.at( 0, 0 )
01589 + at( 0, 1 ) * result.at( 1, 0 )
01590 + at( 0, 2 ) * result.at( 2, 0 );
01591
01592 if ( fabs( determinant ) <= tolerance )
01593 return false;
01594
01595 const float detinv = (float)(1.0 / determinant);
01596
01597 result.at( 0, 0 ) *= detinv;
01598 result.at( 0, 1 ) *= detinv;
01599 result.at( 0, 2 ) *= detinv;
01600 result.at( 1, 0 ) *= detinv;
01601 result.at( 1, 1 ) *= detinv;
01602 result.at( 1, 2 ) *= detinv;
01603 result.at( 2, 0 ) *= detinv;
01604 result.at( 2, 1 ) *= detinv;
01605 result.at( 2, 2 ) *= detinv;
01606
01607 return true;
01608 }
01609
01610
01611
01612 template<>
01613 inline bool
01614 matrix< 3, 3, double >::getInverse( matrix< 3, 3, double >& result, double tolerance ) const
01615 {
01616
01617
01618
01619 result.at( 0, 0 ) = at( 1, 1 ) * at( 2, 2 ) - at( 1, 2 ) * at( 2, 1 );
01620 result.at( 0, 1 ) = at( 0, 2 ) * at( 2, 1 ) - at( 0, 1 ) * at( 2, 2 );
01621 result.at( 0, 2 ) = at( 0, 1 ) * at( 1, 2 ) - at( 0, 2 ) * at( 1, 1 );
01622 result.at( 1, 0 ) = at( 1, 2 ) * at( 2, 0 ) - at( 1, 0 ) * at( 2, 2 );
01623 result.at( 1, 1 ) = at( 0, 0 ) * at( 2, 2 ) - at( 0, 2 ) * at( 2, 0 );
01624 result.at( 1, 2 ) = at( 0, 2 ) * at( 1, 0 ) - at( 0, 0 ) * at( 1, 2 );
01625 result.at( 2, 0 ) = at( 1, 0 ) * at( 2, 1 ) - at( 1, 1 ) * at( 2, 0 );
01626 result.at( 2, 1 ) = at( 0, 1 ) * at( 2, 0 ) - at( 0, 0 ) * at( 2, 1 );
01627 result.at( 2, 2 ) = at( 0, 0 ) * at( 1, 1 ) - at( 0, 1 ) * at( 1, 0 );
01628
01629 const double determinant = at( 0, 0 ) * result.at( 0, 0 )
01630 + at( 0, 1 ) * result.at( 1, 0 )
01631 + at( 0, 2 ) * result.at( 2, 0 );
01632
01633 if ( fabs( determinant ) <= tolerance )
01634 return false;
01635
01636 const double detinv = 1.0 / determinant;
01637
01638 result.at( 0, 0 ) *= detinv;
01639 result.at( 0, 1 ) *= detinv;
01640 result.at( 0, 2 ) *= detinv;
01641 result.at( 1, 0 ) *= detinv;
01642 result.at( 1, 1 ) *= detinv;
01643 result.at( 1, 2 ) *= detinv;
01644 result.at( 2, 0 ) *= detinv;
01645 result.at( 2, 1 ) *= detinv;
01646 result.at( 2, 2 ) *= detinv;
01647
01648 return true;
01649 }
01650
01651
01652
01653 template<>
01654 inline bool
01655 matrix< 4, 4, float >::getInverse( matrix< 4, 4, float >& result, float tolerance ) const
01656 {
01657
01658
01659 const float t1[6] = { array[ 2] * array[ 7] - array[ 6] * array[ 3],
01660 array[ 2] * array[11] - array[10] * array[ 3],
01661 array[ 2] * array[15] - array[14] * array[ 3],
01662 array[ 6] * array[11] - array[10] * array[ 7],
01663 array[ 6] * array[15] - array[14] * array[ 7],
01664 array[10] * array[15] - array[14] * array[11] };
01665
01666
01667 result.array[0] = array[ 5] * t1[5] - array[ 9] * t1[4] + array[13] * t1[3];
01668 result.array[1] = array[ 9] * t1[2] - array[13] * t1[1] - array[ 1] * t1[5];
01669 result.array[2] = array[13] * t1[0] - array[ 5] * t1[2] + array[ 1] * t1[4];
01670 result.array[3] = array[ 5] * t1[1] - array[ 1] * t1[3] - array[ 9] * t1[0];
01671 result.array[4] = array[ 8] * t1[4] - array[ 4] * t1[5] - array[12] * t1[3];
01672 result.array[5] = array[ 0] * t1[5] - array[ 8] * t1[2] + array[12] * t1[1];
01673 result.array[6] = array[ 4] * t1[2] - array[12] * t1[0] - array[ 0] * t1[4];
01674 result.array[7] = array[ 0] * t1[3] - array[ 4] * t1[1] + array[ 8] * t1[0];
01675
01676
01677 const float t2[6] = { array[ 0] * array[ 5] - array[ 4] * array[ 1],
01678 array[ 0] * array[ 9] - array[ 8] * array[ 1],
01679 array[ 0] * array[13] - array[12] * array[ 1],
01680 array[ 4] * array[ 9] - array[ 8] * array[ 5],
01681 array[ 4] * array[13] - array[12] * array[ 5],
01682 array[ 8] * array[13] - array[12] * array[ 9] };
01683
01684
01685 result.array[8] = array[ 7] * t2[5] - array[11] * t2[4] + array[15] * t2[3];
01686 result.array[9] = array[11] * t2[2] - array[15] * t2[1] - array[ 3] * t2[5];
01687 result.array[10] = array[15] * t2[0] - array[ 7] * t2[2] + array[ 3] * t2[4];
01688 result.array[11] = array[ 7] * t2[1] - array[ 3] * t2[3] - array[11] * t2[0];
01689 result.array[12] = array[10] * t2[4] - array[ 6] * t2[5] - array[14] * t2[3];
01690 result.array[13] = array[ 2] * t2[5] - array[10] * t2[2] + array[14] * t2[1];
01691 result.array[14] = array[ 6] * t2[2] - array[14] * t2[0] - array[ 2] * t2[4];
01692 result.array[15] = array[ 2] * t2[3] - array[ 6] * t2[1] + array[10] * t2[0];
01693
01694
01695 const float determinant = array[0] * result.array[0] + array[4] * result.array[1] +
01696 array[8] * result.array[2] + array[12] * result.array[3];
01697
01698 if( fabs( determinant ) <= tolerance )
01699 return false;
01700
01701
01702 const float detinv = (float)(1.0 / determinant);
01703 for( unsigned i = 0; i != 16; ++i )
01704 result.array[i] *= detinv;
01705
01706 return true;
01707 }
01708
01709
01710
01711 template<>
01712 inline bool
01713 matrix< 4, 4, double >::getInverse( matrix< 4, 4, double >& result, double tolerance ) const
01714 {
01715
01716
01717 const double t1[6] = { array[ 2] * array[ 7] - array[ 6] * array[ 3],
01718 array[ 2] * array[11] - array[10] * array[ 3],
01719 array[ 2] * array[15] - array[14] * array[ 3],
01720 array[ 6] * array[11] - array[10] * array[ 7],
01721 array[ 6] * array[15] - array[14] * array[ 7],
01722 array[10] * array[15] - array[14] * array[11] };
01723
01724
01725 result.array[0] = array[ 5] * t1[5] - array[ 9] * t1[4] + array[13] * t1[3];
01726 result.array[1] = array[ 9] * t1[2] - array[13] * t1[1] - array[ 1] * t1[5];
01727 result.array[2] = array[13] * t1[0] - array[ 5] * t1[2] + array[ 1] * t1[4];
01728 result.array[3] = array[ 5] * t1[1] - array[ 1] * t1[3] - array[ 9] * t1[0];
01729 result.array[4] = array[ 8] * t1[4] - array[ 4] * t1[5] - array[12] * t1[3];
01730 result.array[5] = array[ 0] * t1[5] - array[ 8] * t1[2] + array[12] * t1[1];
01731 result.array[6] = array[ 4] * t1[2] - array[12] * t1[0] - array[ 0] * t1[4];
01732 result.array[7] = array[ 0] * t1[3] - array[ 4] * t1[1] + array[ 8] * t1[0];
01733
01734
01735 const double t2[6] = { array[ 0] * array[ 5] - array[ 4] * array[ 1],
01736 array[ 0] * array[ 9] - array[ 8] * array[ 1],
01737 array[ 0] * array[13] - array[12] * array[ 1],
01738 array[ 4] * array[ 9] - array[ 8] * array[ 5],
01739 array[ 4] * array[13] - array[12] * array[ 5],
01740 array[ 8] * array[13] - array[12] * array[ 9] };
01741
01742
01743 result.array[8] = array[ 7] * t2[5] - array[11] * t2[4] + array[15] * t2[3];
01744 result.array[9] = array[11] * t2[2] - array[15] * t2[1] - array[ 3] * t2[5];
01745 result.array[10] = array[15] * t2[0] - array[ 7] * t2[2] + array[ 3] * t2[4];
01746 result.array[11] = array[ 7] * t2[1] - array[ 3] * t2[3] - array[11] * t2[0];
01747 result.array[12] = array[10] * t2[4] - array[ 6] * t2[5] - array[14] * t2[3];
01748 result.array[13] = array[ 2] * t2[5] - array[10] * t2[2] + array[14] * t2[1];
01749 result.array[14] = array[ 6] * t2[2] - array[14] * t2[0] - array[ 2] * t2[4];
01750 result.array[15] = array[ 2] * t2[3] - array[ 6] * t2[1] + array[10] * t2[0];
01751
01752
01753 const double determinant = array[0] * result.array[0] + array[4] * result.array[1] +
01754 array[8] * result.array[2] + array[12] * result.array[3];
01755
01756 if( fabs( determinant ) <= tolerance )
01757 return false;
01758
01759
01760 const double detinv = 1.0 / determinant;
01761 for( unsigned i = 0; i != 16; ++i )
01762 result.array[i] *= detinv;
01763
01764 return true;
01765 }
01766
01767
01768
01769 template< size_t M, size_t N, typename float_t >
01770 inline float_t
01771 matrix< M, N, float_t >::
01772 getMinor(
01773 matrix< M-1, N-1 >& other,
01774 size_t row_to_cut,
01775 size_t col_to_cut
01776 )
01777 {
01778
01779
01780 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
01781
01782 matrix< M-1, N-1, float_t > minor_;
01783 ssize_t rowOffset = 0;
01784 ssize_t colOffset = 0;
01785 for( ssize_t rowIndex = 0; rowIndex < M; ++rowIndex )
01786 {
01787 if ( rowIndex == row_to_cut )
01788 rowOffset = -1;
01789 else
01790 {
01791 for( ssize_t colIndex = 0; colIndex < M; ++colIndex )
01792 {
01793 if ( colIndex == col_to_cut )
01794 colOffset = -1;
01795 else
01796 minor_.at( rowIndex + rowOffset, colIndex + colOffset )
01797 = at( rowIndex, colIndex );
01798 }
01799 colOffset = 0;
01800 }
01801 }
01802
01803 return minor_.getDeterminant();
01804 }
01805
01806
01807
01808 template< size_t M, size_t N, typename float_t >
01809 bool
01810 matrix< M, N, float_t >::
01811 isPositiveDefinite( const float_t limit ) const
01812 {
01813
01814
01815 details::matrix_is_square< M, N, matrix< M, N, float_t > >();
01816
01817 bool isPositiveDef = at( 0, 0 ) >= limit;
01818
01819
01820
01821
01822 if ( isPositiveDef && M > 1 )
01823 {
01824 matrix< 2, 2, float_t > m;
01825 getSubMatrix< 2, 2 >( m, 0, 0 );
01826 isPositiveDef = m.getDeterminant() >= limit;
01827 }
01828
01829 if ( isPositiveDef && M > 2 )
01830 {
01831 matrix< 3, 3, float_t > m;
01832 getSubMatrix< 3, 3 >( m, 0, 0 );
01833 isPositiveDef = m.getDeterminant() >= limit;
01834 }
01835
01836 return isPositiveDef;
01837
01838 }
01839
01840
01841
01842 template< size_t M, size_t N, typename float_t >
01843 void
01844 matrix< M, N, float_t >::
01845 rotate( const float_t angle, const vector< M-1, float_t >& axis )
01846 {
01847
01848
01849 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
01850
01851
01852
01853 const float_t sine = details::getSine( angle );
01854 const float_t cosine = details::getCosine( angle );
01855
01856
01857
01858
01859
01860
01861 const float_t zero = 0.0;
01862 const float_t one = 1.0;
01863 const float_t two = 2.0;
01864
01865 array[0] = cosine + ( one - cosine ) * pow( axis.array[0], two );
01866 array[1] = ( one - cosine ) * axis.array[0] * axis.array[1] + sine * axis.array[2];
01867 array[2] = ( one - cosine ) * axis.array[0] * axis.array[2] - sine * axis.array[1];
01868 array[3] = zero;
01869
01870 array[4] = ( one - cosine ) * axis.array[0] * axis.array[1] - sine * axis.array[2];
01871 array[5] = cosine + ( one - cosine ) * pow( axis.array[1], two );
01872 array[6] = ( one - cosine ) * axis.array[1] * axis.array[2] + sine * axis.array[0];
01873 array[7] = zero;
01874
01875 array[8] = ( one - cosine ) * axis.array[0] * axis.array[2] + sine * axis.array[1];
01876 array[9] = ( one - cosine ) * axis.array[1] * axis.array[2] - sine * axis.array[0];
01877 array[10] = cosine + ( one - cosine ) * pow( axis.array[2], two );
01878 array[11] = zero;
01879
01880 array[12] = zero;
01881 array[13] = zero;
01882 array[14] = zero;
01883 array[15] = one;
01884
01885 }
01886
01887
01888 template< size_t M, size_t N, typename float_t >
01889 void
01890 matrix< M, N, float_t >::
01891 rotateX( const float_t angle )
01892 {
01893
01894
01895 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
01896
01897
01898
01899 const float_t sine = details::getSine( angle );
01900 const float_t cosine = details::getCosine( angle );
01901
01902 float_t tmp;
01903
01904 #if 0
01905
01906 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01907 {
01908 tmp = at( rowIndex, 1 ) * cosine + at( rowIndex, 2 ) * sine;
01909 at( rowIndex, 2 ) = - at( rowIndex, 1 ) * sine + at( rowIndex, 2 ) * cosine;
01910 at( rowIndex, 1 ) = tmp;
01911 }
01912
01913 #else
01914
01915 tmp = array[ 4 ] * cosine + array[ 8 ] * sine;
01916 array[ 8 ] = - array[ 4 ] * sine + array[ 8 ] * cosine;
01917 array[ 4 ] = tmp;
01918
01919 tmp = array[ 5 ] * cosine + array[ 9 ] * sine;
01920 array[ 9 ] = - array[ 5 ] * sine + array[ 9 ] * cosine;
01921 array[ 5 ] = tmp;
01922
01923 tmp = array[ 6 ] * cosine + array[ 10 ] * sine;
01924 array[ 10 ] = - array[ 6 ] * sine + array[ 10 ] * cosine;
01925 array[ 6 ] = tmp;
01926
01927 tmp = array[ 7 ] * cosine + array[ 11 ] * sine;
01928 array[ 11 ] = - array[ 7 ] * sine + array[ 11 ] * cosine;
01929 array[ 7 ] = tmp;
01930
01931
01932 #endif
01933 }
01934
01935
01936 template< size_t M, size_t N, typename float_t >
01937 void
01938 matrix< M, N, float_t >::
01939 rotateY( const float_t angle )
01940 {
01941
01942
01943 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
01944
01945
01946
01947 const float_t sine = details::getSine( angle );
01948 const float_t cosine = details::getCosine( angle );
01949
01950 float_t tmp;
01951
01952 #if 0
01953
01954 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
01955 {
01956 tmp = at( rowIndex, 0 ) * cosine - at( rowIndex, 2 ) * sine;
01957 at( rowIndex, 2 ) = at( rowIndex, 0 ) * sine + at( rowIndex, 2 ) * cosine;
01958 at( rowIndex, 0 ) = tmp;
01959 }
01960
01961 #else
01962
01963 tmp = array[ 0 ] * cosine - array[ 8 ] * sine;
01964 array[ 8 ] = array[ 0 ] * sine + array[ 8 ] * cosine;
01965 array[ 0 ] = tmp;
01966
01967 tmp = array[ 1 ] * cosine - array[ 9 ] * sine;
01968 array[ 9 ] = array[ 1 ] * sine + array[ 9 ] * cosine;
01969 array[ 1 ] = tmp;
01970
01971 tmp = array[ 2 ] * cosine - array[ 10 ] * sine;
01972 array[ 10 ] = array[ 2 ] * sine + array[ 10 ] * cosine;
01973 array[ 2 ] = tmp;
01974
01975 tmp = array[ 3 ] * cosine - array[ 11 ] * sine;
01976 array[ 11 ] = array[ 3 ] * sine + array[ 11 ] * cosine;
01977 array[ 3 ] = tmp;
01978
01979 #endif
01980 }
01981
01982
01983 template< size_t M, size_t N, typename float_t >
01984 void
01985 matrix< M, N, float_t >::
01986 rotateZ( const float_t angle )
01987 {
01988
01989
01990 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
01991
01992
01993
01994 const float_t sine = details::getSine( angle );
01995 const float_t cosine = details::getCosine( angle );
01996
01997 float_t tmp;
01998
01999 #if 0
02000
02001 for( size_t rowIndex = 0; rowIndex < M; ++rowIndex )
02002 {
02003 tmp = at( rowIndex, 0 ) * cosine + at( rowIndex, 1 ) * sine;
02004 at( rowIndex, 1 ) = - at( rowIndex, 0 ) * sine + at( rowIndex, 1 ) * cosine;
02005 at( rowIndex, 0 ) = tmp;
02006 }
02007
02008 #else
02009
02010 tmp = array[ 0 ] * cosine + array[ 4 ] * sine;
02011 array[ 4 ] = - array[ 0 ] * sine + array[ 4 ] * cosine;
02012 array[ 0 ] = tmp;
02013
02014 tmp = array[ 1 ] * cosine + array[ 5 ] * sine;
02015 array[ 5 ] = - array[ 1 ] * sine + array[ 5 ] * cosine;
02016 array[ 1 ] = tmp;
02017
02018 tmp = array[ 2 ] * cosine + array[ 6 ] * sine;
02019 array[ 6 ] = - array[ 2 ] * sine + array[ 6 ] * cosine;
02020 array[ 2 ] = tmp;
02021
02022 tmp = array[ 3 ] * cosine + array[ 7 ] * sine;
02023 array[ 7 ] = - array[ 3 ] * sine + array[ 7 ] * cosine;
02024 array[ 3 ] = tmp;
02025
02026 #endif
02027 }
02028
02029
02030
02031 template< size_t M, size_t N, typename float_t >
02032 void
02033 matrix< M, N, float_t >::
02034 preRotateX( const float_t angle )
02035 {
02036
02037
02038 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02039
02040
02041
02042 const float_t sine = details::getSine( angle );
02043 const float_t cosine = details::getCosine( angle );
02044
02045 float_t tmp;
02046
02047 tmp = array[ 0 ];
02048 array[ 0 ] = array[ 0 ] * cosine - array[ 2 ] * sine;
02049 array[ 2 ] = tmp * sine + array[ 2 ] * cosine;
02050
02051 tmp = array[ 4 ];
02052 array[ 4 ] = array[ 4 ] * cosine - array[ 6 ] * sine;
02053 array[ 6 ] = tmp * sine + array[ 6 ] * cosine;
02054
02055 tmp = array[ 8 ];
02056 array[ 8 ] = array[ 8 ] * cosine - array[ 10 ] * sine;
02057 array[ 10 ] = tmp * sine + array[ 10 ] * cosine;
02058
02059 tmp = array[ 12 ];
02060 array[ 12 ] = array[ 12 ] * cosine - array[ 14 ] * sine;
02061 array[ 14 ] = tmp * sine + array[ 14 ] * cosine;
02062
02063 }
02064
02065
02066
02067 template< size_t M, size_t N, typename float_t >
02068 void
02069 matrix< M, N, float_t >::
02070 preRotateY( const float_t angle )
02071 {
02072
02073
02074 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02075
02076
02077
02078 const float_t sine = details::getSine( angle );
02079 const float_t cosine = details::getCosine( angle );
02080
02081 float_t tmp;
02082
02083 tmp = array[ 1 ];
02084 array[ 1 ] = array[ 1 ] * cosine + array[ 2 ] * sine;
02085 array[ 2 ] = tmp * -sine + array[ 2 ] * cosine;
02086
02087 tmp = array[ 5 ];
02088 array[ 5 ] = array[ 5 ] * cosine + array[ 6 ] * sine;
02089 array[ 6 ] = tmp * -sine + array[ 6 ] * cosine;
02090
02091 tmp = array[ 9 ];
02092 array[ 9 ] = array[ 9 ] * cosine + array[ 10 ] * sine;
02093 array[ 10 ] = tmp * -sine + array[ 10 ] * cosine;
02094
02095 tmp = array[ 13 ];
02096 array[ 13 ] = array[ 13 ] * cosine + array[ 14 ] * sine;
02097 array[ 14 ] = tmp * -sine + array[ 14 ] * cosine;
02098
02099 }
02100
02101
02102
02103 template< size_t M, size_t N, typename float_t >
02104 void
02105 matrix< M, N, float_t >::
02106 preRotateZ( const float_t angle )
02107 {
02108
02109
02110 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02111
02112
02113
02114 const float_t sine = details::getSine( angle );
02115 const float_t cosine = details::getCosine( angle );
02116
02117 float_t tmp;
02118
02119 tmp = array[ 0 ];
02120 array[ 0 ] = array[ 0 ] * cosine + array[ 1 ] * sine;
02121 array[ 1 ] = tmp * -sine + array[ 1 ] * cosine;
02122
02123 tmp = array[ 4 ];
02124 array[ 4 ] = array[ 4 ] * cosine + array[ 5 ] * sine;
02125 array[ 5 ] = tmp * -sine + array[ 5 ] * cosine;
02126
02127 tmp = array[ 8 ];
02128 array[ 8 ] = array[ 8 ] * cosine + array[ 9 ] * sine;
02129 array[ 9 ] = tmp * -sine + array[ 9 ] * cosine;
02130
02131 tmp = array[ 12 ];
02132 array[ 12 ] = array[ 12 ] * cosine + array[ 13 ] * sine;
02133 array[ 13 ] = tmp * -sine + array[ 13 ] * cosine;
02134
02135 }
02136
02137
02138
02139 template< size_t M, size_t N, typename float_t >
02140 void
02141 matrix< M, N, float_t >::
02142 scale( const float_t scale[3] )
02143 {
02144
02145
02146 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02147
02148 array[0] *= scale[0];
02149 array[1] *= scale[0];
02150 array[2] *= scale[0];
02151 array[3] *= scale[0];
02152 array[4] *= scale[1];
02153 array[5] *= scale[1];
02154 array[6] *= scale[1];
02155 array[7] *= scale[1];
02156 array[8] *= scale[2];
02157 array[9] *= scale[2];
02158 array[10] *= scale[2];
02159 array[11] *= scale[2];
02160
02161 }
02162
02163
02164
02165 template< size_t M, size_t N, typename float_t >
02166 void
02167 matrix< M, N, float_t >::
02168 scale( const float_t x, const float_t y, const float_t z )
02169 {
02170
02171
02172 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02173
02174 array[0] *= x;
02175 array[1] *= x;
02176 array[2] *= x;
02177 array[3] *= x;
02178 array[4] *= y;
02179 array[5] *= y;
02180 array[6] *= y;
02181 array[7] *= y;
02182 array[8] *= z;
02183 array[9] *= z;
02184 array[10] *= z;
02185 array[11] *= z;
02186
02187 }
02188
02189
02190
02191 template< size_t M, size_t N, typename float_t >
02192 inline void
02193 matrix< M, N, float_t >::
02194 scale( const vector< 3, float_t >& scale_ )
02195 {
02196 scale( scale_.array );
02197 }
02198
02199
02200
02201 template< size_t M, size_t N, typename float_t >
02202 void
02203 matrix< M, N, float_t >::
02204 scaleTranslation( const float_t scale_[3] )
02205 {
02206
02207
02208 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02209
02210 array[12] *= scale_[0];
02211 array[13] *= scale_[1];
02212 array[14] *= scale_[2];
02213
02214 }
02215
02216
02217
02218 template< size_t M, size_t N, typename float_t >
02219 inline void
02220 matrix< M, N, float_t >::
02221 scaleTranslation( const vector< 3, float_t >& scale_ )
02222 {
02223 scaleTranslation( scale_.array );
02224 }
02225
02226
02227
02228 template< size_t M, size_t N, typename float_t >
02229 void
02230 matrix< M, N, float_t >::
02231 setTranslation( const float_t x, const float_t y, const float_t z )
02232 {
02233
02234
02235 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02236
02237 array[12] = x;
02238 array[13] = y;
02239 array[14] = z;
02240
02241 }
02242
02243
02244
02245 template< size_t M, size_t N, typename float_t >
02246 void
02247 matrix< M, N, float_t >::
02248 setTranslation( const float_t trans[3] )
02249 {
02250
02251
02252 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02253
02254 array[12] = trans[0];
02255 array[13] = trans[1];
02256 array[14] = trans[2];
02257
02258 }
02259
02260
02261
02262 template< size_t M, size_t N, typename float_t >
02263 inline void
02264 matrix< M, N, float_t >::
02265 setTranslation( const vector< 3, float_t >& trans )
02266 {
02267 setTranslation( trans.array );
02268 }
02269
02270
02271
02272 template< size_t M, size_t N, typename float_t >
02273 vector< 3, float_t >
02274 matrix< M, N, float_t >::
02275 getTranslation() const
02276 {
02277
02278
02279 details::matrix_is_4x4< M, N, matrix< M, N, float_t > >();
02280
02281 vector< 3, float_t > translation;
02282
02283 translation.array[ 0 ] = array[ 12 ];
02284 translation.array[ 1 ] = array[ 13 ];
02285 translation.array[ 2 ] = array[ 14 ];
02286
02287 return translation;
02288 }
02289
02290
02291
02292
02293
02294 template< size_t M, size_t N, typename float_t >
02295 bool
02296 matrix< M, N, float_t >::
02297 getString( std::string& result, const std::string& delimiter ) const
02298 {
02299 std::string tmp;
02300 bool ok = true;
02301 for( size_t row = 0; row < M; ++row )
02302 {
02303 for( size_t col = 0; ok && col < N; ++col )
02304 {
02305 ok = stringUtil::toString< float_t >( at( row, col ), tmp );
02306 result += tmp;
02307 result += delimiter;
02308 }
02309 }
02310 return ok;
02311 }
02312
02313
02314
02315 template< size_t M, size_t N, typename float_t >
02316 size_t
02317 matrix< M, N, float_t >::
02318 size() const
02319 {
02320 return M * N;
02321 }
02322
02323
02324
02325 template< size_t M, size_t N, typename float_t >
02326 typename matrix< M, N, float_t >::iterator
02327 matrix< M, N, float_t >::
02328 begin()
02329 {
02330 return array;
02331 }
02332
02333
02334
02335 template< size_t M, size_t N, typename float_t >
02336 typename matrix< M, N, float_t >::iterator
02337 matrix< M, N, float_t >::
02338 end()
02339 {
02340 return array + size();
02341 }
02342
02343
02344
02345 template< size_t M, size_t N, typename float_t >
02346 typename matrix< M, N, float_t >::const_iterator
02347 matrix< M, N, float_t >::
02348 begin() const
02349 {
02350 return array;
02351 }
02352
02353
02354
02355 template< size_t M, size_t N, typename float_t >
02356 typename matrix< M, N, float_t >::const_iterator
02357 matrix< M, N, float_t >::
02358 end() const
02359 {
02360 return array + size();
02361 }
02362
02363
02364
02365 template< size_t M, size_t N, typename float_t >
02366 template< typename init_functor_t >
02367 const matrix< M, N, float_t >
02368 matrix< M, N, float_t >::getInitializedMatrix()
02369 {
02370 init_functor_t functor;
02371 matrix< M, N, float_t > matrix_;
02372 functor( matrix_ );
02373 return matrix_;
02374 }
02375
02376
02377 }
02378
02379 #endif
02380