00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064 #ifndef SCPPNT_LU_H
00065 #define SCPPNT_LU_H
00066
00067 #include <cmath>
00068 #ifdef BOOST_NO_STDC_NAMESPACE
00069 namespace std
00070 { using ::fabs;}
00071 #endif
00072
00073 #ifdef SCPPNT_NO_DIR_PREFIX
00074 #include "scppnt.h"
00075 #include "scppnt_error.h"
00076 #else
00077 #include "scppnt/scppnt.h"
00078 #include "scppnt/scppnt_error.h"
00079 #endif
00080
00081 namespace SCPPNT
00082 {
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106 template<class MaTRiX, class VecToRSubscript> int LU_factor(MaTRiX &A, VecToRSubscript &indx)
00107 {
00108
00109
00110
00111
00112
00113 typedef typename MaTRiX::column_iterator column_iterator;
00114 typedef typename MaTRiX::row_iterator row_iterator;
00115
00116 Subscript M = A.num_rows();
00117 Subscript N = A.num_columns();
00118
00119 if (M == 0 || N==0)
00120 return 0;
00121 if (indx.dim() != M)
00122 indx.newsize(M);
00123
00124 Subscript i, j, k;
00125 Subscript jp;
00126
00127 typename MaTRiX::element_type t;
00128
00129 Subscript minMN = (M < N ? M : N);
00130
00131 typename MaTRiX::diag_iterator adiag = A.begin_diagonal(1, 1);
00132 typename MaTRiX::columns_iterator icolumns = A.begin_columns();
00133 for (j=1; j<= minMN; j++, ++adiag, ++icolumns)
00134 {
00135
00136
00137 jp = j;
00138 t = std::fabs(*adiag);
00139
00140 column_iterator ic = *icolumns + j;
00141 for (i=j+1; i<=M; i++, ++ic)
00142 {
00143 double ab = std::fabs(*ic);
00144 if (ab > t)
00145 {
00146 jp = i;
00147 t = ab;
00148 }
00149 }
00150
00151 indx(j) = jp;
00152
00153
00154
00155
00156 if ((*icolumns)[jp-1] == 0)
00157 return 1;
00158
00159 if (jp != j)
00160 {
00161 row_iterator irowj = A.begin_row(j);
00162 row_iterator irowjp = A.begin_row(jp);
00163
00164 for (k=1; k<=N; k++, ++irowj, ++irowjp)
00165 {
00166 t = *irowj;
00167 *irowj = *irowjp;
00168 *irowjp = t;
00169 }
00170 }
00171
00172 if (j<M)
00173 {
00174
00175
00176
00177 typename MaTRiX::element_type recp = 1.0 / *adiag;
00178
00179 ic = *icolumns + j;
00180 for (k=j+1; k<=M; k++, ++ic)
00181 *ic *= recp;
00182
00183 }
00184
00185 if (j < minMN)
00186 {
00187
00188
00189
00190
00191
00192
00193 Subscript ii, jj;
00194
00195 column_iterator ix = *icolumns + j;
00196 for (ii=j+1; ii<=M; ii++, ++ix)
00197 {
00198 row_iterator iE = A.begin_row(ii) + j;
00199 row_iterator iy = A.begin_row(j) + j;
00200 for (jj=j+1; jj<=N; jj++, ++iE, ++iy)
00201
00202 *iE -= *ix * *iy;
00203 }
00204 }
00205 }
00206 return 0;
00207 }
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224 template<class MaTRiX, class VecToR, class VecToRSubscripts> int LU_solve(const MaTRiX &A,
00225 const VecToRSubscripts &indx, VecToR &b)
00226 {
00227
00228
00229
00230
00231
00232
00233 typedef typename MaTRiX::const_row_iterator row_iterator;
00234 typedef typename VecToR::iterator vector_iterator;
00235
00236 Subscript i, ii=0, ip, j;
00237 Subscript n = b.dim();
00238 typename MaTRiX::element_type sum;
00239
00240 for (i=1; i<=n; i++)
00241 {
00242 ip=indx(i);
00243 sum=b(ip);
00244 b(ip)=b(i);
00245 if (ii)
00246 {
00247 row_iterator Aj = A.begin_row(i) + ii-1;
00248 vector_iterator bj = b.begin() + ii - 1;
00249 for (j=ii; j<=i-1; j++, ++Aj, ++bj)
00250 sum -= *Aj * *bj;
00251 }
00252 else if (sum)
00253 ii=i;
00254 b(i)=sum;
00255 }
00256
00257 typename MaTRiX::const_diag_iterator adiag = A.begin_diagonal(1, 1) + n - 1;
00258 for (i=n; i>=1; --i, --adiag)
00259 {
00260 sum=b(i);
00261 row_iterator Aj = A.begin_row(i) + i;
00262 vector_iterator bj = b.begin() + i;
00263 for (j=i+1; j<=n; j++, ++Aj, ++bj)
00264 sum -= *Aj * *bj;
00265 b(i)=sum / *adiag;
00266 }
00267 return 0;
00268 }
00269
00270 }
00271
00272 #endif
00273