fMatrix.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00016 #include "fMatrix.h"
00017 
00021 fMat::fMat(const fVec& ini)
00022 {
00023         p_data = 0;
00024         n_col = 0;
00025         n_row = 0;
00026         m_info = 0;
00027         int k = ini.size();
00028         resize(k, 1);
00029         dims_copy(ini.p_data, p_data, k);
00030 }
00031 
00032 void fMat::get_submat(int row_start, int col_start, const fMat& allM)
00033 {
00034         assert(row_start >= 0 && col_start >= 0 && 
00035                    row_start+n_row <= allM.row() &&
00036                    col_start+n_col <= allM.col());
00037         for(int i=0; i<n_col; i++)
00038         {
00039                 double* m = allM.data() + allM.row()*(i+col_start) + row_start;
00040                 double* p = p_data + n_row*i;
00041                 for(int j=0; j<n_row; j++, p++, m++)
00042                 {
00043                         *p = *m;
00044                 }
00045         }
00046 }
00047 
00048 void fMat::set_submat(int row_start, int col_start, const fMat& subM)
00049 {
00050         int row_span = subM.row(), col_span = subM.col();
00051         assert(row_start >= 0 && col_start >= 0 &&
00052                    row_start+row_span <= n_row &&
00053                    col_start+col_span <= n_col);
00054         for(int i=0; i<col_span; i++)
00055         {
00056                 double* m = subM.data() + i*subM.row();
00057                 double* p = p_data + n_row*(i+col_start) + row_start;
00058                 for(int j=0; j<row_span; j++, p++, m++)
00059                 {
00060                         *p = *m;
00061                 }
00062         }
00063 }
00064 
00065 void fVec::get_subvec(int start, const fVec& allV)
00066 {
00067         assert(start >= 0 && start+n_row <= allV.size());
00068         double* m = allV.data() + start;
00069         double* p = p_data;
00070         for(int i=0; i<n_row; i++, p++, m++)
00071         {
00072                 *p = *m;
00073         }
00074 }
00075 
00076 void fVec::set_subvec(int start, const fVec& subV)
00077 {
00078         int row_span = subV.size();
00079         assert(start >= 0 && start+row_span <= n_row);
00080         double* m = subV.data();
00081         double* p = p_data + start;
00082         for(int i=0; i<row_span; i++, p++, m++)
00083         {
00084                 *p = *m;
00085         }
00086 }
00087 
00088 void fMat::symmetric(char t)
00089 {
00090         assert(n_row == n_col);
00091         double *p1, *p2;
00092         if(t == 'A')
00093         {
00094                 // use average
00095                 double sum;
00096                 for(int i=0; i<n_row; i++)
00097                 {
00098                         int j;
00099                         for(j=0, p1=p_data+i, p2=p_data+n_row*i; j<i; j++, p1+=n_row, p2++)
00100                         {
00101                                 sum = *p1 + *p2;
00102                                 sum *= 0.5;
00103                                 *p1 = sum;
00104                                 *p2 = sum;
00105                         }
00106                 }
00107         }
00108         else if(t == 'U')
00109         {
00110                 // use upper triangle
00111                 for(int i=0; i<n_row; i++)
00112                 {
00113                         int j;
00114                         for(j=0, p1=p_data+i, p2=p_data+n_row*i; j<i; j++, p1+=n_row, p2++)
00115                                 *p1 = *p2;
00116                 }
00117         }
00118         else if(t == 'L')
00119         {
00120                 // use lower triangle
00121                 for(int i=0; i<n_row; i++)
00122                 {
00123                         int j;
00124                         for(j=0, p1=p_data+i, p2=p_data+n_row*i; j<i; j++, p1+=n_row, p2++)
00125                                 *p2 = *p1;
00126                 }
00127         }
00128         else
00129         {
00130                 assert(0);
00131         }
00132 }
00133 
00134 /*
00135  * LU decomposition
00136  */
00137 fMat lineq(const fMat& A, const fMat& b)
00138 {
00139         assert(A.row() == b.row());
00140         fMat x(A.col(), b.col());
00141         double *a, *bb, *xx;
00142         a = A.data();
00143         bb = b.data();
00144         xx = x.data();
00145         int n = A.row(), nrhs = b.col();
00146         x.m_info = dims_dgesvx(a, xx, bb, n, nrhs);
00147         return x;
00148 }
00149 
00150 int fMat::lineq(const fMat& A, const fMat& b)
00151 {
00152         assert(n_row == A.col() && n_col == b.col() && A.row() == b.row());
00153         double *a, *bb, *xx;
00154         int ret;
00155         a = A.data();
00156         bb = b.data();
00157         xx = p_data;
00158         int n = A.row(), nrhs = b.col();
00159         ret = dims_dgesvx(a, xx, bb, n, nrhs);
00160         return ret;
00161 }
00162 
00163 int fVec::lineq(const fMat& A, const fVec& b)
00164 {
00165         assert(n_row == A.col() && A.row() == b.row());
00166         double *a, *bb, *xx;
00167         int ret;
00168         a = A.data();
00169         bb = b.data();
00170         xx = p_data;
00171         int n = A.row(), nrhs = 1;
00172         ret = dims_dgesvx(a, xx, bb, n, nrhs);
00173         return ret;
00174 }
00175 
00176 fMat inv(const fMat& mat)
00177 {
00178         assert(mat.row() == mat.col());
00179         fMat ret(mat.row(), mat.col());
00180         double *a, *b, *x;
00181         int n = mat.row(), nrhs = mat.row();
00182         fMat I(mat.row(), mat.col());
00183         I.identity();
00184         a = mat.data();
00185         x = ret.data();
00186         b = I.data();
00187         ret.m_info = dims_dgesvx(a, x, b, n, nrhs);
00188         return ret;
00189 }
00190 
00191 fMat p_inv(const fMat& mat)
00192 {
00193         fMat ret(mat.col(), mat.row());
00194         fMat tmat;
00195         fMat MM, MMinv;
00196 
00197         if(mat.row() < mat.col())
00198         {
00199                 tmat.resize(mat.n_col, mat.n_row);
00200                 tmat.tran(mat);
00201                 MM.resize(mat.row(), mat.row());
00202                 MMinv.resize(mat.row(), mat.row());
00203                 MM.mul(mat, tmat);
00204                 MMinv.inv(MM);
00205                 ret.mul(tmat, MMinv);
00206                 ret.m_info = MMinv.m_info;
00207         }
00208         else if(mat.row() > mat.col())
00209         {
00210                 tmat.resize(mat.n_col, mat.n_row);
00211                 tmat.tran(mat);
00212                 MM.resize(mat.col(), mat.col());
00213                 MMinv.resize(mat.col(), mat.col());
00214                 MM.mul(tmat, mat);
00215                 MMinv.inv(MM);
00216                 ret.mul(MMinv, tmat);
00217                 ret.m_info = MMinv.m_info;
00218         }
00219         else
00220         {
00221                 ret.inv(mat);
00222         }
00223         return ret;
00224 }
00225 
00226 fMat sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k)
00227 {
00228         assert(w_err.row() == mat.row() && w_norm.row() == mat.col());
00229         fMat ret(mat.n_col, mat.n_row);
00230 
00231         fMat Jhat, tJhat;
00232         Jhat.resize(mat.row(), mat.col());
00233         tJhat.resize(mat.col(), mat.row());
00234         int i, j, r = mat.n_row, c = mat.n_col;
00235         Jhat.set(mat);
00236         for(i=0; i<r; i++)
00237         {
00238                 if(w_err(i) < 1e-8) 
00239                 {
00240                         w_err(i) = 1.0;
00241                 }
00242                 for(j=0; j<c; j++)
00243                 {
00244                         Jhat(i, j) *= w_err(i);
00245                 }
00246         }
00247         for(j=0; j<c; j++)
00248         {
00249                 if(w_norm(j) < 1e-8) 
00250                 {
00251                         w_norm(j) = 1.0;
00252                 }
00253                 for(i=0; i<r; i++)
00254                 {
00255                         Jhat(i, j) /= w_norm(j);
00256                 }
00257         }
00258         tJhat.tran(Jhat);
00259         static fMat tmp, minv;
00260         if(mat.n_row < mat.n_col)
00261         {
00262                 tmp.resize(mat.n_row, mat.n_row);
00263                 minv.resize(mat.n_row, mat.n_row);
00264                 tmp.mul(Jhat, tJhat);
00265                 for(i=0; i<mat.n_row; i++)
00266                 {
00267                         tmp(i, i) += k;
00268                 }
00269                 minv.inv(tmp);
00270                 ret.mul(tJhat, minv);
00271                 ret.m_info = minv.m_info;
00272         }
00273         else
00274         {
00275                 tmp.resize(mat.n_col, mat.n_col);
00276                 minv.resize(mat.n_col, mat.n_col);
00277                 tmp.mul(tJhat, Jhat);
00278                 for(i=0; i<mat.n_col; i++)
00279                 {
00280                         tmp(i, i) += k;
00281                 }
00282                 minv.inv(tmp);
00283                 ret.mul(minv, tJhat);
00284                 ret.m_info = minv.m_info;
00285         }
00286         for(i=0; i<mat.n_col; i++)
00287         {
00288                 for(j=0; j<mat.n_row; j++)
00289                 {
00290                         ret(i, j) *= w_err(j) / w_norm(i);
00291                 }
00292         }
00293         return ret;
00294 }
00295 
00296 int fMat::inv(const fMat& mat)
00297 {
00298         assert(n_row == n_col && n_row == mat.n_row && n_col == mat.n_col);
00299         double *a, *b, *x;
00300         int n = mat.row(), nrhs = mat.row();
00301         fMat I(mat.row(), mat.col());
00302         I.identity();
00303         a = mat.data();
00304         x = p_data;
00305         b = I.data();
00306         {
00307                 m_info = dims_dgesvx(a, x, b, n, nrhs);
00308         }
00309         return m_info;
00310 }
00311 
00312 int fMat::p_inv(const fMat& mat)
00313 {
00314         assert(n_row == mat.n_col && n_col == mat.n_row);
00315         fMat tmat;
00316         fMat MM, MMinv;
00317 
00318         if(mat.row() < mat.col())
00319         {
00320                 tmat.resize(mat.n_col, mat.n_row);
00321                 tmat.tran(mat);
00322                 MM.resize(mat.row(), mat.row());
00323                 MMinv.resize(mat.row(), mat.row());
00324                 MM.mul_tran(mat, false);
00325                 MMinv.inv(MM);
00326                 mul(tmat, MMinv);
00327                 m_info = MMinv.m_info;
00328         }
00329         else if(mat.row() > mat.col())
00330         {
00331                 tmat.resize(mat.n_col, mat.n_row);
00332                 tmat.tran(mat);
00333                 MM.resize(mat.col(), mat.col());
00334                 MMinv.resize(mat.col(), mat.col());
00335                 MM.mul_tran(mat, true);
00336                 MMinv.inv(MM);
00337                 mul(MMinv, tmat);
00338                 m_info = MMinv.m_info;
00339         }
00340         else inv(mat);
00341         return m_info;
00342 }
00343 
00344 int fMat::sr_inv(const fMat& mat, fVec& w_err, fVec& w_norm, double k)
00345 {
00346         assert(n_col == mat.n_row && n_row == mat.n_col &&
00347                    w_err.row() == mat.row() && w_norm.row() == mat.col());
00348         fMat Jhat, tJhat;
00349         Jhat.resize(mat.row(), mat.col());
00350         tJhat.resize(mat.col(), mat.row());
00351         int i, j, r = mat.n_row, c = mat.n_col;
00352         Jhat.set(mat);
00353         for(i=0; i<r; i++)
00354         {
00355                 if(w_err(i) < 1e-8) 
00356                 {
00357                         w_err(i) = 1.0;
00358                 }
00359                 for(j=0; j<c; j++)
00360                 {
00361                         Jhat(i, j) *= w_err(i);
00362                 }
00363         }
00364         for(j=0; j<c; j++)
00365         {
00366                 if(w_norm(j) < 1e-8) 
00367                 {
00368                         w_norm(j) = 1.0;
00369                 }
00370                 for(i=0; i<r; i++)
00371                 {
00372                         Jhat(i, j) /= w_norm(j);
00373                 }
00374         }
00375         tJhat.tran(Jhat);
00376         fMat tmp, minv;
00377         if(mat.n_row < mat.n_col)
00378         {
00379                 tmp.resize(mat.n_row, mat.n_row);
00380                 minv.resize(mat.n_row, mat.n_row);
00381                 tmp.mul(Jhat, tJhat);
00382                 for(i=0; i<mat.n_row; i++)
00383                 {
00384                         tmp(i, i) += k;
00385                 }
00386                 minv.inv(tmp);
00387                 mul(tJhat, minv);
00388                 m_info = minv.m_info;
00389         }
00390         else
00391         {
00392                 tmp.resize(mat.n_col, mat.n_col);
00393                 minv.resize(mat.n_col, mat.n_col);
00394                 tmp.mul(tJhat, Jhat);
00395                 for(i=0; i<mat.n_col; i++)
00396                 {
00397                         tmp(i, i) += k;
00398                 }
00399                 minv.inv(tmp);
00400                 mul(minv, tJhat);
00401                 m_info = minv.m_info;
00402         }
00403         for(i=0; i<mat.n_col; i++)
00404         {
00405                 for(j=0; j<mat.n_row; j++)
00406                 {
00407                         (*this)(i, j) *= w_err(j) / w_norm(i);
00408                 }
00409         }
00410         return m_info;
00411 }
00412 
00413 /*
00414  * SVD
00415  */
00416 fMat lineq_svd(const fMat& A, const fMat& b, int lwork)
00417 {
00418         assert(A.row() == b.row());
00419         fMat x(A.col(), b.col());
00420         double *a, *bb, *xx, *s;
00421         a = A.data();
00422         bb = b.data();
00423         xx = x.data();
00424         int m = A.row(), n = A.col(), nrhs = b.col(), rank;
00425         if(m < n) s = new double[m];
00426         else s = new double[n];
00427         dims_dgelss(a, xx, bb, m, n, nrhs, s, &rank, lwork);
00428         delete[] s;
00429         return x;
00430 }
00431 
00432 int fMat::lineq_svd(const fMat& A, const fMat& b, int lwork)
00433 {
00434         assert(n_row == A.col() && n_col == b.col() && A.row() == b.row());
00435         if(A.col() == 1)
00436         {
00437                 fMat AA(1,1);
00438                 fMat Ainv(A.col(), A.row());
00439                 AA.mul_tran(A, true);
00440                 Ainv.tran(A);
00441                 Ainv /= AA(0,0);
00442                 mul(Ainv, b);
00443                 return 0;
00444         }
00445         else if(A.row() == 1)
00446         {
00447                 fMat AA(1,1);
00448                 fMat Ainv(A.col(), A.row());
00449                 AA.mul_tran(A, false);
00450                 Ainv.tran(A);
00451                 Ainv /= AA(0,0);
00452                 mul(Ainv, b);
00453                 return 0;
00454         }
00455         double *a, *bb, *xx, *s;
00456         int ret;
00457         a = A.data();
00458         bb = b.data();
00459         xx = p_data;
00460         int m = A.row(), n = A.col(), nrhs = b.col(), rank;
00461         if(m<n) s = new double[m];
00462         else s = new double[n];
00463         ret = dims_dgelss(a, xx, bb, m, n, nrhs, s, &rank, lwork);
00464         delete[] s;
00465         return ret;
00466 }
00467 
00468 int fVec::lineq_svd(const fMat& A, const fVec& b, int lwork)
00469 {
00470         assert(n_row == A.col() && A.row() == b.row());
00471         double *a, *bb, *xx, *s;
00472         int ret;
00473         a = A.data();
00474         bb = b.data();
00475         xx = p_data;
00476         int m = A.row(), n = A.col(), nrhs = 1, rank;
00477         if(m<n) s = new double[m];
00478         else s = new double[n];
00479         ret = dims_dgelss(a, xx, bb, m, n, nrhs, s, &rank, lwork);
00480         delete[] s;
00481         return ret;
00482 }
00483 
00484 fMat inv_svd(const fMat& mat, int lwork)
00485 {
00486         assert(mat.row() == mat.col());
00487         fMat ret(mat.row(), mat.col());
00488         double *a, *b, *x, *s;
00489         int m = mat.row(), n = mat.col(), nrhs = mat.row(), rank;
00490         fMat I(mat.row(), mat.col());
00491         I.identity();
00492         a = mat.data();
00493         x = ret.data();
00494         b = I.data();
00495         s = new double[n];
00496         ret.m_info = dims_dgelss(a, x, b, m, n, nrhs, s, &rank, lwork);
00497         delete[] s;
00498         return ret;
00499 }
00500 
00501 fMat p_inv_svd(const fMat& mat, int lwork)
00502 {
00503         fMat ret(mat.col(), mat.row());
00504         static fMat tmat;
00505         static fMat MM, MMinv;
00506 
00507         if(mat.row() < mat.col())
00508         {
00509                 tmat.resize(mat.n_col, mat.n_row);
00510                 tmat.tran(mat);
00511                 MM.resize(mat.row(), mat.row());
00512                 MMinv.resize(mat.row(), mat.row());
00513                 MM.mul(mat, tmat);
00514                 MMinv.inv_svd(MM, lwork);
00515                 ret.mul(MMinv, tmat);
00516                 ret.m_info = MMinv.m_info;
00517         }
00518         else if(mat.row() > mat.col())
00519         {
00520                 tmat.resize(mat.n_col, mat.n_row);
00521                 tmat.tran(mat);
00522                 MM.resize(mat.col(), mat.col());
00523                 MMinv.resize(mat.col(), mat.col());
00524                 MM.mul(tmat, mat);
00525                 MMinv.inv_svd(MM, lwork);
00526                 ret.mul(tmat, MMinv);
00527                 ret.m_info = MMinv.m_info;
00528         }
00529         else
00530         {
00531                 ret.inv_svd(mat, lwork);
00532         }
00533         return ret;
00534 }
00535 
00536 fMat sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork)
00537 {
00538         assert(w_err.row() == mat.row() && w_norm.row() == mat.col());
00539         fMat ret(mat.n_col, mat.n_row);
00540         static fMat Jhat, tJhat;
00541         Jhat.resize(mat.row(), mat.col());
00542         tJhat.resize(mat.col(), mat.row());
00543         int i, j;
00544         Jhat.set(mat);
00545         for(i=0; i<mat.n_row; i++)
00546         {
00547                 if(w_err(i) < 1e-8) 
00548                 {
00549                         w_err(i) = 1.0;
00550                 }
00551                 for(j=0; j<mat.n_col; j++)
00552                 {
00553                         Jhat(i, j) *= w_err(i);
00554                 }
00555         }
00556         for(j=0; j<mat.n_col; j++)
00557         {
00558                 if(w_norm(j) < 1e-8) 
00559                 {
00560                         w_norm(j) = 1.0;
00561                 }
00562                 for(i=0; i<mat.n_row; i++)
00563                 {
00564                         Jhat(i, j) *= w_norm(j);
00565                 }
00566         }
00567         tJhat.tran(Jhat);
00568         static fMat tmp, minv;
00569         if(mat.n_row < mat.n_col)
00570         {
00571                 tmp.resize(mat.n_row, mat.n_row);
00572                 minv.resize(mat.n_row, mat.n_row);
00573                 tmp.mul(Jhat, tJhat);
00574                 for(i=0; i<mat.n_row; i++)
00575                 {
00576                         tmp(i, i) += k;
00577                 }
00578                 minv.inv_svd(tmp, lwork);
00579                 ret.mul(tJhat, minv);
00580                 ret.m_info = minv.m_info;
00581         }
00582         else
00583         {
00584                 tmp.resize(mat.n_col, mat.n_col);
00585                 minv.resize(mat.n_col, mat.n_col);
00586                 tmp.mul(tJhat, Jhat);
00587                 for(i=0; i<mat.n_col; i++)
00588                 {
00589                         tmp(i, i) += k;
00590                 }
00591                 minv.inv_svd(tmp, lwork);
00592                 ret.mul(minv, tJhat);
00593                 ret.m_info = minv.m_info;
00594         }
00595         for(i=0; i<mat.n_col; i++)
00596         {
00597                 for(j=0; j<mat.n_row; j++)
00598                 {
00599                         ret(i, j) *= w_err(j) / w_norm(i);
00600                 }
00601         }
00602         return ret;
00603 }
00604 
00605 int fMat::inv_svd(const fMat& mat, int lwork)
00606 {
00607         assert(n_row == mat.n_row && n_col == mat.n_col);
00608         if(n_col == 1)
00609         {
00610                 p_data[0] = 1/mat(0, 0);
00611                 m_info = 0;
00612                 return m_info;
00613         }
00614         double *a, *b, *x, *s;
00615         int m = mat.row(), n = mat.col(), nrhs = mat.row(), rank;
00616         fMat I(mat.row(), mat.col());
00617         I.identity();
00618         a = mat.data();
00619         x = p_data;
00620         b = I.data();
00621         if(m<n) s = new double[m];
00622         else s = new double[n];
00623         m_info = dims_dgelss(a, x, b, m, n, nrhs, s, &rank, lwork);
00624         delete[] s;
00625         return m_info;
00626 }
00627 
00628 int fMat::p_inv_svd(const fMat& mat, int lwork)
00629 {
00630         assert(n_row == mat.n_col && n_col == mat.n_row);
00631         fMat tmat;
00632         fMat MM, MMinv;
00633 
00634         if(mat.row() < mat.col())
00635         {
00636                 tmat.resize(mat.n_col, mat.n_row);
00637                 tmat.tran(mat);
00638                 MM.resize(mat.row(), mat.row());
00639                 MMinv.resize(mat.row(), mat.row());
00640 //              MM.mul(mat, tmat);
00641                 MM.mul_tran(mat, false);
00642                 MMinv.inv_svd(MM, lwork);
00643                 mul(tmat, MMinv);
00644                 m_info = MMinv.m_info;
00645         }
00646         else if(mat.row() > mat.col())
00647         {
00648                 tmat.resize(mat.n_col, mat.n_row);
00649                 tmat.tran(mat);
00650                 MM.resize(mat.col(), mat.col());
00651                 MMinv.resize(mat.col(), mat.col());
00652 //              MM.mul(tmat, mat);
00653                 MM.mul_tran(mat, true);
00654                 MMinv.inv_svd(MM, lwork);
00655                 mul(MMinv, tmat);
00656                 m_info = MMinv.m_info;
00657         }
00658         else inv_svd(mat, lwork);
00659         return m_info;
00660 }
00661 
00662 int fMat::sr_inv_svd(const fMat& mat, fVec& w_err, fVec& w_norm, double k, int lwork)
00663 {
00664         assert(n_col == mat.n_row && n_row == mat.n_col &&
00665                    w_err.row() == mat.row() && w_norm.row() == mat.col());
00666         fMat Jhat, tJhat;
00667         Jhat.resize(mat.row(), mat.col());
00668         tJhat.resize(mat.col(), mat.row());
00669         int i, j;
00670         Jhat.set(mat);
00671         for(i=0; i<mat.n_row; i++)
00672         {
00673                 if(w_err(i) < 1e-8) 
00674                 {
00675                         w_err(i) = 1.0;
00676                 }
00677                 for(j=0; j<mat.n_col; j++)
00678                 {
00679                         Jhat(i, j) *= w_err(i);
00680                 }
00681         }
00682         for(j=0; j<mat.n_col; j++)
00683         {
00684                 if(w_norm(j) < 1e-8) 
00685                 {
00686                         w_norm(j) = 1.0;
00687                 }
00688                 for(i=0; i<mat.n_row; i++)
00689                 {
00690                         Jhat(i, j) *= w_norm(j);
00691                 }
00692         }
00693         tJhat.tran(Jhat);
00694         fMat tmp, minv;
00695         if(mat.n_row < mat.n_col)
00696         {
00697                 tmp.resize(mat.n_row, mat.n_row);
00698                 minv.resize(mat.n_row, mat.n_row);
00699 //              tmp.mul_tran(Jhat, false);
00700                 tmp.mul(Jhat, tJhat);
00701                 for(i=0; i<mat.n_row; i++)
00702                 {
00703                         tmp(i, i) += k;
00704                 }
00705                 minv.inv_svd(tmp, lwork);
00706                 mul(tJhat, minv);
00707                 m_info = minv.m_info;
00708         }
00709         else
00710         {
00711                 tmp.resize(mat.n_col, mat.n_col);
00712                 minv.resize(mat.n_col, mat.n_col);
00713 //              tmp.mul_tran(Jhat, true);
00714                 tmp.mul(tJhat, Jhat);
00715                 for(i=0; i<mat.n_col; i++)
00716                 {
00717                         tmp(i, i) += k;
00718                 }
00719                 minv.inv_svd(tmp, lwork);
00720                 mul(minv, tJhat);
00721                 m_info = minv.m_info;
00722         }
00723         for(i=0; i<mat.n_col; i++)
00724         {
00725                 for(j=0; j<mat.n_row; j++)
00726                 {
00727                         (*this)(i, j) *= w_err(j) / w_norm(i);
00728                 }
00729         }
00730         return m_info;
00731 }
00732 
00733 int fMat::svd(fMat& U, fVec& Sigma, fMat& VT)
00734 {
00735         assert(n_row == U.n_row && n_row == U.n_col &&
00736                    Sigma.n_row == MIN(n_row, n_col) &&
00737                    n_col == VT.n_row && n_col == VT.n_col);
00738         double* a = p_data;
00739         int ret = dims_svd(a, n_row, n_col, U.data(), Sigma.data(), VT.data());
00740         return ret;
00741 }
00742 
00743 /*
00744  * POSV
00745  */
00746 int fMat::lineq_posv(const fMat& A, const fMat& b)
00747 {
00748         int m = A.row();
00749         int nrhs = b.col();
00750 //      m_info = dims_dporfs(A.data(), p_data, b.data(), m, nrhs);
00751         m_info = dims_dposv(A.data(), p_data, b.data(), m, nrhs);
00752 //      double rcond;
00753 //      m_info = dims_dposvx(A.data(), p_data, b.data(), m, nrhs, &rcond);
00754         return m_info;
00755 }
00756 
00757 int fMat::inv_posv(const fMat& A)
00758 {
00759         fMat I(A.row(), A.col());
00760         I.identity();
00761         m_info = lineq_posv(A, I);
00762         return m_info;
00763 }
00764 
00765 int fVec::lineq_posv(const fMat& A, const fVec& b)
00766 {
00767         int m = A.row();
00768         m_info = dims_dposv(A.data(), p_data, b.data(), m, 1);
00769         return m_info;
00770 }
00771 
00772 int fMat::lineq_porfs(const fMat& A, const fMat& b)
00773 {
00774         int m = A.row();
00775         int nrhs = b.col();
00776         m_info = dims_dporfs(A.data(), p_data, b.data(), m, nrhs);
00777         return m_info;
00778 }
00779 
00780 int fMat::inv_porfs(const fMat& A)
00781 {
00782         fMat I(A.row(), A.col());
00783         I.identity();
00784         m_info = lineq_porfs(A, I);
00785         return m_info;
00786 }
00787 
00788 int fVec::lineq_porfs(const fMat& A, const fVec& b)
00789 {
00790         int m = A.row();
00791         m_info = dims_dporfs(A.data(), p_data, b.data(), m, 1);
00792         return m_info;
00793 }
00794 
00795 /*
00796  * SR-inverse
00797  */
00798 int fMat::lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b){
00799         assert(n_col == b.col() && n_row == A.col() &&
00800                    w_err.row() == A.row() && w_norm.row() == A.col() &&
00801                    b.row() == A.row());
00802         fMat Jhat, tJhat, wb;
00803         int A_row = A.row(), A_col = A.col(), n_rhs = b.col();
00804         Jhat.resize(A_row, A_col);
00805         tJhat.resize(A_col, A_row);
00806         wb.resize(A_row, n_rhs);
00807         int i, j;
00808         Jhat.set(A);
00809         wb.set(b);
00810         for(i=0; i<A_row; i++)
00811     {
00812                 for(j=0; j<A_col; j++)
00813                 {
00814                         Jhat(i, j) *= w_err(i);
00815                         Jhat(i, j) /= w_norm(j);
00816                 }
00817     }
00818         tJhat.tran(Jhat);
00819         for(i = 0; i < A_row; i++)
00820         {
00821                 for(j = 0; j < n_rhs; j++)
00822                 {
00823                         wb(i, j) *= w_err(i);
00824                 }
00825         }
00826         fMat tmp, tmpx, tmpb;
00827         if(A_row < A_col)
00828     {
00829                 tmpx.resize(A_row, n_rhs);
00830                 tmp.resize(A_row, A_row);
00831                 tmp.mul_tran(Jhat, false);
00832                 for(i=0; i<A_row; i++)
00833                 {
00834                         tmp(i, i) += k;
00835                 }
00836                 tmpx.lineq_posv(tmp, wb);
00837                 mul(tJhat, tmpx);
00838                 m_info = 0; // ???
00839                 for(i=0; i<A_col; i++)
00840                 {
00841                         for(j = 0; j < n_rhs; j++){
00842                                 (*this)(i, j) /= w_norm(i);
00843                         }
00844                 }
00845     }
00846         else
00847     {
00848                 tmp.resize(A_col, A_col);
00849                 tmpb.resize(A_col, n_rhs);
00850                 tmp.mul_tran(Jhat, true);
00851                 for(i=0; i<A_col; i++)
00852                 {
00853                         tmp(i, i) += k;
00854                 }
00855                 tmpb.mul(tJhat, wb);
00856                 (*this).lineq_posv(tmp, tmpb);
00857                 m_info = 0; // ???
00858                 for(i = 0; i < A_col; i++)
00859                 {
00860                         for(j = 0; j < n_rhs; j++)
00861                         {
00862                                 (*this)(i, j) /= w_norm(i);
00863                         }
00864                 }
00865     }
00866         return m_info;
00867 }
00868 
00869 int fVec::lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fVec& b)
00870 {
00871         assert(n_row == A.col() && w_err.row() == A.row() &&
00872                    w_norm.row() == A.col() && b.row() == A.row());
00873         fMat Jhat;
00874         fVec wb;
00875         int A_row = A.row(), A_col = A.col();
00876         Jhat.resize(A_row, A_col);
00877         wb.resize(A_row);
00878         int i, j;
00879         Jhat.set(A);
00880         wb.set(b);
00881         double* pp;
00882         for(i=0; i<A_row; i++)
00883     {
00884                 for(j=0; j<A_col; j++)
00885                 {
00886                         pp = Jhat.data() + i + A_row*j;
00887                         *pp *= w_err(i) / w_norm(j);
00888                 }
00889                 wb(i) *= w_err(i);
00890     }
00891         fMat tmp;
00892         fVec tmpx, tmpb;
00893         if(A_row < A_col)
00894     {
00895                 tmpx.resize(A_row);
00896                 tmp.resize(A_row, A_row);
00897                 tmp.mul_tran(Jhat, false);
00898                 for(i=0; i<A_row; i++)
00899                 {
00900                         tmp(i, i) += k;
00901                 }
00902                 tmpx.lineq_posv(tmp, wb);
00903                 mul(tmpx, Jhat);
00904                 m_info = 0; // ???
00905                 for(i=0; i<A_col; i++)
00906                 {
00907                         (*this)(i) /= w_norm(i);
00908                 }
00909     }
00910         else
00911     {
00912                 tmp.resize(A_col, A_col);
00913                 tmpb.resize(A_col);
00914                 tmp.mul_tran(Jhat, true);
00915                 for(i=0; i<A_col; i++)
00916                 {
00917                         tmp(i, i) += k;
00918                 }
00919                 tmpb.mul(wb, Jhat);
00920                 lineq_posv(tmp, tmpb);
00921                 m_info = 0; // ???
00922                 for(i = 0; i < A_col; i++)
00923                 {
00924                         (*this)(i) /= w_norm(i);
00925                 }
00926     }
00927         return m_info;
00928 }
00929 
00930 fMat lineq_sr(const fMat& A, fVec& w_err, fVec& w_norm, double k, const fMat& b)
00931 {
00932         assert(w_err.row() == A.row() && w_norm.row() == A.col() &&
00933                    b.row() == A.row());
00934         fMat x(A.col(), b.col());
00935         fMat Jhat, tJhat, wb;
00936         Jhat.resize(A.row(), A.col());
00937         tJhat.resize(A.col(), A.row());
00938         wb.resize(b.row(), b.col());
00939         int i, j;
00940         Jhat.set(A);
00941         wb.set(b);
00942         for(i=0; i<A.row(); i++)
00943     {
00944                 if(w_err(i) < 1e-8) 
00945                 {
00946                         w_err(i) = 1.0;
00947                 }
00948                 for(j=0; j<A.col(); j++)
00949                 {
00950                         Jhat(i, j) *= w_err(i);
00951                 }
00952     }
00953         for(j=0; j<A.col(); j++)
00954     {
00955                 if(w_norm(j) < 1e-8) 
00956                 {
00957                         w_norm(j) = 1.0;
00958                 }
00959                 for(i=0; i<A.row(); i++)
00960                 {
00961                         Jhat(i, j) /= w_norm(j);
00962                 }
00963     }
00964         tJhat.tran(Jhat);
00965         for(i = 0; i < A.row(); i++){
00966                 for(j = 0; j < b.col(); j++){
00967                         wb(i, j) *= w_err(i);
00968                 }
00969         }
00970         fMat tmp, tmpx, tmpb;
00971         if(A.row() < A.col())
00972     {
00973                 tmpx.resize(A.row(), b.col());
00974                 tmp.resize(A.row(), A.row());
00975                 tmp.mul(Jhat, tJhat);
00976                 for(i=0; i<A.row(); i++)
00977                 {
00978                         tmp(i, i) += k;
00979                 }
00980                 tmpx.lineq(tmp, wb);
00981                 x.mul(tJhat, tmpx);
00982                 for(i=0; i<A.col(); i++)
00983                 {
00984                         for(j = 0; j < b.col(); j++)
00985                         {
00986                                 x(i, j) /= w_norm(i);
00987                         }
00988                 }
00989     }
00990         else
00991     {
00992                 tmp.resize(A.col(), A.col());
00993                 tmpx.resize(A.col(), b.col());
00994                 tmpb.resize(A.col(), b.col());
00995                 tmp.mul(tJhat, Jhat);
00996                 for(i=0; i<A.col(); i++)
00997                 {
00998                         tmp(i, i) += k;
00999                 }
01000                 tmpb.mul(tJhat, wb);
01001                 x.lineq(tmp, tmpb);
01002                 for(i=0; i<A.col(); i++)
01003                 {
01004                         for(j = 0; j < b.col(); j++)
01005                         {
01006                                 x(i, j) /= w_norm(i);
01007                         }
01008                 }
01009     }
01010         return x;
01011 }
01012 
01013 fMat tran(const fMat& mat)
01014 {
01015         fMat ret(mat.n_col, mat.n_row);
01016         int i, j, c = mat.n_col, r = mat.n_row;
01017         for(i=0; i<c; i++)
01018         {
01019                 for(j=0; j<r; j++)
01020                 {
01021                         ret(i,j) = mat.p_data[j+i*mat.n_row];
01022                 }
01023         }
01024         return ret;
01025 }
01026 
01027 void fMat::tran(const fMat& mat)
01028 {
01029         assert(n_col == mat.n_row && n_row == mat.n_col);
01030         int i, j, n, m, c = mat.n_col, r = mat.n_row;
01031         for(i=0, n=0; i<c; i++, n+=r)
01032         {
01033                 for(j=0, m=0; j<r; j++, m+=c)
01034                 {
01035                         p_data[i+m] = mat.p_data[j+n];
01036                 }
01037         }
01038 }
01039 
01040 void fMat::tran()
01041 {
01042         assert(n_col == n_row);
01043         double tmp;
01044         int i, j, m, n, idx1, idx2;
01045         for(i=0, n=0; i<n_row; i++, n+=n_col)
01046         {
01047                 for(j=i, m=i*n_row; j<n_col; j++, m+=n_row)
01048                 {
01049                         idx1 = i + m;
01050                         idx2 = j + n;
01051                         tmp = p_data[idx1];
01052                         p_data[idx1] = p_data[idx2];
01053                         p_data[idx2] = tmp;
01054                 }
01055         }
01056 }
01057 
01058 void fMat::identity()
01059 {
01060         assert(n_col == n_row);
01061         int i, j, m;
01062         for(i=0; i<n_row; i++)
01063         {
01064                 for(j=0, m=i; j<n_col; j++, m+=n_row)
01065                 {
01066                         if(i==j)
01067                                 p_data[m] = 1.0;
01068                         else
01069                                 p_data[m] = 0.0;
01070                 }
01071         }
01072 }
01073 
01074 void fMat::set(const fMat& mat)
01075 {
01076         assert(n_col == mat.n_col && n_row == mat.n_row);
01077         int n = n_row * n_col;
01078         dims_copy(mat.p_data, p_data, n);
01079 }
01080 
01081 void fMat::neg(const fMat& mat)
01082 {
01083         assert(n_col == mat.n_col && n_row == mat.n_row);
01084         int i, n = n_row * n_col;
01085         for(i=0; i<n; i++) p_data[i] = - mat.p_data[i];
01086 }
01087 
01088 fMat operator - (const fMat& mat)
01089 {
01090         fMat ret(mat.n_row, mat.n_col);
01091         int i, n = mat.n_row * mat.n_col;
01092         for(i=0; i<n; i++) ret.p_data[i] = - mat.p_data[i];
01093         return ret;
01094 }
01095 
01096 void fMat::operator += (const fMat& mat)
01097 {
01098         assert(n_col == mat.n_col && n_row == mat.n_row);
01099         int i, n = n_col * n_row;
01100         for(i=0; i<n; i++) p_data[i] += mat.p_data[i];
01101 }
01102 
01103 void fMat::operator -= (const fMat& mat)
01104 {
01105         assert(n_col == mat.n_col && n_row == mat.n_row);
01106         int i, n = n_col * n_row;
01107         for(i=0; i<n; i++) p_data[i] -= mat.p_data[i];
01108 }
01109 
01110 void fMat::operator *= (double d)
01111 {
01112         int n = n_col * n_row;
01113         dims_scale_myself(p_data, d, n);
01114 }
01115 
01116 void fMat::mul(double d, const fMat& mat)
01117 {
01118         assert(n_col == mat.n_col && n_row == mat.n_row);
01119         int n = n_col * n_row;
01120         dims_scale(mat.data(), d, n, data());
01121 }
01122 
01123 void fMat::mul(const fMat& mat, double d)
01124 {
01125         assert(n_col == mat.n_col && n_row == mat.n_row);
01126         int n = n_col * n_row;
01127         dims_scale(mat.data(), d, n, data());
01128 }
01129 
01130 fMat operator * (double d, const fMat& mat)
01131 {
01132         fMat ret(mat.row(), mat.col());
01133         int i, n = mat.col() * mat.row();
01134         for(i=0; i<n; i++) *(ret.data() + i) = d * *(mat.data() + i);
01135         return ret;
01136 }
01137 
01138 fMat operator * (const fMat& mat, double d)
01139 {
01140         fMat ret(mat.row(), mat.col());
01141         int i, n = mat.col() * mat.row();
01142         for(i=0; i<n; i++) *(ret.data() + i) = d * *(mat.data() + i);
01143         return ret;
01144 }
01145 
01146 void fMat::operator /= (double d)
01147 {
01148         int n = n_col * n_row;
01149         dims_scale_myself(p_data, 1.0/d, n);
01150 }
01151 
01152 fMat fMat::operator = (const fMat& mat)
01153 {
01154         assert(n_row == mat.n_row && n_col == mat.n_col);
01155         int i, n = n_col * n_row;
01156         for(i=0; i<n; i++)
01157         {
01158                 *(p_data + i) = *(mat.data() + i);
01159         }
01160         return *this;
01161 }
01162 
01163 void fMat::operator = (double d)
01164 {
01165         int i, n = n_col * n_row;
01166         for(i=0; i<n; i++) p_data[i] = d;
01167 }
01168 
01169 void fMat::add(const fMat& mat1, const fMat& mat2)
01170 {
01171         assert(n_col == mat1.n_col && n_row == mat1.n_row &&
01172                    mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
01173         int i, n = n_row * n_col;
01174         for(i=0; i<n; i++)
01175         {
01176                 p_data[i] = mat1.p_data[i] + mat2.p_data[i];
01177         }
01178 }
01179 
01180 void fMat::add(const fMat& mat)
01181 {
01182         assert(n_col == mat.n_col && n_row == mat.n_row);
01183         int i, n = n_row * n_col;
01184         for(i=0; i<n; i++) p_data[i] += mat.p_data[i];
01185 }
01186 
01187 fMat operator + (const fMat& mat1, const fMat& mat2)
01188 {
01189         assert(mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
01190         fMat ret(mat1.n_row, mat1.n_col, 0.0);
01191         int i, n = mat1.n_row * mat1.n_col;
01192         for(i=0; i<n; i++) ret.p_data[i] = mat1.p_data[i] + mat2.p_data[i];
01193         return ret;
01194 }
01195 
01196 void fMat::sub(const fMat& mat1, const fMat& mat2)
01197 {
01198         assert(n_col == mat1.n_col && n_row == mat1.n_row &&
01199                    mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
01200         int i, n = n_row * n_col;
01201         double* p1 = mat1.p_data, *p2 = mat2.p_data;
01202         for(i=0; i<n; i++)
01203         {
01204                 p_data[i] = p1[i] - p2[i];
01205         }
01206 }
01207 
01208 fMat operator - (const fMat& mat1, const fMat& mat2)
01209 {
01210         assert(mat1.n_col == mat2.n_col && mat1.n_row == mat2.n_row);
01211         fMat ret(mat1.n_row, mat1.n_col, 0.0);
01212         int i, n = mat1.n_row * mat1.n_col;
01213         for(i=0; i<n; i++) ret.p_data[i] = mat1.p_data[i] - mat2.p_data[i];
01214         return ret;
01215 }
01216 
01217 void fMat::mul(const fMat& mat1, const fMat& mat2)
01218 {
01219         assert(n_row == mat1.n_row && n_col == mat2.n_col && mat1.n_col == mat2.n_row);
01220 #if 0
01221         int i, j, k, n, c = mat1.col(), r = mat1.row();
01222         double* p1, *p2, *pp;
01223         for(i=0; i<n_row; i++)
01224         {
01225                 for(j=0, n=0, pp=p_data+i; j<n_col; j++, n+=c, pp+=n_row)
01226                 {
01227                         double temp = 0.0;
01228                         for(k=0, p1=mat1.p_data+i, p2=mat2.p_data+n; k<c; k++, p1+=r, p2++)
01229                         {
01230                                 temp += *p1 * *p2;
01231                         }
01232                         *pp = temp;
01233                 }
01234         }
01235 #else
01236         int m = mat1.row(), k = mat1.col(), n = mat2.col();
01237         dims_dgemm(mat1.data(), mat2.data(), m, n, k, p_data);
01238 #endif
01239 }
01240 
01241 void fMat::mul_tran(const fMat& mat1, int trans_first)
01242 {
01243         assert(n_row == n_col);
01244         assert((trans_first && n_row == mat1.n_col) || (!trans_first && n_row ==mat1.n_row));
01245         if(trans_first)
01246         {
01247                 int i, j, r = mat1.row();
01248                 double* p1 = mat1.p_data, *p2, *pp;
01249                 double* pp_first;
01250                 for(j=0, p2=mat1.p_data, pp_first=p_data; j<n_col; j++, p2+=r, pp_first+=n_row)
01251                 {
01252                         pp = pp_first;
01253                         for(i=0, p1=mat1.p_data; i<=j; i++, p1+=r, pp++)
01254                         {
01255                                 double temp = dims_dot(p1, p2, r);
01256                                 *pp = temp;
01257                                 if(i!=j) p_data[j+n_row*i] = temp;
01258                         }
01259                 }
01260         }
01261         else
01262         {
01263                 dims_dsyrk(mat1.p_data, mat1.n_row, mat1.n_col, p_data);
01264                 symmetric();
01265         }
01266 }
01267 
01268 fMat operator * (const fMat& mat1, const fMat& mat2)
01269 {
01270         assert(mat1.n_col == mat2.n_row);
01271         fMat ret(mat1.n_row, mat2.n_col, 0.0);
01272         int i, j, k, n;
01273         for(i=0; i<ret.row(); i++)
01274         {
01275                 for(j=0; j<ret.col(); j++)
01276                 {
01277                         n = j * mat2.n_row;
01278                         ret(i, j) = 0;
01279                         for(k=0; k<mat1.col(); k++)
01280                         {
01281                                 ret(i, j) += mat1.p_data[i+k*mat1.n_row] * mat2.p_data[k+n];
01282                         }
01283                 }
01284         }
01285         return ret;
01286 }
01287 
01288 int inv_enlarge(const fMat& m12, const fMat& m21, const fMat& m22, const fMat& P, fMat& X, fMat& y, fMat& z, fMat& w)
01289 {
01290         int n_org = P.col();
01291         int n_add = m12.col();
01292         fMat Pm(n_org, n_add), mPm(n_add, n_add), mP(n_add, n_org);
01293         X.resize(n_org, n_org);
01294         y.resize(n_org, n_add);
01295         z.resize(n_add, n_org);
01296         w.resize(n_add, n_add);
01297         if(n_org == 0)
01298         {
01299                 w.inv_svd(m22);
01300                 return 0;
01301         }
01302         Pm.mul(P, m12);
01303         mPm.mul(m21, Pm);
01304         mPm *= -1.0;
01305         mPm += m22;
01306         w.inv_svd(mPm);
01307         y.mul(Pm, w);
01308         y *= -1.0;
01309         mP.mul(m21, P);
01310         z.mul(w, mP);
01311         z *= -1.0;
01312         X.mul(Pm, z);
01313         X *= -1.0;
01314         X += P;
01315         return 0;
01316 }
01317 
01318 int inv_shrink(const fMat& P, const fMat& q, const fMat& r, const fMat& s, fMat& X)
01319 {
01320         int n_org = P.col();
01321         int n_add = q.col();
01322         if(n_org == 0)
01323         {
01324                 return 0;
01325         }
01326         fMat sr(n_add, n_org), qsr(n_org, n_org);
01327         sr.lineq_svd(s, r);
01328         qsr.mul(q, sr);
01329         X.resize(n_org, n_org);
01330         X.sub(P, qsr);
01331         return 0;
01332 }
01333 
01334 int inv_row_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y)
01335 {
01336         int n_org = P.col();
01337         int n_add = q.col();
01338         int n_total = P.row();
01339         fMat m2d_q(n_add, n_add), m2d_q_inv(n_add, n_add), m2d_P(n_add, n_org);
01340         X.resize(n_total, n_org);
01341         y.resize(n_total, n_add);
01342         if(n_org == 0)
01343         {
01344                 y.inv_svd(m2d);
01345                 return 0;
01346         }
01347         m2d_q.mul(m2d, q);
01348         m2d_q_inv.inv_svd(m2d_q);
01349         y.mul(q, m2d_q_inv);
01350         m2d_P.mul(m2d, P);
01351         X.mul(y, m2d_P);
01352         X *= -1.0;
01353         X += P;
01354         return 0;
01355 }
01356 
01357 int inv_col_replaced(const fMat& P, const fMat& q, const fMat& m2d, fMat& X, fMat& y)
01358 {
01359         int n_org = P.row();
01360         int n_add = q.row();
01361         int n_total = P.col();
01362         fMat q_m2d(n_add, n_add), q_m2d_inv(n_add, n_add), P_m2d(n_org, n_add);
01363         X.resize(n_org, n_total);
01364         y.resize(n_add, n_total);
01365         if(n_org == 0)
01366         {
01367                 y.inv_svd(m2d);
01368                 return 0;
01369         }
01370         q_m2d.mul(q, m2d);
01371         q_m2d_inv.inv_svd(q_m2d);
01372         y.mul(q_m2d_inv, q);
01373         P_m2d.mul(P, m2d);
01374         X.mul(P_m2d, y);
01375         X *= -1.0;
01376         X += P;
01377         return 0;
01378 }
01379         
01380 void fVec::mul(const fMat& mat, const fVec& vec)
01381 {
01382         assert(n_row == mat.row() && mat.col() == vec.row());
01383 #if 0
01384         int i, k, c = mat.col(), r = mat.row();
01385         double* pm, *pv, *pp;
01386         for(i=0, pp=p_data; i<n_row; i++, pp++)
01387         {
01388                 double temp = 0.0;
01389                 for(k=0, pm=mat.data()+i, pv=vec.data(); k<c; k++, pm+=r, pv++)
01390                 {
01391                         temp += *pm * *pv;
01392                 }
01393                 *pp = temp;
01394         }
01395 #else
01396         int c = mat.col(), r = mat.row();
01397         dims_dgemv(mat.data(), r, c, vec.data(), p_data);
01398 #endif
01399 }
01400 
01401 void fVec::mul(const fVec& vec, const fMat& mat)
01402 {
01403         assert(n_row == mat.col() && mat.row() == vec.row());
01404         int i, r = mat.row();
01405         double* pm, *pv = vec.p_data, *pp;
01406         for(i=0, pm=mat.data(), pp=p_data; i<n_row; i++, pm+=r, pp++)
01407         {
01408                 *pp = dims_dot(pv, pm, r);
01409         }
01410 }
01411 
01412 fVec operator * (const fMat& mat, const fVec& vec)
01413 {
01414         assert(mat.col() == vec.row());
01415         fVec ret(mat.row(), 0.0);
01416         int i, k;
01417         for(i=0; i<ret.row(); i++)
01418         {
01419                 ret(i) = 0;
01420                 for(k=0; k<mat.col(); k++)
01421                 {
01422                         ret(i) += mat.data()[i+k*mat.row()]*vec.data()[k];
01423                 }
01424         }
01425         return ret;
01426 }
01427 
01428 void fMat::div(const fMat& mat, double d)
01429 {
01430         assert(n_col == mat.n_col && n_row == mat.n_row);
01431         int n = n_col * n_row;
01432         dims_scale(mat.p_data, 1.0/d, n, p_data);
01433 }
01434 
01435 fMat operator / (const fMat& mat, double d)
01436 {
01437         fMat ret(mat.n_row, mat.n_col);
01438         int i, n = mat.col() * mat.row();
01439         for(i=0; i<n; i++) ret.p_data[i] = mat.p_data[i] / d;
01440         return ret;
01441 }
01442 
01446 int fVec::max_index()
01447 {
01448         int ret = -1;
01449         double max_val = 0.0;
01450         int i;
01451         for(i=0; i<n_row; i++)
01452         {
01453                 if(ret < 0 || p_data[i] > max_val)
01454                 {
01455                         ret = i;
01456                         max_val = p_data[i];
01457                 }
01458         }
01459         return ret;
01460 }
01461 
01462 int fVec::min_index()
01463 {
01464         int ret = -1;
01465         double min_val = 0.0;
01466         int i;
01467         for(i=0; i<n_row; i++)
01468         {
01469                 if(ret < 0 || p_data[i] < min_val)
01470                 {
01471                         ret = i;
01472                         min_val = p_data[i];
01473                 }
01474         }
01475         return ret;
01476 }
01477 
01478 double fVec::max_value()
01479 {
01480         int index = -1;
01481         double max_val = 0.0;
01482         int i;
01483         for(i=0; i<n_row; i++)
01484         {
01485                 if(index < 0 || p_data[i] > max_val)
01486                 {
01487                         index = i;
01488                         max_val = p_data[i];
01489                 }
01490         }
01491         return max_val;
01492 }
01493 
01494 double fVec::min_value()
01495 {
01496         int index = -1;
01497         double min_val = 0.0;
01498         int i;
01499         for(i=0; i<n_row; i++)
01500         {
01501                 if(index < 0 || p_data[i] < min_val)
01502                 {
01503                         index = i;
01504                         min_val = p_data[i];
01505                 }
01506         }
01507         return min_val;
01508 }
01509 
01510 void fVec::abs()
01511 {
01512         for(int i=0; i<n_row; i++)
01513         {
01514                 p_data[i] = fabs(p_data[i]);
01515         }
01516 }
01517 
01518 void fVec::set(const fVec& vec)
01519 {
01520         assert(n_row == vec.n_row);
01521         dims_copy(vec.p_data, p_data, n_row);
01522 }
01523 
01524 void fVec::neg(const fVec& vec)
01525 {
01526         assert(n_row == vec.n_row);
01527         int i;
01528         for(i=0; i<vec.n_row; i++) p_data[i] = - vec.p_data[i];
01529 }
01530 
01531 fVec operator - (const fVec& vec)
01532 {
01533         fVec ret(vec.n_row);
01534         int i;
01535         for(i=0; i<vec.n_row; i++) ret.p_data[i] = - vec.p_data[i];
01536         return ret;
01537 }
01538 
01539 void fVec::operator += (const fVec& vec)
01540 {
01541         assert(n_row == vec.n_row);
01542         int i;
01543         for(i=0; i<n_row; i++) p_data[i] += vec.p_data[i];
01544 }
01545 
01546 void fVec::operator -= (const fVec& vec)
01547 {
01548         assert(n_row == vec.n_row);
01549         dims_daxpy(n_row, -1.0, vec.p_data, p_data);
01550 }
01551 
01552 void fVec::operator *= (double d)
01553 {
01554         dims_scale_myself(p_data, d, n_row);
01555 }
01556 
01557 void fVec::operator /= (double d)
01558 {
01559         dims_scale_myself(p_data, 1.0/d, n_row);
01560 }
01561 
01562 void fVec::add(const fVec& vec1, const fVec& vec2)
01563 {
01564         assert(n_row == vec1.n_row && vec1.n_row == vec2.n_row);
01565         for(int i=0; i<n_row; i++) p_data[i] = vec1.p_data[i] + vec2.p_data[i];
01566 }
01567 
01568 void fVec::add(const fVec& vec)
01569 {
01570         assert(n_row == vec.n_row);
01571         for(int i=0; i<n_row; i++) p_data[i] += vec.p_data[i];
01572 }
01573 
01574 fVec operator + (const fVec& vec1, const fVec& vec2)
01575 {
01576         assert(vec1.n_row == vec2.n_row);
01577         fVec ret(vec1.row(), 0.0);
01578         for(int i=0; i<vec1.n_row; i++)
01579                 ret.p_data[i] = vec1.p_data[i] + vec2.p_data[i];
01580         return ret;
01581 }
01582 
01583 void fVec::sub(const fVec& vec1, const fVec& vec2)
01584 {
01585         assert(n_row == vec1.n_row && vec1.n_row == vec2.n_row);
01586         for(int i=0; i<n_row; i++)
01587                 p_data[i] = vec1.p_data[i] - vec2.p_data[i];
01588 }
01589 
01590 fVec operator - (const fVec& vec1, const fVec& vec2)
01591 {
01592         assert(vec1.n_row == vec2.n_row);
01593         fVec ret(vec1.row(), 0.0);
01594         for(int i=0; i<vec1.n_row; i++)
01595                 ret.p_data[i] = vec1.p_data[i] - vec2.p_data[i];
01596         return ret;
01597 }
01598 
01599 void fVec::div(const fVec& vec, double d)
01600 {
01601         assert(n_row == vec.n_row);
01602         dims_scale(vec.p_data, 1.0/d, n_row, p_data);
01603 }
01604 
01605 fVec operator / (const fVec& vec, double d)
01606 {
01607         fVec ret(vec.n_row);
01608         dims_scale(vec.p_data, 1.0/d, vec.n_row, ret.p_data);
01609         return ret;
01610 }
01611 
01612 double operator * (const fVec& vec1, const fVec& vec2)
01613 {
01614         assert(vec1.n_row == vec2.n_row);
01615         double ret = 0;
01616         ret = dims_dot(vec1.p_data, vec2.p_data, vec1.n_row);
01617         return ret;
01618 }
01619 
01620 void fVec::mul(const fVec& vec, double d)
01621 {
01622         assert(n_row == vec.n_row);
01623         dims_scale(vec.p_data, d, n_row, p_data);
01624 }
01625 
01626 void fVec::mul(double d, const fVec& vec)
01627 {
01628         assert(n_row == vec.n_row);
01629         dims_scale(vec.p_data, d, n_row, p_data);
01630 }
01631 
01632 fVec operator * (double d, const fVec& vec)
01633 {
01634         fVec ret(vec.n_row);
01635         dims_scale(vec.p_data, d, vec.n_row, ret.p_data);
01636         return ret;
01637 }
01638 
01639 fVec operator * (const fVec& vec, double d)
01640 {
01641         fVec ret(vec.row());
01642         dims_scale(vec.p_data, d, vec.n_row, ret.p_data);
01643         return ret;
01644 }
01645 
01646 void fVec::operator = (double d)
01647 {
01648         for(int i=0; i<n_row; i++) p_data[i] = d;
01649 }
01650 
01651 fVec fVec::operator = (const fVec& vec)
01652 {
01653         assert(n_row == vec.n_row);
01654         dims_copy(vec.p_data, p_data, n_row);
01655         return *this;
01656 }
01657 
01658 /**********added by takano*******/
01659 fMat eigs(const fMat& mat,double* w)
01660 {
01661         int i,j,k,dim;
01662         double *a;
01663         fMat   eigmat;
01664         a= new double [mat.col()*mat.row()];
01665 //      w=(double*)malloc(sizeof(double)*mat.col());
01666  
01667         k=0;
01668         for(i=0;i<mat.row();i++){
01669                 for(j=0;j<mat.col();j++){
01670                         a[k]=mat(i,j);
01671                         k=k+1;
01672                 }
01673         }
01674         dim=mat.row();  
01675         i=dims_eigs(dim,a,w);
01676         eigmat.resize(dim,dim);
01677         k=0;
01678         for(j=0;j<dim;j++){
01679                 for(i=0;i<dim;i++){
01680                         eigmat(i,j)=a[k];
01681                         k=k+1;
01682                 }
01683         }
01684         delete [] a;
01685         return eigmat; 
01686 }
01687 
01688 void fMat::dmul(const fMat& mat1, const fMat& mat2)
01689 {
01690 #ifndef NDEBUG
01691 //      if(n_row != mat1.n_row || n_col != mat2.n_col ||
01692 //         mat1.n_col != mat2.n_row)
01693 //      {
01694 //              cerr << "fMat::mul(fMat, fMat): matrix size error" << endl;
01695 //              return;
01696 //      }
01697 #endif
01698         int i, j, m;
01699         if(mat1.n_row == mat2.n_row && mat1.n_col == mat2.n_col)
01700         {
01701                 for(i=0; i<n_row; i++)
01702                 {
01703                         for(j=0, m=i; j<n_col; j++, m+=n_row)
01704                         {
01705                                         p_data[m] = mat1.p_data[m] * mat2.p_data[m];
01706                         }
01707                 }
01708         }
01709         else if(mat1.n_row == mat2.n_row && mat2.n_col == 1)
01710         {
01711                 for(i=0; i<n_row; i++)
01712                 {
01713                         for(j=0, m=i; j<n_col; j++, m+=n_row)
01714                         {
01715                                         p_data[m] = mat1.p_data[m] * mat2.p_data[i];
01716                         }
01717                 }
01718                 
01719         }
01720         else if(mat1.n_col == mat2.n_col && mat2.n_row == 1)
01721         {
01722                 for(i=0; i<n_row; i++)
01723                 {
01724                         for(j=0, m=i; j<n_col; j++, m+=n_row)
01725                         {
01726                                         p_data[m] = mat1.p_data[m] * mat2.p_data[j];
01727                         }
01728                 }
01729         }
01730         else
01731         {
01732                 cerr << "fMat::dmul(fMat, fMat): matrix size error" << endl;
01733         }
01734 }
01735 
01736 
01737 void fMat::ddiv(const fMat& mat1, const fMat& mat2)
01738 {
01739 #ifndef NDEBUG
01740 //      if(n_row != mat1.n_row || n_col != mat2.n_col ||
01741 //         mat1.n_col != mat2.n_row)
01742 //      {
01743 //              cerr << "fMat::mul(fMat, fMat): matrix size error" << endl;
01744 //              return;
01745 //      }
01746 #endif
01747         int i, j, m;
01748         if(mat1.n_row == mat2.n_row && mat1.n_col == mat2.n_col)
01749         {
01750                 for(i=0; i<n_row; i++)
01751                 {
01752                         for(j=0, m=i; j<n_col; j++, m+=n_row)
01753                         {
01754                                         p_data[m] = mat1.p_data[m] / mat2.p_data[m];
01755                         }
01756                 }
01757         }
01758         else if(mat1.n_row == mat2.n_row && mat2.n_col == 1)
01759         {
01760                 for(i=0; i<n_row; i++)
01761                 {
01762                         for(j=0, m=i; j<n_col; j++, m+=n_row)
01763                         {
01764                                         p_data[m] = mat1.p_data[m] / mat2.p_data[i];
01765                         }
01766                 }
01767                 
01768         }
01769         else if(mat1.n_col == mat2.n_col && mat2.n_row == 1)
01770         {
01771                 for(i=0; i<n_row; i++)
01772                 {
01773                         for(j=0, m=i; j<n_col; j++, m+=n_row)
01774                         {
01775                                         p_data[m] = mat1.p_data[m] / mat2.p_data[j];
01776                         }
01777                 }
01778         }
01779         else
01780         {
01781                 cerr << "fMat::dmul(fMat, fMat): matrix size error" << endl;
01782         }
01783 }
01784 
01785 double fMat::det(void)
01786 {
01787 #ifndef NDEBUG
01788   if((n_row !=n_col) || (n_row < 1))
01789     {
01790       cerr << "matrix size error at function det()" << endl;
01791       return 0;
01792     }
01793 #endif
01794   double x;
01795   m_info = dims_det(n_row, p_data, &x);
01796   
01797   return x;
01798 }
01799 
01800 int fMat::eig(fVec& wr, fVec& wi)
01801 {
01802 #ifndef NDEBUG
01803   if((n_row !=n_col) || (n_row < 1) || 
01804      (n_row !=wr.row()) || (n_row !=wi.row()))
01805     {
01806       cerr << "matrix size error at function eig()" << endl;
01807       return -1;
01808     }
01809 #endif
01810 
01811   m_info = dims_dgeev_simple(n_row, 
01812                              p_data, 
01813                              wr.data(), 
01814                              wi.data());
01815   
01816   return m_info;
01817 }
01818 
01819 int fMat::eig(fVec& wr, fVec& wi, fMat& v)
01820 {
01821 #ifndef NDEBUG
01822   if((n_row !=n_col) || (n_row < 1) || 
01823      (n_row !=wr.row()) || (n_row !=wi.row()) ||
01824      (n_row !=v.row()) || (n_row !=v.col()))
01825     {
01826       cerr << "matrix size error at function eig()" << endl;
01827       return -1;
01828     }
01829 #endif
01830 
01831   m_info = dims_dgeev(n_row, 
01832                       p_data, 
01833                       wr.data(), 
01834                       wi.data(),
01835                       v.data());
01836   
01837   return m_info;
01838 }
01839 
01840 int fMat::eig(fVec& wr, fVec& wi, fMat& vr, fMat& vi)
01841 {
01842 #ifndef NDEBUG
01843   if((n_row !=n_col) || (n_row < 1) || 
01844      (n_row !=wr.row()) || (n_row !=wi.row()) ||
01845      (n_row !=vr.row()) || (n_row !=vr.col()) ||
01846      (n_row !=vi.row()) || (n_row !=vi.col()) ) 
01847     {
01848       cerr << "matrix size error at function eig()" << endl;
01849       return -1;
01850     }
01851 #endif
01852   int i, j;
01853   double *p_wi, *p_vr, *p_vi;
01854 
01855   p_wi = wi.data();
01856   p_vr = vr.data();
01857   p_vi = vi.data();
01858   
01859   m_info = dims_dgeev(n_row, 
01860                       p_data, 
01861                       wr.data(), 
01862                       p_wi,
01863                       p_vr);
01864   
01865   for(i=0;i<n_row;i++, p_wi++, p_vr+=n_row, p_vi+=n_row)
01866     if(*p_wi == 0)
01867       for(j=0;j<n_row;j++)
01868         *(p_vi+j) = 0.0;
01869     else
01870       {
01871         p_vr+=n_row;
01872         for(j=0;j<n_row;j++)
01873           *(p_vi+j) = *(p_vr+j);
01874         
01875         p_vi+=n_row;
01876         for(j=0;j<n_row;j++)
01877           *(p_vi+j) = -*(p_vr+j);
01878         
01879         for(j=0;j<n_row;j++)
01880           *(p_vr+j) = *(p_vr+j-n_row);
01881         
01882         i++, p_wi++;
01883       }
01884   
01885   return m_info;
01886 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53