- Home
- About DUNE
- Download
- Documentation
- Community
- Development
00001 #ifndef DUNE_GENERICGEOMETRY_MATRIXHELPER_HH 00002 #define DUNE_GENERICGEOMETRY_MATRIXHELPER_HH 00003 00004 #include <dune/common/fvector.hh> 00005 #include <dune/common/fmatrix.hh> 00006 #include <dune/common/static_assert.hh> 00007 00008 namespace Dune 00009 { 00010 00011 namespace GenericGeometry 00012 { 00013 00014 template< class Traits > 00015 struct MatrixHelper 00016 { 00017 typedef typename Traits :: ctype FieldType; 00018 00019 template< int m, int n > 00020 static void 00021 Ax ( const typename Traits :: template Matrix< m, n > :: type &A, 00022 const typename Traits :: template Vector< n > :: type &x, 00023 typename Traits :: template Vector< m > :: type &ret ) 00024 { 00025 for( int i = 0; i < m; ++i ) 00026 { 00027 ret[ i ] = FieldType( 0 ); 00028 for( int j = 0; j < n; ++j ) 00029 ret[ i ] += A[ i ][ j ] * x[ j ]; 00030 } 00031 } 00032 00033 template< int m, int n > 00034 static void 00035 ATx ( const typename Traits :: template Matrix< m, n > :: type &A, 00036 const typename Traits :: template Vector< m > :: type &x, 00037 typename Traits :: template Vector< n > :: type &ret ) 00038 { 00039 for( int i = 0; i < n; ++i ) 00040 { 00041 ret[ i ] = FieldType( 0 ); 00042 for( int j = 0; j < m; ++j ) 00043 ret[ i ] += A[ j ][ i ] * x[ j ]; 00044 } 00045 } 00046 00047 template< int m, int n, int p > 00048 static void 00049 AB ( const typename Traits :: template Matrix< m, n > :: type &A, 00050 const typename Traits :: template Matrix< n, p > :: type &B, 00051 typename Traits :: template Matrix< m, p > :: type &ret ) 00052 { 00053 for( int i = 0; i < m; ++i ) 00054 { 00055 for( int j = 0; j < p; ++j ) 00056 { 00057 ret[ i ][ j ] = FieldType( 0 ); 00058 for( int k = 0; k < n; ++k ) 00059 ret[ i ][ j ] += A[ i ][ k ] * B[ k ][ j ]; 00060 } 00061 } 00062 } 00063 00064 template< int m, int n, int p > 00065 static void 00066 ATBT ( const typename Traits :: template Matrix< m, n > :: type &A, 00067 const typename Traits :: template Matrix< p, m > :: type &B, 00068 typename Traits :: template Matrix< n, p > :: type &ret ) 00069 { 00070 for( int i = 0; i < n; ++i ) 00071 { 00072 for( int j = 0; j < p; ++j ) 00073 { 00074 ret[ i ][ j ] = FieldType( 0 ); 00075 for( int k = 0; k < m; ++k ) 00076 ret[ i ][ j ] += A[ k ][ i ] * B[ j ][ k ]; 00077 } 00078 } 00079 } 00080 00081 template< int m, int n > 00082 static void 00083 ATA_L ( const typename Traits :: template Matrix< m, n > :: type &A, 00084 typename Traits :: template Matrix< n, n > :: type &ret ) 00085 { 00086 for( int i = 0; i < n; ++i ) 00087 { 00088 for( int j = 0; j <= i; ++j ) 00089 { 00090 ret[ i ][ j ] = FieldType( 0 ); 00091 for( int k = 0; k < m; ++k ) 00092 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ]; 00093 } 00094 } 00095 } 00096 00097 template< int m, int n > 00098 static void 00099 ATA ( const typename Traits :: template Matrix< m, n > :: type &A, 00100 typename Traits :: template Matrix< n, n > :: type &ret ) 00101 { 00102 for( int i = 0; i < n; ++i ) 00103 { 00104 for( int j = 0; j <= i; ++j ) 00105 { 00106 ret[ i ][ j ] = FieldType( 0 ); 00107 for( int k = 0; k < m; ++k ) 00108 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ]; 00109 ret[ j ][ i ] = ret[ i ][ j ]; 00110 } 00111 00112 ret[ i ][ i ] = FieldType( 0 ); 00113 for( int k = 0; k < m; ++k ) 00114 ret[ i ][ i ] += A[ k ][ i ] * A[ k ][ i ]; 00115 } 00116 } 00117 00118 template< int m, int n > 00119 static void 00120 AAT_L ( const typename Traits :: template Matrix< m, n > :: type &A, 00121 typename Traits :: template Matrix< m, m > :: type &ret ) 00122 { 00123 /* 00124 if (m==2) { 00125 ret[0][0] = A[0]*A[0]; 00126 ret[1][1] = A[1]*A[1]; 00127 ret[1][0] = A[0]*A[1]; 00128 } 00129 else 00130 */ 00131 for( int i = 0; i < m; ++i ) 00132 { 00133 for( int j = 0; j <= i; ++j ) 00134 { 00135 FieldType &retij = ret[ i ][ j ]; 00136 retij = A[ i ][ 0 ] * A[ j ][ 0 ]; 00137 for( int k = 1; k < n; ++k ) 00138 retij += A[ i ][ k ] * A[ j ][ k ]; 00139 } 00140 } 00141 } 00142 00143 template< int m, int n > 00144 static void 00145 AAT ( const typename Traits :: template Matrix< m, n > :: type &A, 00146 typename Traits :: template Matrix< m, m > :: type &ret ) 00147 { 00148 for( int i = 0; i < m; ++i ) 00149 { 00150 for( int j = 0; j < i; ++j ) 00151 { 00152 ret[ i ][ j ] = FieldType( 0 ); 00153 for( int k = 0; k < n; ++k ) 00154 ret[ i ][ j ] += A[ i ][ k ] * A[ j ][ k ]; 00155 ret[ j ][ i ] = ret[ i ][ j ]; 00156 } 00157 ret[ i ][ i ] = FieldType( 0 ); 00158 for( int k = 0; k < n; ++k ) 00159 ret[ i ][ i ] += A[ i ][ k ] * A[ i ][ k ]; 00160 } 00161 } 00162 00163 template< int n > 00164 static void 00165 Lx ( const typename Traits :: template Matrix< n, n > :: type &L, 00166 const typename Traits :: template Vector< n > :: type &x, 00167 typename Traits :: template Vector< n > :: type &ret ) 00168 { 00169 for( int i = 0; i < n; ++i ) 00170 { 00171 ret[ i ] = FieldType( 0 ); 00172 for( int j = 0; j <= i; ++j ) 00173 ret[ i ] += L[ i ][ j ] * x[ j ]; 00174 } 00175 } 00176 00177 template< int n > 00178 static void 00179 LTx ( const typename Traits :: template Matrix< n, n > :: type &L, 00180 const typename Traits :: template Vector< n > :: type &x, 00181 typename Traits :: template Vector< n > :: type &ret ) 00182 { 00183 for( int i = 0; i < n; ++i ) 00184 { 00185 ret[ i ] = FieldType( 0 ); 00186 for( int j = i; j < n; ++j ) 00187 ret[ i ] += L[ j ][ i ] * x[ j ]; 00188 } 00189 } 00190 00191 template< int n > 00192 static void 00193 LTL ( const typename Traits :: template Matrix< n, n > :: type &L, 00194 typename Traits :: template Matrix< n, n > :: type &ret ) 00195 { 00196 for( int i = 0; i < n; ++i ) 00197 { 00198 for( int j = 0; j < i; ++j ) 00199 { 00200 ret[ i ][ j ] = FieldType( 0 ); 00201 for( int k = i; k < n; ++k ) 00202 ret[ i ][ j ] += L[ k ][ i ] * L[ k ][ j ]; 00203 ret[ j ][ i ] = ret[ i ][ j ]; 00204 } 00205 ret[ i ][ i ] = FieldType( 0 ); 00206 for( int k = i; k < n; ++k ) 00207 ret[ i ][ i ] += L[ k ][ i ] * L[ k ][ i ]; 00208 } 00209 } 00210 00211 template< int n > 00212 static void 00213 LLT ( const typename Traits :: template Matrix< n, n > :: type &L, 00214 typename Traits :: template Matrix< n, n > :: type &ret ) 00215 { 00216 for( int i = 0; i < n; ++i ) 00217 { 00218 for( int j = 0; j < i; ++j ) 00219 { 00220 ret[ i ][ j ] = FieldType( 0 ); 00221 for( int k = 0; k <= j; ++k ) 00222 ret[ i ][ j ] += L[ i ][ k ] * L[ j ][ k ]; 00223 ret[ j ][ i ] = ret[ i ][ j ]; 00224 } 00225 ret[ i ][ i ] = FieldType( 0 ); 00226 for( int k = 0; k <= i; ++k ) 00227 ret[ i ][ i ] += L[ i ][ k ] * L[ i ][ k ]; 00228 } 00229 } 00230 00231 template< int n > 00232 static void 00233 cholesky_L ( const typename Traits :: template Matrix< n, n > :: type &A, 00234 typename Traits :: template Matrix< n, n > :: type &ret ) 00235 { 00236 for( int i = 0; i < n; ++i ) 00237 { 00238 FieldType &rii = ret[ i ][ i ]; 00239 00240 FieldType x = A[ i ][ i ]; 00241 for( int j = 0; j < i; ++j ) 00242 x -= ret[ i ][ j ] * ret[ i ][ j ]; 00243 assert( x > FieldType( 0 ) ); 00244 rii = sqrt( x ); 00245 00246 FieldType invrii = FieldType( 1 ) / rii; 00247 for( int k = i+1; k < n; ++k ) 00248 { 00249 FieldType x = A[ k ][ i ]; 00250 for( int j = 0; j < i; ++j ) 00251 x -= ret[ i ][ j ] * ret[ k ][ j ]; 00252 ret[ k ][ i ] = invrii * x; 00253 } 00254 } 00255 } 00256 00257 template< int n > 00258 static FieldType 00259 detL ( const typename Traits :: template Matrix< n, n > :: type &L ) 00260 { 00261 FieldType det = FieldType( 1 ); 00262 for( int i = 0; i < n; ++i ) 00263 det *= L[ i ][ i ]; 00264 return det; 00265 } 00266 00267 template< int n > 00268 static FieldType 00269 invL ( typename Traits :: template Matrix< n, n > :: type &L ) 00270 { 00271 FieldType det = FieldType( 1 ); 00272 for( int i = 0; i < n; ++i ) 00273 { 00274 FieldType &lii = L[ i ][ i ]; 00275 det *= lii; 00276 lii = FieldType( 1 ) / lii; 00277 for( int j = 0; j < i; ++j ) 00278 { 00279 FieldType &lij = L[ i ][ j ]; 00280 FieldType x = lij * L[ j ][ j ]; 00281 for( int k = j+1; k < i; ++k ) 00282 x += L[ i ][ k ] * L[ k ][ j ]; 00283 lij = (-lii) * x; 00284 } 00285 } 00286 return det; 00287 } 00288 00289 // calculates x := L^{-1} x 00290 template< int n > 00291 static void 00292 invLx ( typename Traits :: template Matrix< n, n > :: type &L, 00293 typename Traits :: template Vector< n > :: type &x ) 00294 { 00295 for( int i = 0; i < n; ++i ) 00296 { 00297 for( int j = 0; j < i; ++j ) 00298 x[ i ] -= L[ i ][ j ] * x[ j ]; 00299 x[ i ] /= L[ i ][ i ]; 00300 } 00301 } 00302 00303 // calculates x := L^{-T} x 00304 template< int n > 00305 static void 00306 invLTx ( typename Traits :: template Matrix< n, n > :: type &L, 00307 typename Traits :: template Vector< n > :: type &x ) 00308 { 00309 for( int i = n; i > 0; --i ) 00310 { 00311 for( int j = i; j < n; ++j ) 00312 x[ i-1 ] -= L[ j ][ i-1 ] * x[ j ]; 00313 x[ i-1 ] /= L[ i-1 ][ i-1 ]; 00314 } 00315 } 00316 00317 template< int n > 00318 static FieldType 00319 spdDetA ( const typename Traits :: template Matrix< n, n > :: type &A ) 00320 { 00321 // return A[0][0]*A[1][1]-A[1][0]*A[1][0]; 00322 typename Traits :: template Matrix< n, n > :: type L; 00323 cholesky_L< n >( A, L ); 00324 return detL< n >( L ); 00325 } 00326 00327 template< int n > 00328 static FieldType 00329 spdInvA ( typename Traits :: template Matrix< n, n > :: type &A ) 00330 { 00331 typename Traits :: template Matrix< n, n > :: type L; 00332 cholesky_L< n >( A, L ); 00333 const FieldType det = invL< n >( L ); 00334 LTL< n >( L, A ); 00335 return det; 00336 } 00337 00338 // calculate x := A^{-1} x 00339 template< int n > 00340 static void 00341 spdInvAx ( typename Traits :: template Matrix< n, n > :: type &A, 00342 typename Traits :: template Vector< n > :: type &x ) 00343 { 00344 typename Traits :: template Matrix< n, n > :: type L; 00345 cholesky_L< n >( A, L ); 00346 invLx< n >( L, x ); 00347 invLTx< n >( L, x ); 00348 } 00349 00350 template< int m, int n > 00351 static FieldType 00352 detATA ( const typename Traits :: template Matrix< m, n > :: type &A ) 00353 { 00354 if( m >= n ) 00355 { 00356 typename Traits :: template Matrix< n, n > :: type ata; 00357 ATA_L< m, n >( A, ata ); 00358 return spdDetA< n >( ata ); 00359 } 00360 else 00361 return FieldType( 0 ); 00362 } 00363 00369 template< int m, int n > 00370 static FieldType 00371 sqrtDetAAT ( const typename Traits::template Matrix< m, n >::type &A ) 00372 { 00373 if( (n == 2) && (m == 2) ) 00374 { 00375 // Special implementation for 2x2 matrices: faster and more stable 00376 return std::abs( A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ] ); 00377 } 00378 else if( (n == 3) && (m == 3) ) 00379 { 00380 // Special implementation for 3x3 matrices 00381 const FieldType v0 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 1 ][ 1 ] * A[ 0 ][ 2 ]; 00382 const FieldType v1 = A[ 0 ][ 2 ] * A[ 1 ][ 0 ] - A[ 1 ][ 2 ] * A[ 0 ][ 0 ]; 00383 const FieldType v2 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 1 ][ 0 ] * A[ 0 ][ 1 ]; 00384 return std::abs( v0 * A[ 2 ][ 0 ] + v1 * A[ 2 ][ 1 ] + v2 * A[ 2 ][ 2 ] ); 00385 } 00386 else if( n >= m ) 00387 { 00388 // General case 00389 typename Traits::template Matrix< m, m >::type aat; 00390 AAT_L< m, n >( A, aat ); 00391 return spdDetA< m >( aat ); 00392 } 00393 else 00394 return FieldType( 0 ); 00395 } 00396 00397 // A^{-1}_L = (A^T A)^{-1} A^T 00398 // => A^{-1}_L A = I 00399 template< int m, int n > 00400 static FieldType 00401 leftInvA ( const typename Traits :: template Matrix< m, n > :: type &A, 00402 typename Traits :: template Matrix< n, m > :: type &ret ) 00403 { 00404 dune_static_assert( (m >= n), "Matrix has no left inverse." ); 00405 typename Traits :: template Matrix< n, n > :: type ata; 00406 ATA_L< m, n >( A, ata ); 00407 const FieldType det = spdInvA< n >( ata ); 00408 ATBT< n, n, m >( ata, A, ret ); 00409 return det; 00410 } 00411 00412 template< int m, int n > 00413 static void 00414 leftInvAx ( const typename Traits :: template Matrix< m, n > :: type &A, 00415 const typename Traits :: template Vector< m > :: type &x, 00416 typename Traits :: template Vector< n > :: type &y ) 00417 { 00418 dune_static_assert( (m >= n), "Matrix has no left inverse." ); 00419 typename Traits :: template Matrix< n, n > :: type ata; 00420 ATx< m, n >( A, x, y ); 00421 ATA_L< m, n >( A, ata ); 00422 spdInvAx< n >( ata, y ); 00423 } 00424 00425 // A^{-1}_R = A^T (A A^T)^{-1} 00426 // => A A^{-1}_R = I 00427 template< int m, int n > 00428 static FieldType 00429 rightInvA ( const typename Traits :: template Matrix< m, n > :: type &A, 00430 typename Traits :: template Matrix< n, m > :: type &ret ) 00431 { 00432 dune_static_assert( (n >= m), "Matrix has no right inverse." ); 00433 if( (n == 2) && (m == 2) ) 00434 { 00435 const FieldType det = (A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ]); 00436 const FieldType detInv = FieldType( 1 ) / det; 00437 ret[ 0 ][ 0 ] = A[ 1 ][ 1 ] * detInv; 00438 ret[ 1 ][ 1 ] = A[ 0 ][ 0 ] * detInv; 00439 ret[ 1 ][ 0 ] = -A[ 1 ][ 0 ] * detInv; 00440 ret[ 0 ][ 1 ] = -A[ 0 ][ 1 ] * detInv; 00441 return std::abs( det ); 00442 } 00443 else 00444 { 00445 typename Traits :: template Matrix< m , m > :: type aat; 00446 AAT_L< m, n >( A, aat ); 00447 const FieldType det = spdInvA< m >( aat ); 00448 ATBT< m, n, m >( A , aat , ret ); 00449 return det; 00450 } 00451 } 00452 00453 template< int m, int n > 00454 static void 00455 xTRightInvA ( const typename Traits :: template Matrix< m, n > :: type &A, 00456 const typename Traits :: template Vector< n > :: type &x, 00457 typename Traits :: template Vector< m > :: type &y ) 00458 { 00459 dune_static_assert( (n >= m), "Matrix has no right inverse." ); 00460 typename Traits :: template Matrix< m, m > :: type aat; 00461 Ax< m, n >( A, x, y ); 00462 AAT_L< m, n >( A, aat ); 00463 spdInvAx< m >( aat, y ); 00464 } 00465 }; 00466 00467 } 00468 00469 } 00470 00471 #endif // #ifndef DUNE_GENERICGEOMETRY_MATRIXHELPER_HH
Generated on Fri Apr 29 2011 with Doxygen (ver 1.7.1) [doxygen-log,error-log].