|
realroot_doc 0.1.1
|
|
realroot_doc 0.1.1
|
00001 /******************************************************************** 00002 * This file is part of the source code of the realroot library. 00003 * Author(s): J.P. Pavone, GALAAD, INRIA 00004 * $Id: doolittle.hpp,v 1.1 2005/07/11 10:03:55 jppavone Exp $ 00005 ********************************************************************/ 00006 #ifndef realroot_SOLVE_SBDSLV_LINEAR_DOOLITTLE_HPP 00007 #define realroot_SOLVE_SBDSLV_LINEAR_DOOLITTLE_HPP 00008 //-------------------------------------------------------------------- 00009 #include <realroot/system_epsilon.hpp> 00010 //-------------------------------------------------------------------- 00011 namespace mmx { 00012 //-------------------------------------------------------------------- 00013 namespace linear 00014 { 00015 00016 template< typename real_t > 00017 void Lsolve(real_t * x , real_t const * const L, real_t const *const b, unsigned n ) 00018 { 00019 for ( unsigned i = 0; i < n ; i ++ ) 00020 { 00021 real_t sum = 0.0; 00022 for ( unsigned j = 0; j < i; j ++ ) sum += L[i*n+j]*x[j]; 00023 x[i] = (b[i]-sum); 00024 }; 00025 }; 00026 00027 template< typename real_t > 00028 bool Usolve( real_t * x, real_t const * const U, real_t const *const b, unsigned n ) 00029 { 00030 for ( int i = n-1; i >=0 ; i -- ) 00031 { 00032 real_t sum = 0.0; 00033 for ( int j = n-1; j > i; j -- ) sum += U[i*n+j]*x[j]; 00034 if ( std::abs(U[i*n+i]) < numerics::epsilon<real_t>::result*n*n) return false; 00035 x[i] = (b[i]-sum)/U[i*n+i]; 00036 }; 00037 return true; 00038 }; 00039 00040 template< typename real_t > 00041 bool doolittle( real_t const * const A, real_t * L, real_t * U, unsigned n ) 00042 { 00043 unsigned k,i,p; 00044 for ( k = 0; k < n; k++ ) 00045 { 00046 // valueof(k); 00047 for ( i = k; i < n; i ++ ) 00048 { 00049 U[ k*n + i ] = A[ k*n + i ]; 00050 for ( p = 0; p < k; p ++ ) U[ k*n + i ] -= L[ k*n + p ]*U[ p*n + i ]; 00051 }; 00052 for ( i = k + 1; i < n; i ++ ) 00053 { 00054 if ( std::abs( U[ k*n + k ] ) < numerics::epsilon<real_t>::result*n*n ) return false; 00055 real_t sum = 0.0; 00056 for ( p = 0; p < k; p ++ ) sum += L[i*n+p]*U[p*n+k]; 00057 L[i*n+k] = (A[i*n + k] - sum)/U[ k*n + k ]; 00058 }; 00059 }; 00060 return true; 00061 }; 00062 00063 template< typename real_t > 00064 bool LUsolve( real_t * x, real_t const * const M, real_t const * const b, unsigned n ) 00065 { 00066 real_t tmp[ n*n ]; 00067 real_t xtmp[n] ; 00068 if ( !doolittle( M, tmp, tmp, n ) ) return false; 00069 Lsolve( xtmp, tmp, b, n ); 00070 return Usolve( x, tmp, xtmp, n ); 00071 }; 00072 00073 template< typename real_t > 00074 bool LUinverse( real_t * iM, real_t const * const M, unsigned n ) 00075 { 00076 real_t lu[n*n]; 00077 real_t xtmp[n] ; 00078 real_t tmp[n]; 00079 real_t _b[n+1]; 00080 real_t * b = _b+1; 00081 std::fill( b, b + n , (real_t)0.0 ); 00082 if ( ! doolittle( M, lu, lu, n ) ) return false; 00083 for ( unsigned c = 0; c < n; c ++ ) 00084 { 00085 if (c > 0) b[c-1] = 0; 00086 b[c] = 1; 00087 Lsolve( xtmp, lu, b, n ); 00088 if ( !Usolve( tmp, lu, xtmp, n ) ) return false; 00089 for ( unsigned i = 0; i < n; i ++ ) iM[ i*n + c ] = tmp[i]; 00090 }; 00091 return true; 00092 }; 00093 00094 }; 00095 //-------------------------------------------------------------------- 00096 } //namespace mmx 00097 /********************************************************************/ 00098 #endif //
