29 template<
typename Implementation >
32 template<
class ctype,
int dim >
35 template<
class ctype,
int dim >
48 struct FieldMatrixHelper
52 template<
int m,
int n >
53 static void Ax (
const FieldMatrix< ctype, m, n > &A,
const FieldVector< ctype, n > &x, FieldVector< ctype, m > &ret )
55 for(
int i = 0; i < m; ++i )
57 ret[ i ] = ctype( 0 );
58 for(
int j = 0; j < n; ++j )
59 ret[ i ] += A[ i ][ j ] * x[ j ];
63 template<
int m,
int n >
64 static void ATx (
const FieldMatrix< ctype, m, n > &A,
const FieldVector< ctype, m > &x, FieldVector< ctype, n > &ret )
66 for(
int i = 0; i < n; ++i )
68 ret[ i ] = ctype( 0 );
69 for(
int j = 0; j < m; ++j )
70 ret[ i ] += A[ j ][ i ] * x[ j ];
74 template<
int m,
int n,
int p >
75 static void AB (
const FieldMatrix< ctype, m, n > &A,
const FieldMatrix< ctype, n, p > &B, FieldMatrix< ctype, m, p > &ret )
77 for(
int i = 0; i < m; ++i )
79 for(
int j = 0; j < p; ++j )
81 ret[ i ][ j ] = ctype( 0 );
82 for(
int k = 0; k < n; ++k )
83 ret[ i ][ j ] += A[ i ][ k ] * B[ k ][ j ];
88 template<
int m,
int n,
int p >
89 static void ATBT (
const FieldMatrix< ctype, m, n > &A,
const FieldMatrix< ctype, p, m > &B, FieldMatrix< ctype, n, p > &ret )
91 for(
int i = 0; i < n; ++i )
93 for(
int j = 0; j < p; ++j )
95 ret[ i ][ j ] = ctype( 0 );
96 for(
int k = 0; k < m; ++k )
97 ret[ i ][ j ] += A[ k ][ i ] * B[ j ][ k ];
102 template<
int m,
int n >
103 static void ATA_L (
const FieldMatrix< ctype, m, n > &A, FieldMatrix< ctype, n, n > &ret )
105 for(
int i = 0; i < n; ++i )
107 for(
int j = 0; j <= i; ++j )
109 ret[ i ][ j ] = ctype( 0 );
110 for(
int k = 0; k < m; ++k )
111 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
116 template<
int m,
int n >
117 static void ATA (
const FieldMatrix< ctype, m, n > &A, FieldMatrix< ctype, n, n > &ret )
119 for(
int i = 0; i < n; ++i )
121 for(
int j = 0; j <= i; ++j )
123 ret[ i ][ j ] = ctype( 0 );
124 for(
int k = 0; k < m; ++k )
125 ret[ i ][ j ] += A[ k ][ i ] * A[ k ][ j ];
126 ret[ j ][ i ] = ret[ i ][ j ];
129 ret[ i ][ i ] = ctype( 0 );
130 for(
int k = 0; k < m; ++k )
131 ret[ i ][ i ] += A[ k ][ i ] * A[ k ][ i ];
135 template<
int m,
int n >
136 static void AAT_L (
const FieldMatrix< ctype, m, n > &A, FieldMatrix< ctype, m, m > &ret )
146 for(
int i = 0; i < m; ++i )
148 for(
int j = 0; j <= i; ++j )
150 ctype &retij = ret[ i ][ j ];
151 retij = A[ i ][ 0 ] * A[ j ][ 0 ];
152 for(
int k = 1; k < n; ++k )
153 retij += A[ i ][ k ] * A[ j ][ k ];
158 template<
int m,
int n >
159 static void AAT (
const FieldMatrix< ctype, m, n > &A, FieldMatrix< ctype, m, m > &ret )
161 for(
int i = 0; i < m; ++i )
163 for(
int j = 0; j < i; ++j )
165 ret[ i ][ j ] = ctype( 0 );
166 for(
int k = 0; k < n; ++k )
167 ret[ i ][ j ] += A[ i ][ k ] * A[ j ][ k ];
168 ret[ j ][ i ] = ret[ i ][ j ];
170 ret[ i ][ i ] = ctype( 0 );
171 for(
int k = 0; k < n; ++k )
172 ret[ i ][ i ] += A[ i ][ k ] * A[ i ][ k ];
177 static void Lx (
const FieldMatrix< ctype, n, n > &L,
const FieldVector< ctype, n > &x, FieldVector< ctype, n > &ret )
179 for(
int i = 0; i < n; ++i )
181 ret[ i ] = ctype( 0 );
182 for(
int j = 0; j <= i; ++j )
183 ret[ i ] += L[ i ][ j ] * x[ j ];
188 static void LTx (
const FieldMatrix< ctype, n, n > &L,
const FieldVector< ctype, n > &x, FieldVector< ctype, n > &ret )
190 for(
int i = 0; i < n; ++i )
192 ret[ i ] = ctype( 0 );
193 for(
int j = i; j < n; ++j )
194 ret[ i ] += L[ j ][ i ] * x[ j ];
199 static void LTL (
const FieldMatrix< ctype, n, n > &L, FieldMatrix< ctype, n, n > &ret )
201 for(
int i = 0; i < n; ++i )
203 for(
int j = 0; j < i; ++j )
205 ret[ i ][ j ] = ctype( 0 );
206 for(
int k = i; k < n; ++k )
207 ret[ i ][ j ] += L[ k ][ i ] * L[ k ][ j ];
208 ret[ j ][ i ] = ret[ i ][ j ];
210 ret[ i ][ i ] = ctype( 0 );
211 for(
int k = i; k < n; ++k )
212 ret[ i ][ i ] += L[ k ][ i ] * L[ k ][ i ];
217 static void LLT (
const FieldMatrix< ctype, n, n > &L, FieldMatrix< ctype, n, n > &ret )
219 for(
int i = 0; i < n; ++i )
221 for(
int j = 0; j < i; ++j )
223 ret[ i ][ j ] = ctype( 0 );
224 for(
int k = 0; k <= j; ++k )
225 ret[ i ][ j ] += L[ i ][ k ] * L[ j ][ k ];
226 ret[ j ][ i ] = ret[ i ][ j ];
228 ret[ i ][ i ] = ctype( 0 );
229 for(
int k = 0; k <= i; ++k )
230 ret[ i ][ i ] += L[ i ][ k ] * L[ i ][ k ];
235 static bool cholesky_L (
const FieldMatrix< ctype, n, n > &A, FieldMatrix< ctype, n, n > &ret,
const bool checkSingular =
false )
238 for(
int i = 0; i < n; ++i )
240 ctype &rii = ret[ i ][ i ];
242 ctype xDiag = A[ i ][ i ];
243 for(
int j = 0; j < i; ++j )
244 xDiag -= ret[ i ][ j ] * ret[ i ][ j ];
248 if( checkSingular && ! ( xDiag > ctype( 0 )) )
252 assert( xDiag > ctype( 0 ) );
255 ctype invrii = ctype( 1 ) / rii;
256 for(
int k = i+1; k < n; ++k )
258 ctype x = A[ k ][ i ];
259 for(
int j = 0; j < i; ++j )
260 x -= ret[ i ][ j ] * ret[ k ][ j ];
261 ret[ k ][ i ] = invrii * x;
270 static ctype detL (
const FieldMatrix< ctype, n, n > &L )
273 for(
int i = 0; i < n; ++i )
279 static ctype invL ( FieldMatrix< ctype, n, n > &L )
282 for(
int i = 0; i < n; ++i )
284 ctype &lii = L[ i ][ i ];
286 lii = ctype( 1 ) / lii;
287 for(
int j = 0; j < i; ++j )
289 ctype &lij = L[ i ][ j ];
290 ctype x = lij * L[ j ][ j ];
291 for(
int k = j+1; k < i; ++k )
292 x += L[ i ][ k ] * L[ k ][ j ];
301 static void invLx ( FieldMatrix< ctype, n, n > &L, FieldVector< ctype, n > &x )
303 for(
int i = 0; i < n; ++i )
305 for(
int j = 0; j < i; ++j )
306 x[ i ] -= L[ i ][ j ] * x[ j ];
307 x[ i ] /= L[ i ][ i ];
313 static void invLTx ( FieldMatrix< ctype, n, n > &L, FieldVector< ctype, n > &x )
315 for(
int i = n; i > 0; --i )
317 for(
int j = i; j < n; ++j )
318 x[ i-1 ] -= L[ j ][ i-1 ] * x[ j ];
319 x[ i-1 ] /= L[ i-1 ][ i-1 ];
324 static ctype spdDetA (
const FieldMatrix< ctype, n, n > &A )
327 FieldMatrix< ctype, n, n > L;
333 static ctype spdInvA ( FieldMatrix< ctype, n, n > &A )
335 FieldMatrix< ctype, n, n > L;
337 const ctype det = invL( L );
344 static bool spdInvAx ( FieldMatrix< ctype, n, n > &A, FieldVector< ctype, n > &x,
const bool checkSingular =
false )
346 FieldMatrix< ctype, n, n > L;
347 const bool invertible = cholesky_L( A, L, checkSingular );
348 if( ! invertible )
return invertible ;
354 template<
int m,
int n >
355 static ctype detATA (
const FieldMatrix< ctype, m, n > &A )
359 FieldMatrix< ctype, n, n > ata;
361 return spdDetA( ata );
372 template<
int m,
int n >
373 static ctype sqrtDetAAT (
const FieldMatrix< ctype, m, n > &A )
380 if( (n == 2) && (m == 2) )
383 return abs( A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ] );
385 else if( (n == 3) && (m == 3) )
388 const ctype v0 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 1 ][ 1 ] * A[ 0 ][ 2 ];
389 const ctype v1 = A[ 0 ][ 2 ] * A[ 1 ][ 0 ] - A[ 1 ][ 2 ] * A[ 0 ][ 0 ];
390 const ctype v2 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 1 ][ 0 ] * A[ 0 ][ 1 ];
391 return abs( v0 * A[ 2 ][ 0 ] + v1 * A[ 2 ][ 1 ] + v2 * A[ 2 ][ 2 ] );
393 else if ( (n == 3) && (m == 2) )
396 const ctype v0 = A[ 0 ][ 0 ] * A[ 1 ][ 1 ] - A[ 0 ][ 1 ] * A[ 1 ][ 0 ];
397 const ctype v1 = A[ 0 ][ 0 ] * A[ 1 ][ 2 ] - A[ 1 ][ 0 ] * A[ 0 ][ 2 ];
398 const ctype v2 = A[ 0 ][ 1 ] * A[ 1 ][ 2 ] - A[ 0 ][ 2 ] * A[ 1 ][ 1 ];
399 return sqrt( v0*v0 + v1*v1 + v2*v2);
404 FieldMatrix< ctype, m, m > aat;
406 return spdDetA( aat );
414 template<
int m,
int n >
415 static ctype leftInvA (
const FieldMatrix< ctype, m, n > &A, FieldMatrix< ctype, n, m > &ret )
417 static_assert((m >= n),
"Matrix has no left inverse.");
418 FieldMatrix< ctype, n, n > ata;
420 const ctype det = spdInvA( ata );
425 template<
int m,
int n >
426 static void leftInvAx (
const FieldMatrix< ctype, m, n > &A,
const FieldVector< ctype, m > &x, FieldVector< ctype, n > &y )
428 static_assert((m >= n),
"Matrix has no left inverse.");
429 FieldMatrix< ctype, n, n > ata;
436 template<
int m,
int n >
437 static ctype rightInvA (
const FieldMatrix< ctype, m, n > &A, FieldMatrix< ctype, n, m > &ret )
439 static_assert((n >= m),
"Matrix has no right inverse.");
441 if( (n == 2) && (m == 2) )
443 const ctype det = (A[ 0 ][ 0 ]*A[ 1 ][ 1 ] - A[ 1 ][ 0 ]*A[ 0 ][ 1 ]);
444 const ctype detInv = ctype( 1 ) / det;
445 ret[ 0 ][ 0 ] = A[ 1 ][ 1 ] * detInv;
446 ret[ 1 ][ 1 ] = A[ 0 ][ 0 ] * detInv;
447 ret[ 1 ][ 0 ] = -A[ 1 ][ 0 ] * detInv;
448 ret[ 0 ][ 1 ] = -A[ 0 ][ 1 ] * detInv;
453 FieldMatrix< ctype, m , m > aat;
455 const ctype det = spdInvA( aat );
456 ATBT( A , aat , ret );
461 template<
int m,
int n >
462 static bool xTRightInvA (
const FieldMatrix< ctype, m, n > &A,
const FieldVector< ctype, n > &x, FieldVector< ctype, m > &y )
464 static_assert((n >= m),
"Matrix has no right inverse.");
465 FieldMatrix< ctype, m, m > aat;
469 return spdInvAx( aat, y,
true );
482 template<
class ct,
int mydim,
int cdim>
512 typedef FieldMatrix< ctype, coorddimension, mydimension >
Jacobian;
524 typedef Impl::FieldMatrixHelper< ct > MatrixHelper;
530 : refElement_(refElement), origin_(origin), jacobianTransposed_(jt)
532 integrationElement_ = MatrixHelper::template rightInvA< mydimension, coorddimension >( jacobianTransposed_, jacobianInverseTransposed_ );
542 template<
class CoordVector >
544 : refElement_(refElement), origin_(coordVector[0])
547 jacobianTransposed_[ i ] = coordVector[ i+1 ] - origin_;
548 integrationElement_ = MatrixHelper::template rightInvA< mydimension, coorddimension >( jacobianTransposed_, jacobianInverseTransposed_ );
552 template<
class CoordVector >
604 jacobianInverseTransposed_.mtv(
global - origin_,
local );
620 return integrationElement_;
626 return integrationElement_ * refElement_.
volume();
637 return jacobianTransposed_;
648 return jacobianInverseTransposed_;
659 return jacobianTransposed_.transposed();
670 return jacobianInverseTransposed_.transposed();
675 return geometry.refElement_;
683 ctype integrationElement_;