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