ADMB Documentation  11.1.2503
 All Classes Files Functions Variables Typedefs Friends Defines
linad99/expm.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: expm.cpp 2251 2014-08-26 22:12:11Z johnoel $
00003  *
00004  * Authors: Anders Nielsen <anders@nielsensweb.org> and Casper Berg <cbe@aqua.dtu.dk>
00005  * Copyright (c) 2008-2012 Regents of the University of California
00006  */
00011 #include <fvar.hpp>
00012 #define TINY 1.0e-20;
00013 
00018 dmatrix fabs(const dmatrix & X){
00019   int rmin = X.rowmin();
00020   int rmax = X.rowmax();
00021   int cmin = X.colmin();
00022   int cmax = X.colmax();
00023   dmatrix ret(rmin,rmax,cmin,cmax);
00024   for(int i = rmin; i<=rmax; ++i){
00025     for(int j = cmin; j<=cmax; ++j){
00026       ret(i,j) = fabs(X(i,j));
00027     }
00028   }
00029   return ret;
00030 }
00031 
00032 dvar_matrix solve(const dvar_matrix& aa, const dvar_matrix& tz,
00033   dvariable ln_unsigned_det, dvariable& sign);
00034 
00035 dvar_matrix solve(const dvar_matrix& aa, const dvar_matrix& tz)
00036 {
00037   dvariable ln;
00038   dvariable sgn;
00039   return solve(aa,tz,ln,sgn);
00040 }
00041 
00063 dmatrix expm(const dmatrix& A)
00064 {
00065   int rmin = A.rowmin();
00066   int rmax = A.rowmax();
00067   if(rmax != A.colmax())
00068   {
00069     cout<<"Error: Not square matrix in expm."<<endl;
00070     ad_exit(1);
00071   }
00072   if(rmin != A.colmin())
00073   {
00074     cout<<"Error: Not square matrix in expm."<<endl;
00075     ad_exit(1);
00076   }
00077   dmatrix I(rmin,rmax,rmin,rmax);
00078   dmatrix AA(rmin,rmax,rmin,rmax);
00079   dmatrix X(rmin,rmax,rmin,rmax);
00080   dmatrix E(rmin,rmax,rmin,rmax);
00081   dmatrix D(rmin,rmax,rmin,rmax);
00082   dmatrix cX(rmin,rmax,rmin,rmax);
00083   I.initialize();
00084   for(int i = rmin; i<=rmax; ++i){I(i,i) = 1.0;}
00085   double log2NormInf;
00086   log2NormInf = log(max(rowsum(fabs(A))));
00087   log2NormInf/=log(2.0);
00088   int e = (int)log2NormInf + 1;
00089   int s = e+1;
00090   s = (s<0) ? 0 : s;
00091   AA = 1.0/pow(2.0,s)*A;
00092   X = AA;
00093   double c = 0.5;
00094 
00095   E = I+c*AA;
00096   D = I-c*AA;
00097   int q = 6, p = 1;
00098   for(int k = 2;  k<=q; ++k){
00099     c*=((double)q-k+1.0)/((double)k*(2*q-k+1));
00100     X = AA*X;
00101     cX = c*X;
00102     E+=cX;
00103     if(p==1){D+=cX;}else{D-=cX;}
00104     p = (p==1) ? 0 : 1;
00105   }
00106   // E = inv(D)*E;
00107   E = solve(D,E);
00108   for(int k = 1; k<=s; ++k){
00109     E = E*E;
00110   }
00111   return E;
00112 }
00113 
00136 dvar_matrix expm(const dvar_matrix& A)
00137 {
00138   RETURN_ARRAYS_INCREMENT();
00139   int rmin = A.rowmin();
00140   int rmax = A.rowmax();
00141 
00142   if(rmax != A.colmax())
00143   {
00144     cout<<"Error: Not square matrix in expm."<<endl;
00145     ad_exit(1);
00146   }
00147   if(rmin != A.colmin())
00148   {
00149     cout<<"Error: Not square matrix in expm."<<endl;
00150     ad_exit(1);
00151   }
00152 
00153   dvar_matrix I(rmin,rmax,rmin,rmax);
00154   dvar_matrix AA(rmin,rmax,rmin,rmax);
00155   dvar_matrix X(rmin,rmax,rmin,rmax);
00156   dvar_matrix E(rmin,rmax,rmin,rmax);
00157   dvar_matrix D(rmin,rmax,rmin,rmax);
00158   dvar_matrix cX(rmin,rmax,rmin,rmax);
00159 
00160   I.initialize();
00161   for(int i = rmin; i<=rmax; ++i){I(i,i) = 1.0;}
00162 
00163   dvariable log2NormInf;
00164   log2NormInf = log(max(rowsum(fabs(value(A)))));
00165   log2NormInf/=log(2.0);
00166   int e = (int)value(log2NormInf) + 1;
00167   int s = e+1;
00168   s = (s<0) ? 0 : s;
00169   AA = 1.0/pow(2.0,s)*A;
00170 
00171   X = AA;
00172   dvariable c = 0.5;
00173 
00174   E = I+c*AA;
00175   D = I-c*AA;
00176   int q = 6, p = 1, k;
00177   for(k = 2;  k<=q; ++k){
00178     c*=((double)q-k+1.0)/((double)k*(2*q-k+1));
00179     X = AA*X;
00180     cX = c*X;
00181     E+=cX;
00182     if(p==1){D+=cX;}else{D-=cX;}
00183     p = (p==1) ? 0 : 1;
00184   }
00185   // E = inv(D)*E;
00186   E = solve(D,E);
00187   for(k = 1; k<=s; ++k){
00188     E = E*E;
00189   }
00190   RETURN_ARRAYS_DECREMENT();
00191   return E;
00192 }
00193 
00194 dvar_matrix solve(const dvar_matrix& aa, const dvar_matrix& tz,
00195   dvariable ln_unsigned_det, dvariable& sign)
00196 {
00197   RETURN_ARRAYS_INCREMENT();
00198   int i,j,k,n;
00199   n=aa.colsize();
00200   int lb=aa.colmin();
00201   int ub=aa.colmax();
00202   if (lb!=aa.rowmin()||ub!=aa.colmax())
00203   {
00204     cerr << "Error matrix not square in solve()"<<endl;
00205     ad_exit(1);
00206   }
00207   dvar_matrix bb(lb,ub,lb,ub);
00208   bb=aa;
00209   ivector indx(lb,ub);
00210   int One=1;
00211   indx.fill_seqadd(lb,One);
00212   dvariable d;
00213   dvariable big,dum,sum,temp;
00214   dvar_vector vv(lb,ub);
00215 
00216   d=1.0;
00217   for (i=lb;i<=ub;i++)
00218   {
00219     big=0.0;
00220     for (j=lb;j<=ub;j++)
00221     {
00222       temp=fabs(bb(i,j));
00223       if (temp > big)
00224       {
00225         big=temp;
00226       }
00227     }
00228     if (big == 0.0)
00229     {
00230      cerr << "Error in matrix inverse -- matrix singular in inv(dvar_matrix)\n";
00231     }
00232     vv[i]=1.0/big;
00233   }
00234 
00235   for (j=lb;j<=ub;j++)
00236   {
00237     for (i=lb;i<j;i++)
00238     {
00239       sum=bb(i,j);
00240       for (k=lb;k<i;k++)
00241       {
00242         sum -= bb(i,k)*bb(k,j);
00243       }
00244       //a[i][j]=sum;
00245       bb(i,j)=sum;
00246     }
00247     int imax = j;
00248     big=0.0;
00249     for (i=j;i<=ub;i++)
00250     {
00251       sum=bb(i,j);
00252       for (k=lb;k<j;k++)
00253       {
00254         sum -= bb(i,k)*bb(k,j);
00255       }
00256       bb(i,j)=sum;
00257       dum=vv[i]*fabs(sum);
00258       if ( dum >= big)
00259       {
00260         big=dum;
00261         imax=i;
00262       }
00263     }
00264     if (j != imax)
00265     {
00266       for (k=lb;k<=ub;k++)
00267       {
00268         dum=bb(imax,k);
00269         bb(imax,k)=bb(j,k);
00270         bb(j,k)=dum;
00271       }
00272       d = -1.*d;
00273       vv[imax]=vv[j];
00274 
00275       //if (j<ub)
00276       {
00277         int itemp=indx(imax);
00278         indx(imax)=indx(j);
00279         indx(j)=itemp;
00280       }
00281       //cout << "indx= " <<indx<<endl;
00282     }
00283 
00284     if (bb(j,j) == 0.0)
00285     {
00286       bb(j,j)=TINY;
00287     }
00288 
00289     if (j != n)
00290     {
00291       dum=1.0/bb(j,j);
00292       for (i=j+1;i<=ub;i++)
00293       {
00294         bb(i,j) = bb(i,j) * dum;
00295       }
00296     }
00297   }
00298 
00299   // get the determinant
00300   sign=d;
00301   dvar_vector part_prod(lb,ub);
00302   part_prod(lb)=log(fabs(bb(lb,lb)));
00303   if (bb(lb,lb)<0) sign=-sign;
00304   for (j=lb+1;j<=ub;j++)
00305   {
00306     if (bb(j,j)<0) sign=-sign;
00307     part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j)));
00308   }
00309   ln_unsigned_det=part_prod(ub);
00310 
00311   dvar_matrix z=trans(tz);
00312   int mmin=z.indexmin();
00313   int mmax=z.indexmax();
00314   dvar_matrix x(mmin,mmax,lb,ub);
00315   //dvar_vector x(lb,ub);
00316 
00317   dvar_vector y(lb,ub);
00318   //int lb=rowmin;
00319   //int ub=rowmax;
00320   dvar_matrix& b=bb;
00321   ivector indxinv(lb,ub);
00322   for (i=lb;i<=ub;i++)
00323   {
00324     indxinv(indx(i))=i;
00325   }
00326   for (int kk=mmin;kk<=mmax;kk++)
00327   {
00328     for (i=lb;i<=ub;i++)
00329     {
00330       y(indxinv(i))=z(kk)(i);
00331     }
00332 
00333     for (i=lb;i<=ub;i++)
00334     {
00335       sum=y(i);
00336       for (int j=lb;j<=i-1;j++)
00337       {
00338         sum-=b(i,j)*y(j);
00339       }
00340       y(i)=sum;
00341     }
00342     for (i=ub;i>=lb;i--)
00343     {
00344       sum=y(i);
00345       for (int j=i+1;j<=ub;j++)
00346       {
00347         sum-=b(i,j)*x(kk)(j);
00348       }
00349       x(kk)(i)=sum/b(i,i);
00350     }
00351   }
00352   RETURN_ARRAYS_DECREMENT();
00353   return trans(x);
00354 }
00355 #undef TINY