//Y <- a M X + b Y
// ----------------------------------------------------------------------
int dgemv(double alpha, const DblNumMat& A, const DblNumVec& X, double beta, DblNumVec& Y)
{
assert(Y.m() == A.m());
assert(A.n() == X.m());
iC( dgemv(A.m(), A.n(), alpha, A.data(), X.data(), beta, Y.data()) );
return 0;
}
// ----------------------------------------------------------------------
int tran(const DblNumMat& M, DblNumMat& R)
{
assert(R.m()==M.n() && R.n()==M.m()); //R.resize(M.n(), M.m());
for(int i=0; i<M.m(); i++)
for(int j=0; j<M.n(); j++)
R(j,i) = M(i,j);
return 0;
}
//Y <- a M X + b Y
// ----------------------------------------------------------------------
int dgemv(double alpha, const DblNumMat& A, const DblNumVec& X, double beta, DblNumVec& Y)
{
assert(Y.m() == A.m());
assert(A.n() == X.m());
char trans = 'N';
int m = A.m(); int n = A.n();
int incx = 1; int incy = 1;
dgemv_(&trans, &m, &n, &alpha, A.data(), &m, X.data(), &incx, &beta, Y.data(), &incy);
return 0;
}
//Y <- a M X + b Y
// ----------------------------------------------------------------------
int dgemm(double alpha, const DblNumMat& A, const DblNumMat& B, double beta, DblNumMat& C)
{
assert( A.m() == C.m() ); assert( A.n() == B.m() ); assert( B.n() == C.n() );
char transa = 'N';
char transb = 'N';
int m = C.m(); int n = C.n(); int k = A.n();
dgemm_(&transa, &transb, &m, &n, &k,
&alpha, A.data(), &m, B.data(), &k, &beta, C.data(), &m);
return 0;
}
// ----------------------------------------------------------------------
int dger(double alpha, const DblNumVec& X, const DblNumVec& Y, DblNumMat& A)
{
assert(X.m() == A.m());
assert(Y.m() == A.n());
iC( dger(A.m(), A.n(), alpha, X.data(), Y.data(), A.data()) );
return 0;
}
// ----------------------------------------------------------------------
int inv(const DblNumMat& M, DblNumMat& R) //Gaussian Elimination
{
//OR pinv(M, 0.0, R);
assert(M.m()==M.n() && R.m()==R.n() && M.m()==R.m());
memcpy(R.data(), M.data(), M.m()*M.n()*sizeof(double));
int info;
int m = M.m();
int* ipiv = new int[m];
DGETRF(&m, &m, R.data(), &m, ipiv, &info); assert(info==0);
int lwork = m;
double* work = new double[lwork];
DGETRI(&m, R.data(), &m, ipiv, work, &lwork, &info); assert(info==0);
delete [] ipiv;
delete [] work;
return 0;
}
inline void cpp2mex(const DblNumMat& cd, mxArray*& md)
{
int m = cd.m();
int n = cd.n();
md = mxCreateDoubleMatrix(m, n, mxREAL);
double* xr = mxGetPr(md);
int cnt = 0;
for(int j=0; j<n; j++)
for(int i=0; i<m; i++) {
xr[cnt] = cd(i,j);
cnt++;
}
return;
}
// ----------------------------------------------------------------------
int pinv(const DblNumMat& M, double epsilon, DblNumMat& R)
{
assert(M.m() == R.n()); assert(M.n() == R.m());
SVDRep svd;
iC( svd.construct(epsilon, M) );
//invert Svd //cerr<<svd.S()(0)<<" "<<svd.S()(svd.S().m()-1)<<" "<<svd.S()(0)/svd.S()(svd.S().m()-1)<<endl;
double cutoff = epsilon * svd.S()(0); //double cutoff = epsilon;
for(int i=0; i<svd.S().m(); i++) {
if( svd.S()(i) >= cutoff) {
svd.S()(i) = 1.0/(svd.S()(i));
} else {
assert(0); //svd.S()(i) = 0.0;
}
}
DblNumMat UT(svd.U().n(), svd.U().m());
DblNumMat V( svd.VT().n(), svd.VT().m());
iC( tran(svd.U(), UT) );
iC( tran(svd.VT(), V) );
for(int i=0; i<V.m(); i++)
for(int j=0; j<V.n(); j++) {
V(i,j) = V(i,j) * svd.S()(j);
}
char transa = 'N';
char transb = 'N';
double alpha = 1.0;
double beta = 0.0;
int m = V.m();
int n = UT.n();
int k = V.n();
DGEMM(&transa, &transb, &m, &n, &k, &alpha,
V.data(), &m, UT.data(), &k,
&beta, R.data(), &m);
return 0;
}
/* ********************************************************************** */
int SVDRep::construct(double epsilon, const DblNumMat& K)
{
int m = K.m();
int n = K.n();
int k = min(m, n);
DblNumMat tU(m, k);
DblNumVec tS(k);
DblNumMat tVT(k, n);
//SVD
int INFO;
char JOBU = 'S';
char JOBVT = 'S';
int wssize = max(3*min(m,n)+max(m,n), 5*min(m,n));
double* wsbuf = new double[wssize];
DGESVD(&JOBU, &JOBVT, &m, &n, K.data(), &m, tS.data(), tU.data(), &m, tVT.data(), &k, wsbuf, &wssize, &INFO); iA(INFO==0);
delete [] wsbuf;
//cutoff
double cutoff = epsilon*tS(0);
int cnt=0;
while(cnt< k)
if(abs(tS(cnt)) >= cutoff)
cnt++;
else
break;
_matU.resize(m, cnt);
_matS.resize(cnt);
_matVT.resize(cnt,n);
for(int i=0; i<m; i++)
for(int j=0; j<cnt; j++)
_matU(i,j) = tU(i,j);
for(int i=0; i<cnt; i++)
_matS(i) = tS(i);
for(int i=0; i<cnt; i++)
for(int j=0; j<n; j++)
_matVT(i,j) = tVT(i,j);
return 0;
}
//Y <- a M X + b Y
// ----------------------------------------------------------------------
int dgemm(double alpha, const DblNumMat& A, const DblNumMat& B, double beta, DblNumMat& C)
{
assert( A.m() == C.m() ); assert( A.n() == B.m() ); assert( B.n() == C.n() );
iC( dgemm(C.m(), C.n(), A.n(), alpha, A.data(), B.data(), beta, C.data()) );
return 0;
}
// ----------------------------------------------------------------------
int daxpy(double a, const DblNumMat& X, DblNumMat& Y)
{
assert( X.m() == Y.m() ); assert( X.n() == Y.n() );
iC( daxpy(X.m()*X.n(), a, X.data(), Y.data()) );
return 0;
}