/*
* Copyright (C) 2007-2008 Dynare Team
*
* This file is part of Dynare.
*
* Dynare is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Dynare is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Dynare. If not, see .
*/
#include "linbcg.hh"
//#define DEBUG
LinBCG::LinBCG()
{
ijat_p=NULL;
}
void LinBCG::Preconditioner(int periods, int y_kmin, int y_kmax, int Size, std::map ,int>, int> IM_i, int* index_vara, int* index_equa, int y_size, double *y, bool print_it, int type, Mat_O_DP &a, Vec_O_INT &indx)
{
std::map, int> , int>::iterator it4_i;
std::map, double>::iterator it4;
int i, j, k, eq=0, eqp, var, lag, pos_u;
std::map, double> IM;
LinBCG U_linbcg, L_linbcg;
Vec_INT ijL, ijU;
Vec_DP sL, sU;
double d;
IM.clear();
it4_i=IM_i.begin();
int u_size=IM_i.size();
//mexPrintf("Preconditioner\n");
double u_pos_u;
while(it4_i!=IM_i.end())
{
eq= it4_i->first.first.first;
var=it4_i->first.first.second;
lag=it4_i->first.second;
pos_u=it4_i->second;
if(pos_u>=Size) // to eliminate the residual contain in the first (Size) elements of IM
{
//mexPrintf("IM_i[%d, (%d, %d)]=%d val=%f ",eq, var, lag, pos_u, u[pos_u]);
IM[make_pair(eq,var-lag*Size)]+=u[pos_u/*+(periods-1)*u_size*/];
//mexPrintf(" IM[%d, %d]=%f\n",eq,var-lag*Size, IM[make_pair(eq,var-lag*Size)]);
}
it4_i++;
}
int Size_SparseMatrixRow=IM.size();
//mexPrintf("Size_SparseMatrixRow+1=%d\n",Size_SparseMatrixRow+1);
int ija_d[Size_SparseMatrixRow+1];
DP sa_d[Size_SparseMatrixRow+1];
k=Size;
ija_d[0]=k+1;
j=0;
eqp=-99;
it4=IM.begin();
while(it4!=IM.end())
{
eq=it4->first.first;
var=it4->first.second;
u_pos_u=it4->second;
//mexPrintf("IM[eq=%d, var=%d]=%f \n",eq, var, u_pos_u);
if(eq!=eqp)
ija_d[eq+j*Size]=k+1;
if(eq==var)
sa_d[var+j*Size]+=u_pos_u;//u[pos_u+j*u_size];
else
{
k++;
sa_d[k]+=u_pos_u;//u[pos_u+j*u_size];
ija_d[k]=var+j*Size;
}
eqp=eq;
it4++;
}
eq=Size;//Size*periods;
ija_d[eq]=k+1;
sa_d[eq]=0;
ija_p.Format(ija_d,Size_SparseMatrixRow+1);
sa_p.Format(sa_d,Size_SparseMatrixRow+1);
/*sprsprt();
sprs_sprt();*/
//mexPrintf("ok0\n");
a.Format(Size,Size);
sprsout(a);
indx.Format(Size);
//mexPrintf("ok1\n");
/*Mat_DP aa(0.0,Size*periods,Size*periods);
for(k=0;k ,int>, int> IM_i, int* index_vara, int* index_equa, int y_size, double *y, bool print_it, bool cvg, Mat_DP &a, Vec_IO_INT &indx)
{
// Solve iteratively the systme A*x=b
// A is sparse and rewritten using sa_p and ija_p
mexPrintf("SolveLinear\n");
mexEvalString("drawnow;");
std::map, int> , int>::iterator it4_i;
std::map >, int>::iterator it4;
int i, j, k, eq=0, eqp, var, lag, pos_u;
std::map >, int> IM;
clock_t t1 = clock();
IM.clear();
it4_i=IM_i.begin();
int u_size=IM_i.size();
int Size_SparseMatrixRow=0;
Vec_DP b(Size*periods), x(Size*periods);
int ITMAX=10*Size*periods;
double TOL=1e-9;
int ITOL=1;
int sub_iter;
DP err;
//Vec_DP Err(Size*periods);
if (iter>0)
mexPrintf("Sim : %f ms\n",(1000.0*(double(clock())-double(time00)))/double(CLOCKS_PER_SEC));
if (isnan(res1) || isinf(res1))
{
if (slowc_save<1e-8)
{
mexPrintf("Dynare cannot improve the simulation\n");
mexEvalString("drawnow;");
filename+=" stopped";
mexEvalString("st=fclose('all');clear all;");
mexErrMsgTxt(filename.c_str());
}
slowc_save/=2;
mexPrintf("Error: Simulation diverging, trying to correct it using slowc=%f\n",slowc_save);
for (i=0;ifirst.first.first;
var=it4_i->first.first.second;
lag=it4_i->first.second;
pos_u=it4_i->second;
if(pos_u>=Size) // to eliminate the residual contain in the first (Size) elements of IM
{
//mexPrintf("IM[%d, (%d, %d)]=%d\n",eq, var, lag, pos_u);
IM[make_pair(eq,make_pair(var,lag))]=pos_u;
for(j=0;j<=min(y_kmax+y_kmin,periods-1);j++)
{
//mexPrintf("j=%d\n",j);
if(j-y_kmin<=0)
k=j;
else
k=periods+j-y_kmin-y_kmax-1;
if(lag+j<0 || lag+j>min(periods-1,y_kmax+y_kmin))
{
//mexPrintf("eliminate b[eq+(k)*Size=%d]=u[%d]*y[%d]\n",eq+(k)*Size,pos_u+(k)*u_size,var+(k+1)*y_size);
b[eq+(k)*Size]-=u[pos_u+k*u_size]*y[index_vara[var-lag*Size]+(k+lag+1)*y_size];
Size_SparseMatrixRow--;
}
}
}
it4_i++;
}
//mexPrintf("Size_SparseMatrixRow+1=%d\n",Size_SparseMatrixRow+1);
int ija_d[Size_SparseMatrixRow+1];
DP sa_d[Size_SparseMatrixRow+1];
k=Size*periods;
ija_d[0]=k+1;
for(j=0;jfirst.first;
var=it4->first.second.first;
lag=it4->first.second.second;
pos_u=it4->second;
/*if(j==1)
mexPrintf("IM[eq=%d, var=%d, lag=%d]=%d val=%f \n",eq, var, lag, pos_u,u[pos_u+j*u_size]);*/
if(lag+j>=0 && lag+j=0 && j+lag fabs(sx[isamax])) isamax=i;
}
return fabs(sx[isamax]);
}
}
void LinBCG::sprsax(Vec_I_DP &sa, Vec_I_INT &ija, Vec_I_DP &x, Vec_O_DP &b)
{
int i,k;
//mexPrintf("sprsax\n");
int n=x.size();
/*mexPrintf("n=%d\n",n);
mexPrintf("ija[0]=%d\n",ija[0]);*/
if (ija[0] != n+1)
mexPrintf("Error: \n sprsax: mismatched vector and matrix\n");
/*mexPrintf("ok0a\n");*/
for (i=0;i=thresh && ((i)%(NbRow+1))!=0)
j++;
}
//mexPrintf("j=%d\n",j);
ija_p.Format(j+NbRow+1);
sa_p.Format(j+NbRow+1);
//mexPrintf("nz=%d\n",j+NbRow+1);
for(j=0;j= thresh && i != j)
{
sa_p[++k]=a[j*n+i];
ija_p[k]=j;
}
}
ija_p[i+1]=k+1;
}
}
void LinBCG::sprsin(Mat_DP &a, const DP thresh)
{
int i,j,k=0;
int n=a.nrows();
for(i=0;i=thresh && j!=i)
k++;
ija_p.Format(k+n+1);
sa_p.Format(k+n+1);
for (j=0;j= thresh && i != j) {
//if (++k > nmax) nrerror("sprsin: sa and ija too small");
k++;
sa_p[k]=a[i][j];
ija_p[k]=j;
}
}
ija_p[i+1]=k+1;
}
}
void LinBCG::sprsout(Mat_DP &a)
{
int i,j,k,l,ke;
double maxi=0;
int N=ija_p[0]-1;
//int total=ija_p[N];
//mexPrintf("sprsout\n");
for(i=0;imaxi)
maxi=fabs(sa_p[i]);
int size=int(log10(maxi))+1;
if(size<7)
{
itoa(size+8,fmt,10);
pf= fmt;
pf.insert(0,"% ");
pf.append(".6f ");
pf.copy(fmt,8);
}
else
strcpy(fmt,"%e ");
for(i=0;imaxi)
maxi=fabs(sa_p[i]);
int size=int(log10(maxi))+1;
if(size<7)
{
itoa(size+8,fmt,10);
pf= fmt;
pf.insert(0,"% ");
pf1=pf2=pf;
pf.append(".6f ");
pf1.append("s ");
pf2.append("d ");
pf.copy(fmt,8);
pf1.copy(fmts,8);
pf2.copy(fmtd,8);
}
else
strcpy(fmt,"%e ");
mexPrintf(fmts,"i");
mexPrintf(fmts,"ija");
mexPrintf(fmts,"sa");
if(ijat_p.size())
{
mexPrintf(fmts,"ijat");
mexPrintf(fmts,"sat");
}
mexPrintf("\n");
for(i=0;i EPS*znrm)
{
dxnrm=fabs(ak)*snrm(p,itol);
err=znrm/fabs(zm1nrm-znrm)*dxnrm;
}
else
{
err=znrm/bnrm;
continue;
}
xnrm=snrm(x,itol);
if (err <= 0.5*xnrm)
err /= xnrm;
else
{
err=znrm/bnrm;
continue;
}
}
//cout << "iter=" << setw(4) << iter+1 << setw(12) << err << endl;
mexPrintf("iter=%d err=%f ak=%f akden=%f\n",iter,err,ak,akden);
if (err <= tol)
break;
}
}
void LinBCG::BiCGStab(Vec_I_DP &b, Vec_IO_DP &x, const int itol, const DP tol,
const int itmax, int &iter, DP &err, Mat_DP &a, Vec_IO_INT &indx, const int periods)
{
DP akden,bkden=1.0,bknum,bnrm=0,dxnrm,xnrm,zm1nrm,znrm=0, alpha, omega, beta, omega_num, omega_den, snorm;
const DP EPS=1.0e-14;
int j;
int n=b.size();
//mexPrintf("n=%d\n",n);
Vec_DP p(n),v(n),r(n),rr(n),z(n),s(n), t(n), phat(n), shat(n);
iter=0;
// Compute the residual
atimes(x,r,0);
for (j=0;j EPS*znrm)
{
dxnrm=fabs(alpha)*snrm(p,itol);
err=znrm/fabs(zm1nrm-znrm)*dxnrm;
}
else
{
err=znrm/bnrm;
continue;
}
xnrm=snrm(x,itol);
if (err <= 0.5*xnrm)
err /= xnrm;
else
{
err=znrm/bnrm;
continue;
}
}
//cout << "iter=" << setw(4) << iter+1 << setw(12) << err << endl;
mexPrintf("iter=%d err=%f alpha=%f akden=%f\n",iter,err,alpha,akden);
mexEvalString("drawnow;");
if (err <= tol)
break;
}
}
void LinBCG::sprs_col_index()
{
int i, j, k, begin, end;
int nze=sa_p.size();
ijat_p.Format(nze);
sat_p.Format(nze);
begin=ija_p[0];
int n=begin-1;
//mexPrintf("n=%d\n",n);
map, double> list;
map, double>::iterator list_it;
for(j=0;jfirst.first=%d, list_it->first.second=%d, list_it->second=%f\n",list_it->first.first, list_it->first.second, list_it->second);
while(col!=list_it->first.first)
{
//while(col!=list_it->first.first
col++;
ijat_p[col]=cur_pos;
}
ijat_p[cur_pos]=list_it->first.second;
sat_p[cur_pos++]=list_it->second;
list_it++;
}
}
void LinBCG::sprs_swap_line_copy(map, double> &list, int &pos_2_blck, int begin, int end)
{
int i, j, init_pos;
for(i=begin;i, double> &list, int &pos_2_blck, int LS, int LD)
{
int j;
bool OK=false, OK1=true;
int init_pos=pos_2_blck;
for(j=ija_p[LS];j=LS && OK1)
{
OK1=false;
if(sa_p[LS]!=0.0)
{
//mexPrintf("list[%d,%d]=%f\n",pos_2_blck,LS,sa_p[LS]);
list[make_pair(pos_2_blck++,LS)]=sa_p[LS];
}
}
//mexPrintf("list[%d,%d]=%f\n",pos_2_blck,ija_p[j],sa_p[j]);
list[make_pair(pos_2_blck++,ija_p[j])]=sa_p[j];
}
}
if(OK1)
{
if(sa_p[LS]!=0.0)
{
//mexPrintf("list[%d,%d]=%f\n",pos_2_blck,LS,sa_p[LS]);
list[make_pair(pos_2_blck++,LS)]=sa_p[LS];
}
}
if(!OK)
{
list[make_pair(LD,init_pos)]=0.0;
//mexPrintf("not found list[%d,%d]=%f\n",LD,init_pos,0.0);
}
}
void LinBCG::sprs_swap_line(int L0, int L1)
{
int i,j,k, pos_2_blck, init_pos;
map, double> list;
map, double>::iterator list_it;
int n=ija_p[0]-1;
if(L0>L1)
{
i=L0;
L0=L1;
L1=i;
}
else if(L0==L1)
return;
else if(L0>=n || L1>=n)
{
mexPrintf("out of range in sprs_swap_line (L0=%d and L1=%d)\n",L0, L1);
mexEvalString("drawnow;");
mexEvalString("st=fclose('all');clear all;");
filename+=" stopped";
mexErrMsgTxt(filename.c_str());
}
int nze=sa_p.size();
bool OK, OK1;
int nze0=0, nze1=0;
pos_2_blck=n+1;
sprs_swap_line_copy(list, pos_2_blck, 0, L0);
//mexPrintf("L1=>L0\n");
sprs_swap_line_exchange(list, pos_2_blck, L1, L0);
sprs_swap_line_copy(list, pos_2_blck, L0+1, L1);
//mexPrintf("L0=>L1\n");
sprs_swap_line_exchange(list, pos_2_blck, L0, L1);
sprs_swap_line_copy(list, pos_2_blck, L1+1, n);
sa_p.Format(list.size()+1);
ija_p.Format(list.size()+1);
//mexPrintf("list.size()+1=%d\n",list.size()+1);
list_it=list.begin();
while(list_it!=list.end())
{
//mexPrintf("sa_p[list_it->first.first=%d]=list_it->second=%f\n",list_it->first.first,list_it->second);
sa_p[list_it->first.first]=list_it->second;
//mexPrintf("ija_p[list_it->first.first=%d]=list_it->first.second=%d\n",list_it->first.first,list_it->first.second);
ija_p[list_it->first.first]=list_it->first.second;
list_it++;
}
ija_p[n]=list.size()+1;
sa_p[n]=0;
sprs_col_index();
}
void LinBCG::sprsludcmp(Mat_DP &a, Vec_O_INT &indx, DP &d)
{
const DP TINY=1.0e-20;
int i,imax,j,k, ii, ik, begin_ija, end_ija, begin_ijat, end_ijat, dum1, dum2;
DP big,dum,sum,temp;
Vec_DP saa_p(sa_p);
Vec_INT ijaa_p(ija_p);
int nze=sa_p.size();
int n=ija_p[0]-1;
Vec_DP vv(n);
d=1.0;
n=a.nrows();
//a=(double**)mxMalloc(n*n*sizeof(double));
//mexPrintf("sprsludcmp\n");
for (i=0;i big) big=temp;
}
//mexPrintf("\n");
if(big == 0.0)
{
mexPrintf("Singular Matrix in routine sprsludcmp\n");
mexEvalString("st=fclose('all');clear all;");
mexErrMsgTxt("End of simulate");
}
vv[i]=1.0/big;
}
/*mexPrintf("First Step\n");
mexPrintf("a.ncols()=%d\n",a.ncols());
mexPrintf("a.nrows()=%d\n",a.nrows());*/
/*for(i=0;ibig)
big=temp;
if(big==0.0)
{
mexPrintf("Singular Matrix in routine sprsludcmp\n");
mexErrMsgTxt("End of simulate");
}
vv[i]=1.0/big;
}*/
/*sprs_col_index(); // create a col-index compact storage
for (j=0;jijat_p[begin_ijat])
begin_ijat++;
else
sum -= sa_p[begin_ija++]*sat_p[begin_ijat++];
}
sat_p[i]=sum;
}
big=0.0;
for (;iijat_p[begin_ijat])
begin_ijat++;
else
sum -= sa_p[begin_ija++]*sat_p[begin_ijat++];
}
sat_p[i]=sum;
if((dum=vv[ija_p[i]]*fabs(sum)) >= big)
{
big=dum;
mexPrintf("imax=i=%d\n",i);
imax=ija_p[i];
}
}
if (j != imax)
{
sprs_swap_line(j,imax);
//for (k=0;kj)
sa_p[i] *= dum;
}
}
}*/
for (j=0;j= big)
{
big=dum;
imax=i;
}
}
//mexPrintf("ok1\n");
if (j != imax)
{
for (k=0;k=0;k--)
{
for (i=n-1;i>=0;i--)
{
sum=b[i+k*n];
for (j=i+1;j=0;k--)
{
for (i=n-1;i>=0;i--)
{
sum=b[i+k*n];
for (j=i+1;j=0;i--)
{
sum=b[i];
for (j=i+1;j (LU)'.x=b <=> U'L'x=b <=> U'.y=b with x solution of L'x=y
int i,ii,ip,j;
DP sum;
int n=a.nrows();
ii=0;
// Solving U'.y=b
// U' is a lower triangular matrix with element on the main diagonal differnet from 1
// Implementation of a foreward substitution to solve this system
for (i=0;i=0;i--)
{
sum=b[i];
for (j=i+1;j