Dune Core Modules (2.4.1)

matrixhelper.hh
1// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
2// vi: set et ts=4 sw=2 sts=2:
3#ifndef DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
4#define DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
5
6#include <cmath>
7
10
11namespace Dune
12{
13
14 namespace GenericGeometry
15 {
16
17 // FieldHelper
18 // -----------
19
20 template< class Field >
21 struct FieldHelper
22 {
23 static Field abs ( const Field &x ) { return std::abs( x ); }
24 };
25
26
27
28 // MatrixHelper
29 // ------------
30
31 template< class Traits >
32 struct MatrixHelper
33 {
34 typedef typename Traits::ctype FieldType;
35
36 static FieldType abs ( const FieldType &x )
37 {
38 //return std::abs( x );
39 return FieldHelper< FieldType >::abs( x );
40 }
41
42 template< int m, int n >
43 static void
44 Ax ( const typename Traits :: template Matrix< m, n > :: type &A,
45 const typename Traits :: template Vector< n > :: type &x,
46 typename Traits :: template Vector< m > :: type &ret )
47 {
48 for( int i = 0; i < m; ++i )
49 {
50 ret[ i ] = FieldType( 0 );
51 for( int j = 0; j < n; ++j )
52 ret[ i ] += A[ i ][ j ] * x[ j ];
53 }
54 }
55
56 template< int m, int n >
57 static void
58 ATx ( const typename Traits :: template Matrix< m, n > :: type &A,
59 const typename Traits :: template Vector< m > :: type &x,
60 typename Traits :: template Vector< n > :: type &ret )
61 {
62 for( int i = 0; i < n; ++i )
63 {
64 ret[ i ] = FieldType( 0 );
65 for( int j = 0; j < m; ++j )
66 ret[ i ] += A[ j ][ i ] * x[ j ];
67 }
68 }
69
70 template< int m, int n, int p >
71 static void
72 AB ( const typename Traits :: template Matrix< m, n > :: type &A,
73 const typename Traits :: template Matrix< n, p > :: type &B,
74 typename Traits :: template Matrix< m, p > :: type &ret )
75 {
76 for( int i = 0; i < m; ++i )
77 {
78 for( int j = 0; j < p; ++j )
79 {
80 ret[ i ][ j ] = FieldType( 0 );
81 for( int k = 0; k < n; ++k )
82 ret[ i ][ j ] += A[ i ][ k ] * B[ k ][ j ];
83 }
84 }
85 }
86
87 template< int m, int n, int p >
88 static void
89 ATBT ( const typename Traits :: template Matrix< m, n > :: type &A,
90 const typename Traits :: template Matrix< p, m > :: type &B,
91 typename Traits :: template Matrix< n, p > :: type &ret )
92 {
93 for( int i = 0; i < n; ++i )
94 {
95 for( int j = 0; j < p; ++j )
96 {
97 ret[ i ][ j ] = FieldType( 0 );
98 for( int k = 0; k < m; ++k )
99 ret[ i ][ j ] += A[ k ][ i ] * B[ j ][ k ];
100 }
101 }
102 }
103
104 template< int m, int n >
105 static void
106 ATA_L ( const typename Traits :: template Matrix< m, n > :: type &A,
107 typename Traits :: template Matrix< n, n > :: type &ret )
108 {
109 for( int i = 0; i < n; ++i )
110 {
111 for( int j = 0; j <= i; ++j )
112 {
113 ret[ i ][ j ] = FieldType( 0 );
114 for( int k = 0; k < m; ++k )
115 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
116 }
117 }
118 }
119
120 template< int m, int n >
121 static void
122 ATA ( const typename Traits :: template Matrix< m, n > :: type &A,
123 typename Traits :: template Matrix< n, n > :: type &ret )
124 {
125 for( int i = 0; i < n; ++i )
126 {
127 for( int j = 0; j <= i; ++j )
128 {
129 ret[ i ][ j ] = FieldType( 0 );
130 for( int k = 0; k < m; ++k )
131 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
132 ret[ j ][ i ] = ret[ i ][ j ];
133 }
134
135 ret[ i ][ i ] = FieldType( 0 );
136 for( int k = 0; k < m; ++k )
137 ret[ i ][ i ] += A[ k ][ i ] * A[ k ][ i ];
138 }
139 }
140
141 template< int m, int n >
142 static void
143 AAT_L ( const typename Traits :: template Matrix< m, n > :: type &A,
144 typename Traits :: template Matrix< m, m > :: type &ret )
145 {
146 /*
147 if (m==2) {
148 ret[0][0] = A[0]*A[0];
149 ret[1][1] = A[1]*A[1];
150 ret[1][0] = A[0]*A[1];
151 }
152 else
153 */
154 for( int i = 0; i < m; ++i )
155 {
156 for( int j = 0; j <= i; ++j )
157 {
158 FieldType &retij = ret[ i ][ j ];
159 retij = A[ i ][ 0 ] * A[ j ][ 0 ];
160 for( int k = 1; k < n; ++k )
161 retij += A[ i ][ k ] * A[ j ][ k ];
162 }
163 }
164 }
165
166 template< int m, int n >
167 static void
168 AAT ( const typename Traits :: template Matrix< m, n > :: type &A,
169 typename Traits :: template Matrix< m, m > :: type &ret )
170 {
171 for( int i = 0; i < m; ++i )
172 {
173 for( int j = 0; j < i; ++j )
174 {
175 ret[ i ][ j ] = FieldType( 0 );
176 for( int k = 0; k < n; ++k )
177 ret[ i ][ j ] += A[ i ][ k ] * A[ j ][ k ];
178 ret[ j ][ i ] = ret[ i ][ j ];
179 }
180 ret[ i ][ i ] = FieldType( 0 );
181 for( int k = 0; k < n; ++k )
182 ret[ i ][ i ] += A[ i ][ k ] * A[ i ][ k ];
183 }
184 }
185
186 template< int n >
187 static void
188 Lx ( const typename Traits :: template Matrix< n, n > :: type &L,
189 const typename Traits :: template Vector< n > :: type &x,
190 typename Traits :: template Vector< n > :: type &ret )
191 {
192 for( int i = 0; i < n; ++i )
193 {
194 ret[ i ] = FieldType( 0 );
195 for( int j = 0; j <= i; ++j )
196 ret[ i ] += L[ i ][ j ] * x[ j ];
197 }
198 }
199
200 template< int n >
201 static void
202 LTx ( const typename Traits :: template Matrix< n, n > :: type &L,
203 const typename Traits :: template Vector< n > :: type &x,
204 typename Traits :: template Vector< n > :: type &ret )
205 {
206 for( int i = 0; i < n; ++i )
207 {
208 ret[ i ] = FieldType( 0 );
209 for( int j = i; j < n; ++j )
210 ret[ i ] += L[ j ][ i ] * x[ j ];
211 }
212 }
213
214 template< int n >
215 static void
216 LTL ( const typename Traits :: template Matrix< n, n > :: type &L,
217 typename Traits :: template Matrix< n, n > :: type &ret )
218 {
219 for( int i = 0; i < n; ++i )
220 {
221 for( int j = 0; j < i; ++j )
222 {
223 ret[ i ][ j ] = FieldType( 0 );
224 for( int k = i; k < n; ++k )
225 ret[ i ][ j ] += L[ k ][ i ] * L[ k ][ j ];
226 ret[ j ][ i ] = ret[ i ][ j ];
227 }
228 ret[ i ][ i ] = FieldType( 0 );
229 for( int k = i; k < n; ++k )
230 ret[ i ][ i ] += L[ k ][ i ] * L[ k ][ i ];
231 }
232 }
233
234 template< int n >
235 static void
236 LLT ( const typename Traits :: template Matrix< n, n > :: type &L,
237 typename Traits :: template Matrix< n, n > :: type &ret )
238 {
239 for( int i = 0; i < n; ++i )
240 {
241 for( int j = 0; j < i; ++j )
242 {
243 ret[ i ][ j ] = FieldType( 0 );
244 for( int k = 0; k <= j; ++k )
245 ret[ i ][ j ] += L[ i ][ k ] * L[ j ][ k ];
246 ret[ j ][ i ] = ret[ i ][ j ];
247 }
248 ret[ i ][ i ] = FieldType( 0 );
249 for( int k = 0; k <= i; ++k )
250 ret[ i ][ i ] += L[ i ][ k ] * L[ i ][ k ];
251 }
252 }
253
254 template< int n >
255 static void
256 cholesky_L ( const typename Traits :: template Matrix< n, n > :: type &A,
257 typename Traits :: template Matrix< n, n > :: type &ret )
258 {
259 for( int i = 0; i < n; ++i )
260 {
261 FieldType &rii = ret[ i ][ i ];
262
263 FieldType xDiag = A[ i ][ i ];
264 for( int j = 0; j < i; ++j )
265 xDiag -= ret[ i ][ j ] * ret[ i ][ j ];
266 assert( xDiag > FieldType( 0 ) );
267 rii = sqrt( xDiag );
268
269 FieldType invrii = FieldType( 1 ) / rii;
270 for( int k = i+1; k < n; ++k )
271 {
272 FieldType x = A[ k ][ i ];
273 for( int j = 0; j < i; ++j )
274 x -= ret[ i ][ j ] * ret[ k ][ j ];
275 ret[ k ][ i ] = invrii * x;
276 }
277 }
278 }
279
280 template< int n >
281 static FieldType
282 detL ( const typename Traits :: template Matrix< n, n > :: type &L )
283 {
284 FieldType det = FieldType( 1 );
285 for( int i = 0; i < n; ++i )
286 det *= L[ i ][ i ];
287 return det;
288 }
289
290 template< int n >
291 static FieldType
292 invL ( typename Traits :: template Matrix< n, n > :: type &L )
293 {
294 FieldType det = FieldType( 1 );
295 for( int i = 0; i < n; ++i )
296 {
297 FieldType &lii = L[ i ][ i ];
298 det *= lii;
299 lii = FieldType( 1 ) / lii;
300 for( int j = 0; j < i; ++j )
301 {
302 FieldType &lij = L[ i ][ j ];
303 FieldType x = lij * L[ j ][ j ];
304 for( int k = j+1; k < i; ++k )
305 x += L[ i ][ k ] * L[ k ][ j ];
306 lij = (-lii) * x;
307 }
308 }
309 return det;
310 }
311
312 // calculates x := L^{-1} x
313 template< int n >
314 static void
315 invLx ( typename Traits :: template Matrix< n, n > :: type &L,
316 typename Traits :: template Vector< n > :: type &x )
317 {
318 for( int i = 0; i < n; ++i )
319 {
320 for( int j = 0; j < i; ++j )
321 x[ i ] -= L[ i ][ j ] * x[ j ];
322 x[ i ] /= L[ i ][ i ];
323 }
324 }
325
326 // calculates x := L^{-T} x
327 template< int n >
328 static void
329 invLTx ( typename Traits :: template Matrix< n, n > :: type &L,
330 typename Traits :: template Vector< n > :: type &x )
331 {
332 for( int i = n; i > 0; --i )
333 {
334 for( int j = i; j < n; ++j )
335 x[ i-1 ] -= L[ j ][ i-1 ] * x[ j ];
336 x[ i-1 ] /= L[ i-1 ][ i-1 ];
337 }
338 }
339
340 template< int n >
341 static FieldType
342 spdDetA ( const typename Traits :: template Matrix< n, n > :: type &A )
343 {
344 // return A[0][0]*A[1][1]-A[1][0]*A[1][0];
345 typename Traits :: template Matrix< n, n > :: type L;
346 cholesky_L< n >( A, L );
347 return detL< n >( L );
348 }
349
350 template< int n >
351 static FieldType
352 spdInvA ( typename Traits :: template Matrix< n, n > :: type &A )
353 {
354 typename Traits :: template Matrix< n, n > :: type L;
355 cholesky_L< n >( A, L );
356 const FieldType det = invL< n >( L );
357 LTL< n >( L, A );
358 return det;
359 }
360
361 // calculate x := A^{-1} x
362 template< int n >
363 static void
364 spdInvAx ( typename Traits :: template Matrix< n, n > :: type &A,
365 typename Traits :: template Vector< n > :: type &x )
366 {
367 typename Traits :: template Matrix< n, n > :: type L;
368 cholesky_L< n >( A, L );
369 invLx< n >( L, x );
370 invLTx< n >( L, x );
371 }
372
373 template< int m, int n >
374 static FieldType
375 detATA ( const typename Traits :: template Matrix< m, n > :: type &A )
376 {
377 if( m >= n )
378 {
379 typename Traits :: template Matrix< n, n > :: type ata;
380 ATA_L< m, n >( A, ata );
381 return spdDetA< n >( ata );
382 }
383 else
384 return FieldType( 0 );
385 }
386
392 template< int m, int n >
393 static FieldType
394 sqrtDetAAT ( const typename Traits::template Matrix< m, n >::type &A )
395 {
396 // These special cases are here not only for speed reasons:
397 // The general implementation aborts if the matrix is almost singular,
398 // and the special implementation provide a stable way to handle that case.
399 if( (n == 2) && (m == 2) )
400 {
401 // Special implementation for 2x2 matrices: faster and more stable
402 return abs( A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ] );
403 }
404 else if( (n == 3) && (m == 3) )
405 {
406 // Special implementation for 3x3 matrices
407 const FieldType v0 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 1 ][ 1 ] * A[ 0 ][ 2 ];
408 const FieldType v1 = A[ 0 ][ 2 ] * A[ 1 ][ 0 ] - A[ 1 ][ 2 ] * A[ 0 ][ 0 ];
409 const FieldType v2 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 1 ][ 0 ] * A[ 0 ][ 1 ];
410 return abs( v0 * A[ 2 ][ 0 ] + v1 * A[ 2 ][ 1 ] + v2 * A[ 2 ][ 2 ] );
411 }
412 else if ( (n == 3) && (m == 2) )
413 {
414 // Special implementation for 2x3 matrices
415 const FieldType v0 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 0 ][ 1 ] * A[ 1 ][ 0 ];
416 const FieldType v1 = A[ 0 ][ 0 ] * A[ 1 ][ 2 ] - A[ 1 ][ 0 ] * A[ 0 ][ 2 ];
417 const FieldType v2 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 0 ][ 2 ] * A[ 1 ][ 1 ];
418 return sqrt( v0*v0 + v1*v1 + v2*v2);
419 }
420 else if( n >= m )
421 {
422 // General case
423 typename Traits::template Matrix< m, m >::type aat;
424 AAT_L< m, n >( A, aat );
425 return spdDetA< m >( aat );
426 }
427 else
428 return FieldType( 0 );
429 }
430
431 // A^{-1}_L = (A^T A)^{-1} A^T
432 // => A^{-1}_L A = I
433 template< int m, int n >
434 static FieldType
435 leftInvA ( const typename Traits :: template Matrix< m, n > :: type &A,
436 typename Traits :: template Matrix< n, m > :: type &ret )
437 {
438 static_assert((m >= n), "Matrix has no left inverse.");
439 typename Traits :: template Matrix< n, n > :: type ata;
440 ATA_L< m, n >( A, ata );
441 const FieldType det = spdInvA< n >( ata );
442 ATBT< n, n, m >( ata, A, ret );
443 return det;
444 }
445
446 template< int m, int n >
447 static void
448 leftInvAx ( const typename Traits :: template Matrix< m, n > :: type &A,
449 const typename Traits :: template Vector< m > :: type &x,
450 typename Traits :: template Vector< n > :: type &y )
451 {
452 static_assert((m >= n), "Matrix has no left inverse.");
453 typename Traits :: template Matrix< n, n > :: type ata;
454 ATx< m, n >( A, x, y );
455 ATA_L< m, n >( A, ata );
456 spdInvAx< n >( ata, y );
457 }
458
460 template< int m, int n >
461 static FieldType
462 rightInvA ( const typename Traits :: template Matrix< m, n > :: type &A,
463 typename Traits :: template Matrix< n, m > :: type &ret )
464 {
465 static_assert((n >= m), "Matrix has no right inverse.");
466 if( (n == 2) && (m == 2) )
467 {
468 const FieldType det = (A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ]);
469 const FieldType detInv = FieldType( 1 ) / det;
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;
474 return abs( det );
475 }
476 else
477 {
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 );
482 return det;
483 }
484 }
485
486 template< int m, int n >
487 static void
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 )
491 {
492 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 );
497 }
498 };
499
500 } // namespace GenericGeometry
501
502} // namespace Dune
503
504#endif // #ifndef DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
Implements a matrix constructed from a given type representing a field and compile-time given number ...
Implements a vector constructed from a given type representing a field and a compile-time given size.
Dune namespace.
Definition: alignment.hh:10
Creative Commons License   |  Legal Statements / Impressum  |  Hosted by TU Dresden  |  generated with Hugo v0.111.3 (Nov 22, 23:33, 2024)