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 #ifndef SCPPNT_QR_H
00044 #define SCPPNT_QR_H
00045
00046 #include <cmath>
00047 #ifdef BOOST_NO_STDC_NAMESPACE
00048 namespace std
00049 { using ::fabs; using ::sqrt;}
00050 #endif
00051
00052 #ifdef SCPPNT_NO_DIR_PREFIX
00053 #include "scppnt.h"
00054 #include "scppnt_error.h"
00055 #include "scppnt_math.h"
00056 #else
00057 #include "scppnt/scppnt.h"
00058 #include "scppnt/scppnt_error.h"
00059 #include "scppnt/scppnt_math.h"
00060 #endif
00061
00062 namespace SCPPNT
00063 {
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093 template<class MaTRiX, class Vector> int QR_factor(MaTRiX &A, Vector& C, Vector &D)
00094 {
00095
00096 if (A.lbound() != 1)
00097 throw BoundsError("SCPPNT::QR_factor");
00098
00099
00100 if (C.lbound() != 1)
00101 throw BoundsError("SCPPNT::QR_factor");
00102
00103
00104 if (D.lbound() != 1)
00105 throw BoundsError("SCPPNT::R_factor");
00106
00107 Subscript M = A.num_rows();
00108 Subscript N = A.num_columns();
00109
00110
00111 if (M != N)
00112 throw BoundsError("QR_factor");
00113
00114 Subscript i, j, k;
00115 typename MaTRiX::element_type eta, sigma, sum;
00116
00117
00118
00119 if (N != C.size())
00120 C.newsize(N);
00121 if (N != D.size())
00122 D.newsize(N);
00123
00124 typename MaTRiX::diag_iterator diagA = A.begin_diagonal(1, 1);
00125 typename Vector::iterator iC = C.begin();
00126 typename Vector::iterator iD = D.begin();
00127 typename MaTRiX::columns_iterator columnsA = A.begin_columns();
00128 typename Vector::iterator dn = D.begin() + N - 1;
00129 typename MaTRiX::row_iterator ann = A.begin_row(N) + N - 1;
00130
00131 for (k=1; k<N; k++, ++columnsA, ++iC, ++iD)
00132 {
00133
00134
00135 eta = 0;
00136 typename MaTRiX::column_iterator columnA = *columnsA + k - 1;
00137
00138 for (i=k; i<=N; i++, ++columnA)
00139 {
00140 double absA = std::fabs(*columnA);
00141 eta = (absA > eta ? absA : eta );
00142 }
00143
00144 if (eta == 0)
00145 {
00146
00147 return 1;
00148 }
00149
00150
00151
00152 columnA = *columnsA + k - 1;
00153 for (i=k; i<=N; i++, ++columnA)
00154 *columnA /= eta;
00155
00156 sum = 0;
00157 columnA = *columnsA + k - 1;
00158 for (i=k; i<=N; i++, ++columnA)
00159 sum += *columnA * *columnA;
00160
00161 sigma = sign(*diagA) * std::sqrt(sum);
00162
00163 *diagA += sigma;
00164 *iC = sigma * *diagA;
00165 ++diagA;
00166 *iD = -eta * sigma;
00167
00168 typename MaTRiX::columns_iterator columnsj = columnsA+1;
00169 for (j=k+1; j<=N; j++, ++columnsj)
00170 {
00171 sum = 0;
00172 columnA = *columnsA + k - 1;
00173 typename MaTRiX::column_iterator columnj = *columnsj + k - 1;
00174
00175 for (i=k; i<=N; i++, ++columnj, ++columnA)
00176 sum += *columnA * *columnj;
00177
00178 sum /= *iC;
00179
00180 columnA = *columnsA + k - 1;
00181 columnj = *columnsj + k - 1;
00182 for (i=k; i<=N; i++, ++columnA, ++columnj)
00183 *columnj -= sum * *columnA;
00184 }
00185
00186 *dn = *ann;
00187 }
00188 return 0;
00189 }
00190
00191
00192 template<class MaTRiX, class Vector> int R_solve(const MaTRiX &A, Vector &D, Vector &b)
00193 {
00194 char *funcname = "SCPPNT::R_solve";
00195
00196
00197 if (A.lbound() != 1)
00198 throw BoundsError(funcname);
00199
00200
00201 if (D.lbound() != 1)
00202 throw BoundsError(funcname);
00203
00204
00205 if (b.lbound() != 1)
00206 throw BoundsError(funcname);
00207
00208 Subscript i, j;
00209 Subscript N = A.num_rows();
00210
00211 if (N != A.num_columns())
00212 throw BoundsError(funcname);
00213 if (N != D.dim())
00214 throw BoundsError(funcname);
00215 if (N != b.dim())
00216 throw BoundsError(funcname);
00217
00218 typename MaTRiX::element_type sum;
00219
00220 if (D(N) == 0)
00221 return 1;
00222
00223 b(N) = b(N) / D(N);
00224
00225 typename MaTRiX::const_rows_iterator rowsA = A.begin_rows() + N - 2;
00226 typename Vector::iterator iD = D.begin() + N - 2;
00227 typename Vector::iterator ib = b.begin() + N - 2;
00228
00229 for (i=N-1; i>=1; i--, --iD, --ib)
00230 {
00231 if (D(i) == 0)
00232 return 1;
00233
00234 sum = 0;
00235 typename MaTRiX::const_row_iterator rowA = *rowsA + i;
00236 typename Vector::iterator jb = ib + 1;
00237
00238 for (j=i+1; j<=N; j++, ++rowA, ++jb)
00239 sum += *rowA * *jb;
00240
00241 --rowsA;
00242 *ib = (*ib - sum) / *iD;
00243 }
00244 return 0;
00245 }
00246
00247
00248 template<class MaTRiX, class Vector> int QR_solve(const MaTRiX &A, const Vector &c,
00249 Vector &d, Vector &b)
00250 {
00251 char *funcname = "SCPPNT::QR_solve";
00252
00253
00254 if (A.lbound() != 1)
00255 throw BoundsError(funcname);
00256
00257 if (c.lbound() != 1)
00258 throw BoundsError(funcname);
00259 if (d.lbound() != 1)
00260 throw BoundsError(funcname);
00261
00262 Subscript N=A.num_rows();
00263
00264 if (N != A.num_columns())
00265 throw BoundsError(funcname);
00266 if (N != c.dim())
00267 throw BoundsError(funcname);
00268 if (N != d.dim())
00269 throw BoundsError(funcname);
00270 if (N != b.dim())
00271 throw BoundsError(funcname);
00272
00273 Subscript i, j;
00274 typename MaTRiX::element_type sum, tau;
00275 typename MaTRiX::const_columns_iterator columnsA = A.begin_columns();
00276 typename Vector::const_iterator cj = c.begin();
00277 for (j=1; j<N; j++, ++columnsA, ++cj)
00278 {
00279
00280 sum = 0;
00281 typename MaTRiX::const_column_iterator columnA = *columnsA + j - 1;
00282 typename Vector::iterator bi = b.begin() + j - 1;
00283
00284 for (i=j; i<=N; i++, ++columnA, ++bi)
00285 sum += *columnA * *bi;
00286
00287 if (*cj == 0)
00288 return 1;
00289 tau = sum / *cj;
00290 bi = b.begin() + j - 1;
00291 columnA = *columnsA + j - 1;
00292
00293 for (i=j; i<=N; i++, ++bi, ++columnA)
00294 *bi -= tau * *columnA;
00295 }
00296 return R_solve(A, d, b);
00297 }
00298
00299 }
00300
00301 #endif
00302