00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <iostream>
00011 #include <vector>
00012 #include "MatrixSolvers.h"
00013
00014 using namespace std;
00015
00016 #ifdef USE_CLAPACK_INTERFACE
00017 extern "C" {
00018 #include <cblas.h>
00019 #include <clapack.h>
00020 };
00021 #else
00022 extern "C" void dgesvx_(char *fact, char *trans, int *n,
00023 int *nrhs, double *a, int *lda,
00024 double *af, int *ldaf, int *ipiv,
00025 char *equed, double *r, double *c,
00026 double *b, int *ldb, double *x,
00027 int *ldx, double *rcond, double *ferr,
00028 double *berr, double *work, int *iwork,
00029 int *info);
00030
00031 extern "C" void dgesv_(const int* n, const int* nrhs,
00032 double* a, int* lda, int* ipiv,
00033 double* b, int* ldb, int* info);
00034
00035 extern "C" void dgetrf_( int *m, int *n, double *a, int *lda, int *ipiv, int *info);
00036 #endif
00037
00038 extern "C" void dgesvd_(char const* jobu, char const* jobvt,
00039 int const* m, int const* n, double* a, int const* lda,
00040 double* s, double* u, int const* ldu,
00041 double* vt, int const* ldvt,
00042 double* work, int const* lwork, int* info);
00043
00044 extern "C" void dgeev_(char const*jobvl, char const*jobvr, int *n, double *A,
00045 int *lda, double *wr,double *wi, double *vl,
00046 int *ldvl, double *vr, int *ldvr, double *work, int *lwork, int *info);
00047
00048
00049
00050
00051
00052 namespace hrp {
00053
00054 static inline int max(int a, int b) { return (a >= b) ? a : b; }
00055 static inline int min(int a, int b) { return (a <= b) ? a : b; }
00056
00057
00058 int solveLinearEquationLU(dmatrix a, const dmatrix &b, dmatrix &out_x)
00059 {
00060 assert(a.rows() == a.cols() && a.cols() == b.rows() );
00061
00062 out_x = b;
00063
00064 const int n = a.rows();
00065 const int nrhs = b.cols();
00066 int info;
00067 std::vector<int> ipiv(n);
00068
00069 #ifndef USE_CLAPACK_INTERFACE
00070
00071 int lda = n;
00072 int ldb = n;
00073 dgesv_(&n, &nrhs, &(a(0,0)), &lda, &(ipiv[0]), &(out_x(0,0)), &ldb, &info);
00074 #else
00075 info = clapack_dgesv(CblasColMajor,
00076 n, nrhs, &(a(0,0)), n,
00077 &(ipiv[0]),
00078 &(out_x(0,0)),
00079 n);
00080 #endif
00081 assert(info == 0);
00082
00083 return info;
00084 }
00085
00086
00091 int solveLinearEquationLU(const dmatrix &_a, const dvector &_b, dvector &_x)
00092 {
00093 assert(_a.cols() == _a.rows() && _a.cols() == _b.size() );
00094
00095 int n = (int)_a.cols();
00096 int nrhs = 1;
00097
00098 int lda = n;
00099
00100 std::vector<int> ipiv(n);
00101
00102 int ldb = n;
00103
00104 int info;
00105
00106
00107 #ifndef USE_CLAPACK_INTERFACE
00108 char fact = 'N';
00109 char transpose = 'N';
00110
00111 double *af = new double[n*n];
00112
00113 int ldaf = n;
00114
00115 char equed = 'N';
00116
00117 double *r = new double[n];
00118 double *c = new double[n];
00119
00120 int ldx = n;
00121
00122 double rcond;
00123
00124 double *ferr = new double[nrhs];
00125 double *berr = new double[nrhs];
00126 double *work = new double[4*n];
00127
00128 int *iwork = new int[n];
00129
00130 _x.resize(n);
00131 dgesvx_(&fact, &transpose, &n, &nrhs, const_cast<double *>(&(_a(0,0))), &lda, af, &ldaf, &(ipiv[0]),
00132 &equed, r, c, const_cast<double *>(&(_b(0))), &ldb, &(_x(0)), &ldx, &rcond,
00133 ferr, berr, work, iwork, &info);
00134
00135 delete [] iwork;
00136 delete [] work;
00137 delete [] berr;
00138 delete [] ferr;
00139 delete [] c;
00140 delete [] r;
00141
00142 delete [] af;
00143 #else
00144 _x = _b;
00145 info = clapack_dgesv(CblasColMajor,
00146 n, nrhs, const_cast<double *>(&(a(0,0))), lda, &(ipiv[0]),
00147 &(_x(0)), ldb);
00148 #endif
00149
00150 return info;
00151 }
00152
00157 int solveLinearEquationSVD(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
00158 {
00159 const int m = _a.rows();
00160 const int n = _a.cols();
00161 assert( m == static_cast<int>(_b.size()) );
00162 _x.resize(n);
00163
00164 int i, j;
00165 char jobu = 'A';
00166 char jobvt = 'A';
00167
00168 int max_mn = max(m,n);
00169 int min_mn = min(m,n);
00170
00171 dmatrix a(m,n);
00172 a = _a;
00173
00174 int lda = m;
00175 double *s = new double[max_mn];
00176 int ldu = m;
00177 double *u = new double[ldu*m];
00178 int ldvt = n;
00179 double *vt = new double[ldvt*n];
00180
00181 int lwork = max(3*min_mn+max_mn, 5*min_mn);
00182 double *work = new double[lwork];
00183 int info;
00184
00185 for(i = 0; i < max_mn; i++) s[i] = 0.0;
00186
00187 dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
00188 &lwork, &info);
00189
00190 double tmp;
00191
00192 double smin, smax=0.0;
00193 for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
00194 smin = smax*_sv_ratio;
00195 for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
00196
00197 double *utb = new double[m];
00198
00199 for (j = 0; j < m; j++){
00200 tmp = 0;
00201 if (s[j]){
00202 for (i = 0; i < m; i++) tmp += u[j*m+i] * _b(i);
00203 tmp /= s[j];
00204 }
00205 utb[j] = tmp;
00206 }
00207
00208
00209 for (j = 0; j < n; j++){
00210 tmp = 0;
00211 for (i = 0; i < n; i++){
00212 if(s[i]) tmp += utb[i] * vt[j*n+i];
00213 }
00214 _x(j) = tmp;
00215 }
00216
00217 delete [] utb;
00218 delete [] work;
00219 delete [] vt;
00220 delete [] s;
00221 delete [] u;
00222
00223 return info;
00224 }
00225
00226
00230 int solveLinearEquation(const dmatrix &_a, const dvector &_b, dvector &_x, double _sv_ratio)
00231 {
00232 if(_a.cols() == _a.rows())
00233 return solveLinearEquationLU(_a, _b, _x);
00234 else
00235 return solveLinearEquationSVD(_a, _b, _x, _sv_ratio);
00236 }
00237
00238
00243 int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
00244 {
00245 int i, j, k;
00246 char jobu = 'A';
00247 char jobvt = 'A';
00248 int m = (int)_a.rows();
00249 int n = (int)_a.cols();
00250 int max_mn = max(m,n);
00251 int min_mn = min(m,n);
00252
00253 dmatrix a(m,n);
00254 a = _a;
00255
00256 int lda = m;
00257 double *s = new double[max_mn];
00258 int ldu = m;
00259 double *u = new double[ldu*m];
00260 int ldvt = n;
00261 double *vt = new double[ldvt*n];
00262 int lwork = max(3*min_mn+max_mn, 5*min_mn);
00263 double *work = new double[lwork];
00264 int info;
00265
00266 for(i = 0; i < max_mn; i++) s[i] = 0.0;
00267
00268 dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
00269 &lwork, &info);
00270
00271
00272 double smin, smax=0.0;
00273 for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
00274 smin = smax*_sv_ratio;
00275 for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
00276
00277
00278
00279 for (j = 0; j < m; j++){
00280 if (s[j]){
00281 for (i = 0; i < m; i++) u[j*m+i] /= s[j];
00282 }
00283 else {
00284 for (i = 0; i < m; i++) u[j*m+i] = 0.0;
00285 }
00286 }
00287
00288
00289 _a_pseu.resize(n,m);
00290 for(j = 0; j < n; j++){
00291 for(i = 0; i < m; i++){
00292 _a_pseu(j,i) = 0.0;
00293 for(k = 0; k < min_mn; k++){
00294 if(s[k]) _a_pseu(j,i) += vt[j*n+k] * u[k*m+i];
00295 }
00296 }
00297 }
00298
00299 delete [] work;
00300 delete [] vt;
00301 delete [] s;
00302 delete [] u;
00303
00304 return info;
00305 }
00306
00307
00308 int calcEigenVectors(const dmatrix &_a, dmatrix &_evec, dvector &_eval)
00309 {
00310 assert( _a.cols() == _a.rows() );
00311
00312 typedef dmatrix mlapack;
00313 typedef dvector vlapack;
00314
00315 mlapack a = _a;
00316 mlapack evec = _evec;
00317 vlapack eval = _eval;
00318
00319 int n = (int)_a.cols();
00320
00321 double *wi = new double[n];
00322 double *vl = new double[n*n];
00323 double *work = new double[4*n];
00324
00325 int lwork = 4*n;
00326 int info;
00327
00328 dgeev_("N","V", &n, &(a(0,0)), &n, &(eval(0)), wi, vl, &n, &(evec(0,0)), &n, work, &lwork, &info);
00329
00330 _evec = evec.transpose();
00331 _eval = eval;
00332
00333 delete [] wi;
00334 delete [] vl;
00335 delete [] work;
00336
00337 return info;
00338 }
00339
00340
00341
00342 int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w) {
00343
00344
00345
00346
00347
00348
00349 const int c = _a.rows();
00350 const int n = _a.cols();
00351
00352 if ( _w.cols() != n || _w.rows() != n ) {
00353 _w = dmatrix::Identity(n, n);
00354 }
00355
00356 dmatrix at = _a.transpose();
00357 dmatrix a1(c, c);
00358 a1 = (_a * _w * at + _sr_ratio * dmatrix::Identity(c,c)).inverse();
00359
00360
00361
00362 _a_sr = _w * at * a1;
00363
00364 return 0;
00365 }
00366
00367
00368 double det(const dmatrix &_a)
00369 {
00370 assert( _a.cols() == _a.rows() );
00371
00372 typedef dmatrix mlapack;
00373 mlapack a = _a;
00374
00375 int info;
00376 int n = a.cols();
00377 int lda = n;
00378 std::vector<int> ipiv(n);
00379
00380 #ifdef USE_CLAPACK_INTERFACE
00381 info = clapack_dgetrf(CblasColMajor,
00382 n, n, &(a(0,0)), lda, &(ipiv[0]));
00383 #else
00384 dgetrf_(&n, &n, &a(0,0), &lda, &(ipiv[0]), &info);
00385 #endif
00386
00387 double det=1.0;
00388
00389 for(int i=0; i < n-1; i++)
00390 if(ipiv[i] != i+1) det = -det;
00391
00392 for(int i=0; i < n; i++) det *= a(i,i);
00393
00394 assert(info == 0);
00395
00396 return det;
00397 }
00398
00399 }