The Parametric Pseudo-Manifold (PPS) Library 1.0
|
00001 00031 #include "ludcmp.h" 00032 00033 #include <cassert> // assert 00034 #include <cmath> // fabs 00035 00036 00051 namespace pps { 00052 00053 00063 LUdcmp::LUdcmp( 00064 double** mat, 00065 unsigned n 00066 ) 00067 { 00068 _nele = n; 00069 _matA = allocate( _nele ) ; 00070 00071 for ( unsigned i = 0 ; i < _nele ; i++ ) { 00072 for ( unsigned j = 0 ; j < _nele ; j++ ) { 00073 _matA[ i ][ j ] = mat[ i ][ j ] ; 00074 } 00075 } 00076 00077 _perm.resize( _nele ) ; 00078 _sign = 1 ; 00079 00080 decomp() ; 00081 00082 return ; 00083 } 00084 00085 00094 LUdcmp::LUdcmp( const LUdcmp& lu ) : _nele( lu._nele ) 00095 { 00096 _matA = allocate( _nele ) ; 00097 00098 _perm.resize( _nele ) ; 00099 _sign = lu._sign ; 00100 00101 for ( unsigned i = 0 ; i < _nele; i++ ) { 00102 _perm[ i ] = lu._perm[ i ] ; 00103 00104 for ( unsigned j = 0 ; j < _nele ; j++ ) { 00105 _matA[ i ][ j ] = lu._matA[ i ][ j ] ; 00106 } 00107 } 00108 00109 return ; 00110 } 00111 00112 00119 LUdcmp::~LUdcmp() 00120 { 00121 deallocate( _matA , _nele ) ; 00122 00123 return ; 00124 } 00125 00126 00135 void 00136 LUdcmp::solve( 00137 double* b, 00138 unsigned n 00139 ) 00140 const 00141 { 00142 assert( n == _nele ) ; 00143 00144 int i, ii = 0, ip, j; 00145 double sum; 00146 for ( i = 0 ; i < (int) n ; i++ ) { 00147 ip = _perm[ i ] ; 00148 sum = b[ ip ] ; 00149 b[ ip ] = b[ i ] ; 00150 if ( ii != 0 ) { 00151 for ( j = ii - 1 ; j < i ; j++ ) { 00152 sum -= _matA[ i ][ j ] * b[ j ] ; 00153 } 00154 } 00155 else if ( sum != 0.0 ) { 00156 ii = i + 1 ; 00157 } 00158 00159 b[ i ] = sum ; 00160 } 00161 00162 for ( i = n - 1 ; i >= 0 ; i-- ) { 00163 sum = b[ i ] ; 00164 for ( j = i + 1 ; j < (int) n ; j++ ) { 00165 sum -= _matA[ i ][ j ] * b[ j ] ; 00166 } 00167 00168 b[ i ] = sum / _matA[ i ][ i ] ; 00169 } 00170 00171 return ; 00172 } 00173 00174 00175 00185 double** 00186 LUdcmp::allocate( unsigned n ) const 00187 { 00188 double** mat = (double **) new double*[n]; 00189 00190 for ( unsigned i = 0 ; i < n ; i++ ) { 00191 mat[ i ] = ( double* ) new double[ n ] ; 00192 } 00193 00194 return mat ; 00195 } 00196 00197 00206 void 00207 LUdcmp::deallocate( 00208 double** mat, 00209 unsigned n 00210 ) 00211 const 00212 { 00213 for ( unsigned i = 0 ; i < n ; i++ ) { 00214 delete[] mat[ i ] ; 00215 } 00216 00217 delete mat ; 00218 00219 return ; 00220 } 00221 00222 00230 void 00231 LUdcmp::decomp() 00232 { 00233 const double TINY = 1.0e-20 ; 00234 int i , imax=0 , j , k ; 00235 double big , dum , sum , temp ; 00236 00237 int n = ( int ) _nele ; 00238 std::vector< double > vv( n ) ; 00239 _sign = 1 ; 00240 for ( i = 0 ; i < n ; i++ ) { 00241 big = 0.0 ; 00242 for ( j = 0 ; j < n ; j++ ) { 00243 if ( ( temp = fabs( _matA[ i ][ j ] ) ) > big ) { 00244 big = temp ; 00245 } 00246 } 00247 00248 assert( big != 0.0 ) ; 00249 00250 vv[ i ] = 1.0 / big ; 00251 } 00252 00253 for ( j = 0 ; j < n ; j++ ) { 00254 for ( i = 0 ; i < j ; i++ ) { 00255 sum = _matA[ i ][ j ] ; 00256 00257 for ( k = 0 ; k < i ; k++ ) { 00258 sum -= _matA[ i ][ k ] * _matA[ k ][ j ] ; 00259 } 00260 00261 _matA[ i ][ j ] = sum ; 00262 } 00263 00264 big = 0.0 ; 00265 00266 for ( i = j ; i < n ; i++ ) { 00267 sum = _matA[ i ][ j ] ; 00268 for ( k = 0 ; k < j ; k++ ) { 00269 sum -= _matA[ i ][ k ] * _matA[ k ][ j ] ; 00270 } 00271 00272 _matA[ i ][ j ] = sum ; 00273 00274 if ( ( dum = vv[ i ] * fabs( sum ) ) >= big ) { 00275 big = dum ; 00276 imax = i ; 00277 } 00278 } 00279 00280 if ( j != imax ) { 00281 for ( k = 0 ; k < n ; k++ ) { 00282 dum = _matA[ imax ][ k ] ; 00283 _matA[ imax ][ k ] = _matA[ j ][ k ] ; 00284 _matA[ j ][ k ] = dum ; 00285 } 00286 00287 _sign = -_sign ; 00288 vv[ imax ] = vv[ j ] ; 00289 } 00290 00291 _perm[ j ] = imax ; 00292 00293 if ( _matA[ j ][ j ] == 0.0 ) { 00294 _matA[ j ][ j ] = TINY ; 00295 } 00296 00297 if ( j != ( n - 1 ) ) { 00298 dum = 1.0 / ( _matA[ j ][ j ] ) ; 00299 for ( i = j + 1 ; i < n ; i++ ) { 00300 _matA[ i ][ j ] *= dum ; 00301 } 00302 } 00303 } 00304 00305 return ; 00306 } 00307 00308 } 00309 //end of group class.