dune-geometry  2.2.0
matrixhelper.hh
Go to the documentation of this file.
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