matrixhelper.hh

Go to the documentation of this file.
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].