32 #include <boost/mpl/int.hpp>
33 #include <feel/feelcore/feel.hpp>
34 #include <feel/feelcore/traits.hpp>
39 namespace mpl = boost::mpl;
43 template<
typename Matrix>
44 inline typename Matrix::value_type
45 det( Matrix
const& M, mpl::int_<1> )
49 template<
typename Matrix>
50 inline typename Matrix::value_type
51 det( Matrix
const& M, mpl::int_<2> )
53 return M( 0, 0 )*M( 1, 1 )-M( 0, 1 )*M( 1, 0 );
55 template<
typename Matrix>
56 inline typename Matrix::value_type
57 det( Matrix
const& M, mpl::int_<3> )
59 return ( M( 0, 0 )*M( 1, 1 )*M( 2, 2 )-
60 M( 0, 0 )*M( 1, 2 )*M( 2, 1 )-
61 M( 1, 0 )*M( 0, 1 )*M( 2, 2 )+
62 M( 1, 0 )*M( 0, 2 )*M( 2, 1 )+
63 M( 2, 0 )*M( 0, 1 )*M( 1, 2 )-
64 M( 2, 0 )*M( 0, 2 )*M( 1, 1 ) );
67 template<
typename Matrix>
68 inline typename Matrix::value_type
69 det( Matrix
const& M, mpl::int_<4> )
71 return ( M( 0, 0 ) * M( 1, 1 ) * M( 2, 2 ) * M( 3, 3 ) -
72 M( 0, 0 ) * M( 1, 1 ) * M( 2, 3 ) * M( 3, 2 ) -
73 M( 0, 0 ) * M( 2, 1 ) * M( 1, 2 ) * M( 3, 3 ) +
74 M( 0, 0 ) * M( 2, 1 ) * M( 1, 3 ) * M( 3, 2 ) +
75 M( 0, 0 ) * M( 3, 1 ) * M( 1, 2 ) * M( 2, 3 ) -
76 M( 0, 0 ) * M( 3, 1 ) * M( 1, 3 ) * M( 2, 2 ) -
77 M( 1, 0 ) * M( 0, 1 ) * M( 2, 2 ) * M( 3, 3 ) +
78 M( 1, 0 ) * M( 0, 1 ) * M( 2, 3 ) * M( 3, 2 ) +
79 M( 1, 0 ) * M( 2, 1 ) * M( 0, 2 ) * M( 3, 3 ) -
80 M( 1, 0 ) * M( 2, 1 ) * M( 0, 3 ) * M( 3, 2 ) -
81 M( 1, 0 ) * M( 3, 1 ) * M( 0, 2 ) * M( 2, 3 ) +
82 M( 1, 0 ) * M( 3, 1 ) * M( 0, 3 ) * M( 2, 2 ) +
83 M( 2, 0 ) * M( 0, 1 ) * M( 1, 2 ) * M( 3, 3 ) -
84 M( 2, 0 ) * M( 0, 1 ) * M( 1, 3 ) * M( 3, 2 ) -
85 M( 2, 0 ) * M( 1, 1 ) * M( 0, 2 ) * M( 3, 3 ) +
86 M( 2, 0 ) * M( 1, 1 ) * M( 0, 3 ) * M( 3, 2 ) +
87 M( 2, 0 ) * M( 3, 1 ) * M( 0, 2 ) * M( 1, 3 ) -
88 M( 2, 0 ) * M( 3, 1 ) * M( 0, 3 ) * M( 1, 2 ) -
89 M( 3, 0 ) * M( 0, 1 ) * M( 1, 2 ) * M( 2, 3 ) +
90 M( 3, 0 ) * M( 0, 1 ) * M( 1, 3 ) * M( 2, 2 ) +
91 M( 3, 0 ) * M( 1, 1 ) * M( 0, 2 ) * M( 2, 3 ) -
92 M( 3, 0 ) * M( 1, 1 ) * M( 0, 3 ) * M( 2, 2 ) -
93 M( 3, 0 ) * M( 2, 1 ) * M( 0, 2 ) * M( 1, 3 ) +
94 M( 3, 0 ) * M( 2, 1 ) * M( 0, 3 ) * M( 1, 2 ) );
99 template<
typename Matrix>
101 inverse( Matrix
const& M, Matrix& Minv, mpl::int_<1> )
103 Minv( 0, 0 ) = 1.0/M( 0, 0 );
105 template<
typename Matrix>
107 inverse( Matrix
const& M, Matrix& Minv, mpl::int_<2> )
109 typename Matrix::value_type J = M( 0, 0 )*M( 1, 1 )-M( 0, 1 )*M( 1, 0 );
110 Minv( 0, 0 ) = M( 1, 1 )/J;
111 Minv( 1, 0 ) = -M( 1, 0 )/J;
112 Minv( 0, 1 ) = -M( 0, 1 )/J;
113 Minv( 1, 1 ) = M( 0, 0 )/J;
115 template<
typename Matrix>
117 inverse( Matrix
const& M, Matrix& Minv, mpl::int_<3> )
119 typedef typename Matrix::value_type value_type;
121 value_type t4 = M( 0, 0 )*M( 1, 1 );
122 value_type t6 = M( 0, 0 )*M( 1, 2 );
123 value_type t8 = M( 0, 1 )*M( 1, 0 );
124 value_type t10 = M( 0, 2 )*M( 1, 0 );
126 value_type t12 = M( 0, 1 )*M( 2, 0 );
127 value_type t14 = M( 0, 2 )*M( 2, 0 );
128 value_type t17 = 1.0/( M( 2, 2 )*t4-t6*M( 2, 1 )-t8*M( 2, 2 )+t10*M( 2, 1 )+t12*M( 1, 2 )-t14*M( 1, 1 ) );
130 Minv( 0, 0 ) = ( M( 1, 1 )*M( 2, 2 )-M( 1, 2 )*M( 2, 1 ) )*t17;
131 Minv( 0, 1 ) = -( M( 0, 1 )*M( 2, 2 )-M( 0, 2 )*M( 2, 1 ) )*t17;
132 Minv( 0, 2 ) = -( -M( 0, 1 )*M( 1, 2 )+M( 0, 2 )*M( 1, 1 ) )*t17;
133 Minv( 1, 0 ) = -( M( 1, 0 )*M( 2, 2 )-M( 1, 2 )*M( 2, 0 ) )*t17;
134 Minv( 1, 1 ) = ( M( 0, 0 )*M( 2, 2 )-t14 )*t17;
135 Minv( 1, 2 ) = -( t6-t10 )*t17;
136 Minv( 2, 0 ) = -( -M( 1, 0 )*M( 2, 1 )+M( 1, 1 )*M( 2, 0 ) )*t17;
137 Minv( 2, 1 ) = -( M( 0, 0 )*M( 2, 1 )-t12 )*t17;
138 Minv( 2, 2 ) = ( t4-t8 )*t17;
141 template<
typename Matrix>
143 inverse( Matrix
const& __restrict__ M, Matrix& __restrict__ Minv,
typename Matrix::value_type
const& J, mpl::int_<1> )
145 Minv( 0, 0 ) =
typename Matrix::value_type( 1.0 )/J;
147 template<
typename Matrix>
149 inverse( Matrix
const& __restrict__ M, Matrix& __restrict__ Minv,
typename Matrix::value_type
const& J, mpl::int_<2> )
151 Minv( 0, 0 ) = M( 1, 1 )/J;
152 Minv( 1, 0 ) = -M( 1, 0 )/J;
153 Minv( 0, 1 ) = -M( 0, 1 )/J;
154 Minv( 1, 1 ) = M( 0, 0 )/J;
156 template<
typename Matrix>
158 inverse( Matrix
const& __restrict__ M, Matrix& __restrict__ Minv,
typename Matrix::value_type
const& J, mpl::int_<3> )
160 typedef typename Matrix::value_type value_type;
162 value_type t4 = M( 0, 0 )*M( 1, 1 );
163 value_type t6 = M( 0, 0 )*M( 1, 2 );
164 value_type t8 = M( 0, 1 )*M( 1, 0 );
165 value_type t10 = M( 0, 2 )*M( 1, 0 );
167 value_type t12 = M( 0, 1 )*M( 2, 0 );
168 value_type t14 = M( 0, 2 )*M( 2, 0 );
170 Minv( 0, 0 ) = ( M( 1, 1 )*M( 2, 2 )-M( 1, 2 )*M( 2, 1 ) )/J;
171 Minv( 0, 1 ) = -( M( 0, 1 )*M( 2, 2 )-M( 0, 2 )*M( 2, 1 ) )/J;
172 Minv( 0, 2 ) = -( -M( 0, 1 )*M( 1, 2 )+M( 0, 2 )*M( 1, 1 ) )/J;
173 Minv( 1, 0 ) = -( M( 1, 0 )*M( 2, 2 )-M( 1, 2 )*M( 2, 0 ) )/J;
174 Minv( 1, 1 ) = ( M( 0, 0 )*M( 2, 2 )-t14 )/J;
175 Minv( 1, 2 ) = -( t6-t10 )/J;
176 Minv( 2, 0 ) = -( -M( 1, 0 )*M( 2, 1 )+M( 1, 1 )*M( 2, 0 ) )/J;
177 Minv( 2, 1 ) = -( M( 0, 0 )*M( 2, 1 )-t12 )/J;
178 Minv( 2, 2 ) = ( t4-t8 )/J;
184 template<
int Dim,
typename Matrix>
185 inline typename Matrix::value_type
186 det( Matrix
const& M )
188 return details::det( M, mpl::int_<Dim>() );
191 template<
int Dim,
typename Matrix>
193 inverse( Matrix
const& M, Matrix& Minv )
195 details::inverse( M, Minv, mpl::int_<Dim>() );
198 template<
int Dim,
typename Matrix>
200 inverse( Matrix
const& __restrict__ M, Matrix& __restrict__ Minv,
typename Matrix::value_type
const& J )
202 details::inverse( M, Minv, J, mpl::int_<Dim>() );
218 template <
typename MatrixType>
224 typedef typename MatrixType::value_type value_type;
225 typedef MatrixType matrix_type;
226 typedef boost::numeric::ublas::vector<value_type> vector_type;
227 typedef boost::numeric::ublas::vector<uint> vector_uint_type;
234 LU (
const matrix_type &A )
236 __LU( A.size1(), A.size2() ),
245 DVLOG(2) <<
"LU m = "<< m <<
"\n";
246 DVLOG(2) <<
"LU n = "<< n <<
"\n";
250 for ( uint i = 0; i < m; i++ )
258 vector_type LUcolj( m );
262 for ( uint j = 0; j < n; j++ )
267 for ( uint i = 0; i < m; i++ )
269 LUcolj( i ) = __LU( i,j );
274 for ( uint i = 0; i < m; i++ )
276 boost::numeric::ublas::matrix_row<matrix_type> LUrowi ( __LU, i );
281 uint kmax = std::min( i,j );
282 value_type s = value_type( 0 );
284 for ( uint k = 0; k < kmax; k++ )
286 s += LUrowi( k )*LUcolj( k );
289 LUrowi( j ) = LUcolj( i ) -= s;
296 for ( uint i = j+1; i < m; i++ )
298 if ( math::abs( LUcolj( i ) ) > math::abs( LUcolj( p ) ) )
306 for ( uint k = 0; k < n; k++ )
308 value_type t = __LU( p,k );
309 __LU( p,k ) = __LU( j,k );
321 if ( ( j < m ) && ( __LU( j,j ) != value_type( 0.0 ) ) )
323 for ( uint i = j+1; i < m; i++ )
325 __LU( i,j ) /= __LU( j,j );
339 for ( uint j = 0; j < n; j++ )
341 if ( __LU( j,j ) == value_type( 0.0 ) )
355 matrix_type L_( m,n );
357 for ( uint i = 0; i < m; i++ )
359 for ( uint j = 0; j < n; j++ )
363 L_( i,j ) = __LU( i,j );
387 matrix_type U_( n,n );
389 for ( uint i = 0; i < n; i++ )
391 for ( uint j = 0; j < n; j++ )
395 U_( i,j ) = __LU( i,j );
424 return value_type( 0 );
427 value_type d = value_type( pivsign );
430 for ( uint j = 0; j < n; j++ )
438 void inverse( matrix_type& __inv )
440 __inv =
solve( ublas::identity_matrix<double>( m, m ) );
443 __t = ZeroVector( __t.size() );
445 for (
size_type i = 0; i < piv.size(); ++i )
447 __t[i] = value_type( 1 );
448 ublas::row( __inv, i ) =
solve( __t );
449 __t[i] = value_type( 0 );
459 matrix_type
solve (
const matrix_type &B )
464 if ( B.size1() != m )
466 return matrix_type( 0,0 );
471 return matrix_type( 0,0 );
478 matrix_type X ( permute_copy( B, piv, 0, nx-1 ) );
481 for ( uint k = 0; k < n; k++ )
483 for ( uint i = k+1; i < n; i++ )
485 for ( uint j = 0; j < nx; j++ )
487 X( i,j ) -= X( k,j )*__LU( i,k );
493 for (
int k = (
int )n-1; k >= 0; k-- )
495 for ( uint j = 0; j < nx; j++ )
497 X( k,j ) /= __LU( k,k );
500 for (
int i = 0; i < k; i++ )
502 for ( uint j = 0; j < nx; j++ )
504 X( i,j ) -= X( k,j )*__LU( i,k );
522 vector_type
solve (
const vector_type &b )
529 return vector_type();
534 return vector_type();
538 vector_type x = permute_copy( b, piv );
541 for ( uint k = 0; k < n; k++ )
543 for ( uint i = k+1; i < n; i++ )
545 x( i ) -= x( k )*__LU( i,k );
550 for (
int k = (
int )n-1; k >= 0; k-- )
552 x( k ) /= __LU( k,k );
554 for (
int i = 0; i < k; i++ )
556 x( i ) -= x( k )*__LU( i,k );
569 vector_uint_type piv;
573 permute_copy(
const matrix_type &A,
const vector_uint_type &piv, uint j0, uint j1 )
575 uint piv_length = piv.size();
577 matrix_type X( piv_length, j1-j0+1 );
580 for ( uint i = 0; i < piv_length; i++ )
581 for ( uint j = j0; j <= j1; j++ )
582 X( i,j-j0 ) = A( piv( i ),j );
588 permute_copy(
const vector_type &A,
const vector_uint_type &piv )
590 uint piv_length = piv.size();
592 if ( piv_length != A.size() )
593 return vector_type();
595 vector_type x( piv_length );
598 for ( uint i = 0; i < piv_length; i++ )
599 x( i ) = A( piv( i ) );
vector_uint_type getPivot()
Definition: lu.hpp:411
Definition: solverlinear.hpp:33
LU(const matrix_type &A)
Definition: lu.hpp:234
vector_type solve(const vector_type &b)
Definition: lu.hpp:522
value_type det()
Definition: lu.hpp:420
matrix_type getU()
Definition: lu.hpp:385
matrix_type getL()
Definition: lu.hpp:353
size_t size_type
Indices (starting from 0)
Definition: feelcore/feel.hpp:319
uint isNonsingular()
Definition: lu.hpp:337
matrix_type solve(const matrix_type &B)
Definition: lu.hpp:459