dune-geometry  2.2.1
matrixhelper.hh
Go to the documentation of this file.
1 #ifndef DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
2 #define DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH
3 
4 #include <cmath>
5 
6 #include <dune/common/fvector.hh>
7 #include <dune/common/fmatrix.hh>
8 #include <dune/common/static_assert.hh>
9 
10 namespace Dune
11 {
12 
13  namespace GenericGeometry
14  {
15 
16  // FieldHelper
17  // -----------
18 
19  template< class Field >
20  struct FieldHelper
21  {
22  static Field abs ( const Field &x ) { return std::abs( x ); }
23  };
24 
25 
26 
27  // MatrixHelper
28  // ------------
29 
30  template< class Traits >
31  struct MatrixHelper
32  {
33  typedef typename Traits::ctype FieldType;
34 
35  static FieldType abs ( const FieldType &x )
36  {
37  //return std::abs( x );
39  }
40 
41  template< int m, int n >
42  static void
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 )
46  {
47  for( int i = 0; i < m; ++i )
48  {
49  ret[ i ] = FieldType( 0 );
50  for( int j = 0; j < n; ++j )
51  ret[ i ] += A[ i ][ j ] * x[ j ];
52  }
53  }
54 
55  template< int m, int n >
56  static void
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 )
60  {
61  for( int i = 0; i < n; ++i )
62  {
63  ret[ i ] = FieldType( 0 );
64  for( int j = 0; j < m; ++j )
65  ret[ i ] += A[ j ][ i ] * x[ j ];
66  }
67  }
68 
69  template< int m, int n, int p >
70  static void
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 )
74  {
75  for( int i = 0; i < m; ++i )
76  {
77  for( int j = 0; j < p; ++j )
78  {
79  ret[ i ][ j ] = FieldType( 0 );
80  for( int k = 0; k < n; ++k )
81  ret[ i ][ j ] += A[ i ][ k ] * B[ k ][ j ];
82  }
83  }
84  }
85 
86  template< int m, int n, int p >
87  static void
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 )
91  {
92  for( int i = 0; i < n; ++i )
93  {
94  for( int j = 0; j < p; ++j )
95  {
96  ret[ i ][ j ] = FieldType( 0 );
97  for( int k = 0; k < m; ++k )
98  ret[ i ][ j ] += A[ k ][ i ] * B[ j ][ k ];
99  }
100  }
101  }
102 
103  template< int m, int n >
104  static void
105  ATA_L ( const typename Traits :: template Matrix< m, n > :: type &A,
106  typename Traits :: template Matrix< n, n > :: type &ret )
107  {
108  for( int i = 0; i < n; ++i )
109  {
110  for( int j = 0; j <= i; ++j )
111  {
112  ret[ i ][ j ] = FieldType( 0 );
113  for( int k = 0; k < m; ++k )
114  ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
115  }
116  }
117  }
118 
119  template< int m, int n >
120  static void
121  ATA ( const typename Traits :: template Matrix< m, n > :: type &A,
122  typename Traits :: template Matrix< n, n > :: type &ret )
123  {
124  for( int i = 0; i < n; ++i )
125  {
126  for( int j = 0; j <= i; ++j )
127  {
128  ret[ i ][ j ] = FieldType( 0 );
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 ];
132  }
133 
134  ret[ i ][ i ] = FieldType( 0 );
135  for( int k = 0; k < m; ++k )
136  ret[ i ][ i ] += A[ k ][ i ] * A[ k ][ i ];
137  }
138  }
139 
140  template< int m, int n >
141  static void
142  AAT_L ( const typename Traits :: template Matrix< m, n > :: type &A,
143  typename Traits :: template Matrix< m, m > :: type &ret )
144  {
145  /*
146  if (m==2) {
147  ret[0][0] = A[0]*A[0];
148  ret[1][1] = A[1]*A[1];
149  ret[1][0] = A[0]*A[1];
150  }
151  else
152  */
153  for( int i = 0; i < m; ++i )
154  {
155  for( int j = 0; j <= i; ++j )
156  {
157  FieldType &retij = ret[ 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 ];
161  }
162  }
163  }
164 
165  template< int m, int n >
166  static void
167  AAT ( const typename Traits :: template Matrix< m, n > :: type &A,
168  typename Traits :: template Matrix< m, m > :: type &ret )
169  {
170  for( int i = 0; i < m; ++i )
171  {
172  for( int j = 0; j < i; ++j )
173  {
174  ret[ i ][ j ] = FieldType( 0 );
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 ];
178  }
179  ret[ i ][ i ] = FieldType( 0 );
180  for( int k = 0; k < n; ++k )
181  ret[ i ][ i ] += A[ i ][ k ] * A[ i ][ k ];
182  }
183  }
184 
185  template< int n >
186  static void
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 )
190  {
191  for( int i = 0; i < n; ++i )
192  {
193  ret[ i ] = FieldType( 0 );
194  for( int j = 0; j <= i; ++j )
195  ret[ i ] += L[ i ][ j ] * x[ j ];
196  }
197  }
198 
199  template< int n >
200  static void
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 )
204  {
205  for( int i = 0; i < n; ++i )
206  {
207  ret[ i ] = FieldType( 0 );
208  for( int j = i; j < n; ++j )
209  ret[ i ] += L[ j ][ i ] * x[ j ];
210  }
211  }
212 
213  template< int n >
214  static void
215  LTL ( const typename Traits :: template Matrix< n, n > :: type &L,
216  typename Traits :: template Matrix< n, n > :: type &ret )
217  {
218  for( int i = 0; i < n; ++i )
219  {
220  for( int j = 0; j < i; ++j )
221  {
222  ret[ i ][ j ] = FieldType( 0 );
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 ];
226  }
227  ret[ i ][ i ] = FieldType( 0 );
228  for( int k = i; k < n; ++k )
229  ret[ i ][ i ] += L[ k ][ i ] * L[ k ][ i ];
230  }
231  }
232 
233  template< int n >
234  static void
235  LLT ( const typename Traits :: template Matrix< n, n > :: type &L,
236  typename Traits :: template Matrix< n, n > :: type &ret )
237  {
238  for( int i = 0; i < n; ++i )
239  {
240  for( int j = 0; j < i; ++j )
241  {
242  ret[ i ][ j ] = FieldType( 0 );
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 ];
246  }
247  ret[ i ][ i ] = FieldType( 0 );
248  for( int k = 0; k <= i; ++k )
249  ret[ i ][ i ] += L[ i ][ k ] * L[ i ][ k ];
250  }
251  }
252 
253  template< int n >
254  static void
255  cholesky_L ( const typename Traits :: template Matrix< n, n > :: type &A,
256  typename Traits :: template Matrix< n, n > :: type &ret )
257  {
258  for( int i = 0; i < n; ++i )
259  {
260  FieldType &rii = ret[ i ][ i ];
261 
262  FieldType x = A[ i ][ i ];
263  for( int j = 0; j < i; ++j )
264  x -= ret[ i ][ j ] * ret[ i ][ j ];
265  assert( x > FieldType( 0 ) );
266  rii = sqrt( x );
267 
268  FieldType invrii = FieldType( 1 ) / rii;
269  for( int k = i+1; k < n; ++k )
270  {
271  FieldType x = A[ k ][ i ];
272  for( int j = 0; j < i; ++j )
273  x -= ret[ i ][ j ] * ret[ k ][ j ];
274  ret[ k ][ i ] = invrii * x;
275  }
276  }
277  }
278 
279  template< int n >
280  static FieldType
281  detL ( const typename Traits :: template Matrix< n, n > :: type &L )
282  {
283  FieldType det = FieldType( 1 );
284  for( int i = 0; i < n; ++i )
285  det *= L[ i ][ i ];
286  return det;
287  }
288 
289  template< int n >
290  static FieldType
291  invL ( typename Traits :: template Matrix< n, n > :: type &L )
292  {
293  FieldType det = FieldType( 1 );
294  for( int i = 0; i < n; ++i )
295  {
296  FieldType &lii = L[ i ][ i ];
297  det *= lii;
298  lii = FieldType( 1 ) / lii;
299  for( int j = 0; j < i; ++j )
300  {
301  FieldType &lij = L[ i ][ j ];
302  FieldType x = lij * L[ j ][ j ];
303  for( int k = j+1; k < i; ++k )
304  x += L[ i ][ k ] * L[ k ][ j ];
305  lij = (-lii) * x;
306  }
307  }
308  return det;
309  }
310 
311  // calculates x := L^{-1} x
312  template< int n >
313  static void
314  invLx ( typename Traits :: template Matrix< n, n > :: type &L,
315  typename Traits :: template Vector< n > :: type &x )
316  {
317  for( int i = 0; i < n; ++i )
318  {
319  for( int j = 0; j < i; ++j )
320  x[ i ] -= L[ i ][ j ] * x[ j ];
321  x[ i ] /= L[ i ][ i ];
322  }
323  }
324 
325  // calculates x := L^{-T} x
326  template< int n >
327  static void
328  invLTx ( typename Traits :: template Matrix< n, n > :: type &L,
329  typename Traits :: template Vector< n > :: type &x )
330  {
331  for( int i = n; i > 0; --i )
332  {
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 ];
336  }
337  }
338 
339  template< int n >
340  static FieldType
341  spdDetA ( const typename Traits :: template Matrix< n, n > :: type &A )
342  {
343  // return A[0][0]*A[1][1]-A[1][0]*A[1][0];
344  typename Traits :: template Matrix< n, n > :: type L;
345  cholesky_L< n >( A, L );
346  return detL< n >( L );
347  }
348 
349  template< int n >
350  static FieldType
351  spdInvA ( typename Traits :: template Matrix< n, n > :: type &A )
352  {
353  typename Traits :: template Matrix< n, n > :: type L;
354  cholesky_L< n >( A, L );
355  const FieldType det = invL< n >( L );
356  LTL< n >( L, A );
357  return det;
358  }
359 
360  // calculate x := A^{-1} x
361  template< int n >
362  static void
363  spdInvAx ( typename Traits :: template Matrix< n, n > :: type &A,
364  typename Traits :: template Vector< n > :: type &x )
365  {
366  typename Traits :: template Matrix< n, n > :: type L;
367  cholesky_L< n >( A, L );
368  invLx< n >( L, x );
369  invLTx< n >( L, x );
370  }
371 
372  template< int m, int n >
373  static FieldType
374  detATA ( const typename Traits :: template Matrix< m, n > :: type &A )
375  {
376  if( m >= n )
377  {
378  typename Traits :: template Matrix< n, n > :: type ata;
379  ATA_L< m, n >( A, ata );
380  return spdDetA< n >( ata );
381  }
382  else
383  return FieldType( 0 );
384  }
385 
391  template< int m, int n >
392  static FieldType
393  sqrtDetAAT ( const typename Traits::template Matrix< m, n >::type &A )
394  {
395  // These special cases are here not only for speed reasons:
396  // The general implementation aborts if the matrix is almost singular,
397  // and the special implementation provide a stable way to handle that case.
398  if( (n == 2) && (m == 2) )
399  {
400  // Special implementation for 2x2 matrices: faster and more stable
401  return abs( A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ] );
402  }
403  else if( (n == 3) && (m == 3) )
404  {
405  // Special implementation for 3x3 matrices
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 ] );
410  }
411  else if ( (n == 3) && (m == 2) )
412  {
413  // Special implementation for 2x3 matrices
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);
418  }
419  else if( n >= m )
420  {
421  // General case
422  typename Traits::template Matrix< m, m >::type aat;
423  AAT_L< m, n >( A, aat );
424  return spdDetA< m >( aat );
425  }
426  else
427  return FieldType( 0 );
428  }
429 
430  // A^{-1}_L = (A^T A)^{-1} A^T
431  // => A^{-1}_L A = I
432  template< int m, int n >
433  static FieldType
434  leftInvA ( const typename Traits :: template Matrix< m, n > :: type &A,
435  typename Traits :: template Matrix< n, m > :: type &ret )
436  {
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 );
442  return det;
443  }
444 
445  template< int m, int n >
446  static void
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 )
450  {
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 );
456  }
457 
458  // A^{-1}_R = A^T (A A^T)^{-1}
459  // => A A^{-1}_R = I
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  dune_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  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 );
497  }
498  };
499 
500  } // namespace GenericGeometry
501 
502 } // namespace Dune
503 
504 #endif // #ifndef DUNE_GEOMETRY_GENERICGEOMETRY_MATRIXHELPER_HH