1 #ifndef DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
2 #define DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
6 #include <dune/common/fvector.hh>
7 #include <dune/common/fmatrix.hh>
8 #include <dune/common/static_assert.hh>
13 namespace GenericGeometry
19 template<
class Field >
22 static Field
abs (
const Field &x ) {
return std::abs( x ); }
30 template<
class Traits >
41 template<
int m,
int n >
43 Ax (
const typename Traits :: template Matrix< m, n > :: type &A,
44 const typename Traits :: template Vector< n > :: type &x,
45 typename Traits :: template Vector< m > :: type &ret )
47 for(
int i = 0; i < m; ++i )
50 for(
int j = 0; j < n; ++j )
51 ret[ i ] += A[ i ][ j ] * x[ j ];
55 template<
int m,
int n >
57 ATx (
const typename Traits :: template Matrix< m, n > :: type &A,
58 const typename Traits :: template Vector< m > :: type &x,
59 typename Traits :: template Vector< n > :: type &ret )
61 for(
int i = 0; i < n; ++i )
64 for(
int j = 0; j < m; ++j )
65 ret[ i ] += A[ j ][ i ] * x[ j ];
69 template<
int m,
int n,
int p >
71 AB (
const typename Traits :: template Matrix< m, n > :: type &A,
72 const typename Traits :: template Matrix< n, p > :: type &B,
73 typename Traits :: template Matrix< m, p > :: type &ret )
75 for(
int i = 0; i < m; ++i )
77 for(
int j = 0; j < p; ++j )
80 for(
int k = 0; k < n; ++k )
81 ret[ i ][ j ] += A[ i ][ k ] * B[ k ][ j ];
86 template<
int m,
int n,
int p >
88 ATBT (
const typename Traits :: template Matrix< m, n > :: type &A,
89 const typename Traits :: template Matrix< p, m > :: type &B,
90 typename Traits :: template Matrix< n, p > :: type &ret )
92 for(
int i = 0; i < n; ++i )
94 for(
int j = 0; j < p; ++j )
97 for(
int k = 0; k < m; ++k )
98 ret[ i ][ j ] += A[ k ][ i ] * B[ j ][ k ];
103 template<
int m,
int n >
105 ATA_L (
const typename Traits :: template Matrix< m, n > :: type &A,
106 typename Traits :: template Matrix< n, n > :: type &ret )
108 for(
int i = 0; i < n; ++i )
110 for(
int j = 0; j <= i; ++j )
113 for(
int k = 0; k < m; ++k )
114 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
119 template<
int m,
int n >
121 ATA (
const typename Traits :: template Matrix< m, n > :: type &A,
122 typename Traits :: template Matrix< n, n > :: type &ret )
124 for(
int i = 0; i < n; ++i )
126 for(
int j = 0; j <= i; ++j )
129 for(
int k = 0; k < m; ++k )
130 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
131 ret[ j ][ i ] = ret[ i ][ j ];
135 for(
int k = 0; k < m; ++k )
136 ret[ i ][ i ] += A[ k ][ i ] * A[ k ][ i ];
140 template<
int m,
int n >
142 AAT_L (
const typename Traits :: template Matrix< m, n > :: type &A,
143 typename Traits :: template Matrix< m, m > :: type &ret )
153 for(
int i = 0; i < m; ++i )
155 for(
int j = 0; j <= i; ++j )
158 retij = A[ i ][ 0 ] * A[ j ][ 0 ];
159 for(
int k = 1; k < n; ++k )
160 retij += A[ i ][ k ] * A[ j ][ k ];
165 template<
int m,
int n >
167 AAT (
const typename Traits :: template Matrix< m, n > :: type &A,
168 typename Traits :: template Matrix< m, m > :: type &ret )
170 for(
int i = 0; i < m; ++i )
172 for(
int j = 0; j < i; ++j )
175 for(
int k = 0; k < n; ++k )
176 ret[ i ][ j ] += A[ i ][ k ] * A[ j ][ k ];
177 ret[ j ][ i ] = ret[ i ][ j ];
180 for(
int k = 0; k < n; ++k )
181 ret[ i ][ i ] += A[ i ][ k ] * A[ i ][ k ];
187 Lx (
const typename Traits :: template Matrix< n, n > :: type &L,
188 const typename Traits :: template Vector< n > :: type &x,
189 typename Traits :: template Vector< n > :: type &ret )
191 for(
int i = 0; i < n; ++i )
194 for(
int j = 0; j <= i; ++j )
195 ret[ i ] += L[ i ][ j ] * x[ j ];
201 LTx (
const typename Traits :: template Matrix< n, n > :: type &L,
202 const typename Traits :: template Vector< n > :: type &x,
203 typename Traits :: template Vector< n > :: type &ret )
205 for(
int i = 0; i < n; ++i )
208 for(
int j = i; j < n; ++j )
209 ret[ i ] += L[ j ][ i ] * x[ j ];
215 LTL (
const typename Traits :: template Matrix< n, n > :: type &L,
216 typename Traits :: template Matrix< n, n > :: type &ret )
218 for(
int i = 0; i < n; ++i )
220 for(
int j = 0; j < i; ++j )
223 for(
int k = i; k < n; ++k )
224 ret[ i ][ j ] += L[ k ][ i ] * L[ k ][ j ];
225 ret[ j ][ i ] = ret[ i ][ j ];
228 for(
int k = i; k < n; ++k )
229 ret[ i ][ i ] += L[ k ][ i ] * L[ k ][ i ];
235 LLT (
const typename Traits :: template Matrix< n, n > :: type &L,
236 typename Traits :: template Matrix< n, n > :: type &ret )
238 for(
int i = 0; i < n; ++i )
240 for(
int j = 0; j < i; ++j )
243 for(
int k = 0; k <= j; ++k )
244 ret[ i ][ j ] += L[ i ][ k ] * L[ j ][ k ];
245 ret[ j ][ i ] = ret[ i ][ j ];
248 for(
int k = 0; k <= i; ++k )
249 ret[ i ][ i ] += L[ i ][ k ] * L[ i ][ k ];
255 cholesky_L (
const typename Traits :: template Matrix< n, n > :: type &A,
256 typename Traits :: template Matrix< n, n > :: type &ret )
258 for(
int i = 0; i < n; ++i )
263 for(
int j = 0; j < i; ++j )
264 x -= ret[ i ][ j ] * ret[ i ][ j ];
269 for(
int k = i+1; k < n; ++k )
272 for(
int j = 0; j < i; ++j )
273 x -= ret[ i ][ j ] * ret[ k ][ j ];
274 ret[ k ][ i ] = invrii * x;
281 detL (
const typename Traits :: template Matrix< n, n > :: type &L )
284 for(
int i = 0; i < n; ++i )
291 invL (
typename Traits :: template Matrix< n, n > :: type &L )
294 for(
int i = 0; i < n; ++i )
299 for(
int j = 0; j < i; ++j )
303 for(
int k = j+1; k < i; ++k )
304 x += L[ i ][ k ] * L[ k ][ j ];
314 invLx (
typename Traits :: template Matrix< n, n > :: type &L,
315 typename Traits :: template Vector< n > :: type &x )
317 for(
int i = 0; i < n; ++i )
319 for(
int j = 0; j < i; ++j )
320 x[ i ] -= L[ i ][ j ] * x[ j ];
321 x[ i ] /= L[ i ][ i ];
328 invLTx (
typename Traits :: template Matrix< n, n > :: type &L,
329 typename Traits :: template Vector< n > :: type &x )
331 for(
int i = n; i > 0; --i )
333 for(
int j = i; j < n; ++j )
334 x[ i-1 ] -= L[ j ][ i-1 ] * x[ j ];
335 x[ i-1 ] /= L[ i-1 ][ i-1 ];
341 spdDetA (
const typename Traits :: template Matrix< n, n > :: type &A )
344 typename Traits :: template Matrix< n, n > :: type L;
345 cholesky_L< n >( A, L );
346 return detL< n >( L );
351 spdInvA (
typename Traits :: template Matrix< n, n > :: type &A )
353 typename Traits :: template Matrix< n, n > :: type L;
354 cholesky_L< n >( A, L );
363 spdInvAx (
typename Traits :: template Matrix< n, n > :: type &A,
364 typename Traits :: template Vector< n > :: type &x )
366 typename Traits :: template Matrix< n, n > :: type L;
367 cholesky_L< n >( A, L );
372 template<
int m,
int n >
374 detATA (
const typename Traits :: template Matrix< m, n > :: type &A )
378 typename Traits :: template Matrix< n, n > :: type ata;
379 ATA_L< m, n >( A, ata );
380 return spdDetA< n >( ata );
391 template<
int m,
int n >
393 sqrtDetAAT (
const typename Traits::template Matrix< m, n >::type &A )
398 if( (n == 2) && (m == 2) )
401 return abs( A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ] );
403 else if( (n == 3) && (m == 3) )
406 const FieldType v0 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 1 ][ 1 ] * A[ 0 ][ 2 ];
407 const FieldType v1 = A[ 0 ][ 2 ] * A[ 1 ][ 0 ] - A[ 1 ][ 2 ] * A[ 0 ][ 0 ];
408 const FieldType v2 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 1 ][ 0 ] * A[ 0 ][ 1 ];
409 return abs( v0 * A[ 2 ][ 0 ] + v1 * A[ 2 ][ 1 ] + v2 * A[ 2 ][ 2 ] );
411 else if ( (n == 3) && (m == 2) )
414 const FieldType v0 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 0 ][ 1 ] * A[ 1 ][ 0 ];
415 const FieldType v1 = A[ 0 ][ 0 ] * A[ 1 ][ 2 ] - A[ 1 ][ 0 ] * A[ 0 ][ 2 ];
416 const FieldType v2 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 0 ][ 2 ] * A[ 1 ][ 1 ];
417 return sqrt( v0*v0 + v1*v1 + v2*v2);
422 typename Traits::template Matrix< m, m >::type aat;
423 AAT_L< m, n >( A, aat );
424 return spdDetA< m >( aat );
432 template<
int m,
int n >
434 leftInvA (
const typename Traits :: template Matrix< m, n > :: type &A,
435 typename Traits :: template Matrix< n, m > :: type &ret )
437 dune_static_assert( (m >= n),
"Matrix has no left inverse." );
438 typename Traits :: template Matrix< n, n > :: type ata;
439 ATA_L< m, n >( A, ata );
440 const FieldType det = spdInvA< n >( ata );
441 ATBT< n, n, m >( ata, A, ret );
445 template<
int m,
int n >
447 leftInvAx (
const typename Traits :: template Matrix< m, n > :: type &A,
448 const typename Traits :: template Vector< m > :: type &x,
449 typename Traits :: template Vector< n > :: type &y )
451 dune_static_assert( (m >= n),
"Matrix has no left inverse." );
452 typename Traits :: template Matrix< n, n > :: type ata;
453 ATx< m, n >( A, x, y );
454 ATA_L< m, n >( A, ata );
455 spdInvAx< n >( ata, y );
460 template<
int m,
int n >
462 rightInvA (
const typename Traits :: template Matrix< m, n > :: type &A,
463 typename Traits :: template Matrix< n, m > :: type &ret )
465 dune_static_assert( (n >= m),
"Matrix has no right inverse." );
466 if( (n == 2) && (m == 2) )
468 const FieldType det = (A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ]);
470 ret[ 0 ][ 0 ] = A[ 1 ][ 1 ] * detInv;
471 ret[ 1 ][ 1 ] = A[ 0 ][ 0 ] * detInv;
472 ret[ 1 ][ 0 ] = -A[ 1 ][ 0 ] * detInv;
473 ret[ 0 ][ 1 ] = -A[ 0 ][ 1 ] * detInv;
478 typename Traits :: template Matrix< m , m > :: type aat;
479 AAT_L< m, n >( A, aat );
480 const FieldType det = spdInvA< m >( aat );
481 ATBT< m, n, m >( A , aat , ret );
486 template<
int m,
int n >
488 xTRightInvA (
const typename Traits :: template Matrix< m, n > :: type &A,
489 const typename Traits :: template Vector< n > :: type &x,
490 typename Traits :: template Vector< m > :: type &y )
492 dune_static_assert( (n >= m),
"Matrix has no right inverse." );
493 typename Traits :: template Matrix< m, m > :: type aat;
494 Ax< m, n >( A, x, y );
495 AAT_L< m, n >( A, aat );
496 spdInvAx< m >( aat, y );
504 #endif // #ifndef DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH