ADMB Documentation  11.2.2828
 All Classes Files Functions Variables Typedefs Friends Defines
df1b2-separable/expm.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: expm.cpp 2584 2014-11-07 18:41:56Z johnoel $
00003  *
00004  * Authors: Anders Nielsen <anders@nielsensweb.org> and Casper W. Berg <cbe@aqua.dtu.dk>
00005  * Copyright (c) 2008-2012 Regents of the University of California
00006  */
00012 #include <df1b2fun.h>
00013 #define TINY 1.0e-20;
00014 
00019 df1b2matrix solve(const df1b2matrix& aa,const df1b2matrix& tz)
00020 {
00021   df1b2variable ln;
00022   df1b2variable sgn;
00023   return solve(aa,tz,ln,sgn);
00024 }
00025 
00033 df1b2matrix solve(const df1b2matrix& aa,const df1b2matrix& tz,
00034   df1b2variable ln_unsigned_det,df1b2variable& sign)
00035 {
00036   RETURN_ARRAYS_INCREMENT();
00037   int n = aa.colsize();
00038   int lb = aa.colmin();
00039   int ub = aa.colmax();
00040   if (lb!=aa.rowmin()||ub!=aa.colmax())
00041   {
00042     cerr << "Error matrix not square in solve()"<<endl;
00043     ad_exit(1);
00044   }
00045   df1b2matrix bb(lb,ub,lb,ub);
00046   bb = aa;
00047   ivector indx(lb,ub);
00048   int One = 1;
00049   indx.fill_seqadd(lb,One);
00050   df1b2variable d;
00051   df1b2variable big,dum,sum,temp;
00052   df1b2vector vv(lb,ub);
00053 
00054   d = 1.0;
00055   for (int i = lb;i<=ub;i++)
00056   {
00057     big = 0.0;
00058     for (int j = lb;j<=ub;j++)
00059     {
00060       temp = fabs(bb(i,j));
00061       if (value(temp) > value(big))
00062       {
00063         big = temp;
00064       }
00065     }
00066     if (value(big) == 0.0)
00067     {
00068       cerr <<
00069         "Error in matrix inverse -- matrix singular in inv(df1b2matrix)\n";
00070     }
00071     vv[i] = 1.0/big;
00072   }
00073 
00074   for (int j = lb;j<=ub;j++)
00075   {
00076     for (int i = lb;i<j;i++)
00077     {
00078       sum = bb(i,j);
00079       for (int k = lb;k<i;k++)
00080       {
00081         sum -= bb(i,k)*bb(k,j);
00082       }
00083       // a[i][j] = sum;
00084       bb(i,j) = sum;
00085     }
00086     int imax = j;
00087     big = 0.0;
00088     for (int i = j;i<=ub;i++)
00089     {
00090       sum = bb(i,j);
00091       for (int k = lb;k<j;k++)
00092       {
00093         sum -= bb(i,k)*bb(k,j);
00094       }
00095       bb(i,j) = sum;
00096       dum = vv[i]*fabs(sum);
00097       if (value(dum) >= value(big))
00098       {
00099         big = dum;
00100         imax = i;
00101       }
00102     }
00103     if (j != imax)
00104     {
00105       for (int k = lb;k<=ub;k++)
00106       {
00107         dum = bb(imax,k);
00108         bb(imax,k) = bb(j,k);
00109         bb(j,k) = dum;
00110       }
00111       d = -1.*d;
00112       vv[imax] = vv[j];
00113 
00114       // if (j<ub)
00115       {
00116         int itemp = indx(imax);
00117         indx(imax) = indx(j);
00118         indx(j) = itemp;
00119       }
00120       // cout << "indx= " <<indx<<endl;
00121     }
00122 
00123     if (value(bb(j,j)) == value(0.0))
00124     {
00125       bb(j,j) = TINY;
00126     }
00127 
00128     if (j != n)
00129     {
00130       dum = 1.0/bb(j,j);
00131       for (int i = j+1;i<=ub;i++)
00132       {
00133         bb(i,j) = bb(i,j) * dum;
00134       }
00135     }
00136   }
00137 
00138   // get the determinant
00139   sign = d;
00140   df1b2vector part_prod(lb,ub);
00141   part_prod(lb) = log(fabs(bb(lb,lb)));
00142   if (value(bb(lb,lb))<0) sign=-sign;
00143   for (int j = lb+1;j<=ub;j++)
00144   {
00145       if (value(bb(j,j))<0) sign=-sign;
00146     part_prod(j) = part_prod(j-1)+log(fabs(bb(j,j)));
00147   }
00148   ln_unsigned_det = part_prod(ub);
00149 
00150   df1b2matrix z = trans(tz);
00151   int mmin = z.indexmin();
00152   int mmax = z.indexmax();
00153   df1b2matrix x(mmin,mmax,lb,ub);
00154   // df1b2vector x(lb,ub);
00155 
00156   df1b2vector y(lb,ub);
00157   // int lb = rowmin;
00158   // int ub = rowmax;
00159   df1b2matrix& b = bb;
00160   ivector indxinv(lb,ub);
00161   for (int i = lb;i<=ub;i++)
00162   {
00163     indxinv(indx(i)) = i;
00164   }
00165   for (int kk = mmin;kk<=mmax;kk++)
00166   {
00167     for (int i = lb;i<=ub;i++)
00168     {
00169       y(indxinv(i)) = z(kk)(i);
00170     }
00171 
00172     for (int i = lb;i<=ub;i++)
00173     {
00174       sum = y(i);
00175       for (int j = lb;j<=i-1;j++)
00176       {
00177         sum-=b(i,j)*y(j);
00178       }
00179       y(i) = sum;
00180     }
00181     for (int i = ub;i>=lb;i--)
00182     {
00183       sum = y(i);
00184       for (int j = i+1;j<=ub;j++)
00185       {
00186         sum-=b(i,j)*x(kk)(j);
00187       }
00188       x(kk)(i) = sum/b(i,i);
00189     }
00190   }
00191   RETURN_ARRAYS_DECREMENT();
00192   return trans(x);
00193 }
00194 
00213 df1b2matrix expm(const df1b2matrix & A)
00214 {
00215   RETURN_ARRAYS_INCREMENT();
00216   int rmin = A.rowmin();
00217   int rmax = A.rowmax();
00218 
00219   if(rmax != A.colmax())
00220     {cout<<"Error: Not square matrix in expm."<<endl; ad_exit(1);}
00221   if(rmin != A.colmin())
00222     {cout<<"Error: Not square matrix in expm."<<endl; ad_exit(1);}
00223 
00224   df1b2matrix I(rmin,rmax,rmin,rmax);
00225   df1b2matrix AA(rmin,rmax,rmin,rmax);
00226   df1b2matrix X(rmin,rmax,rmin,rmax);
00227   df1b2matrix E(rmin,rmax,rmin,rmax);
00228   df1b2matrix D(rmin,rmax,rmin,rmax);
00229   df1b2matrix cX(rmin,rmax,rmin,rmax);
00230 
00231   I.initialize();
00232   for(int i = rmin; i<=rmax; ++i){I(i,i) = 1.0;}
00233 
00234   df1b2variable log2NormInf;
00235   log2NormInf = log(max(rowsum(fabs(value(A)))));
00236   log2NormInf/=log(2.0);
00237   int e = (int)value(log2NormInf) + 1;
00238   int s = e+1;
00239   s = (s<0) ? 0 : s;
00240   AA = 1.0/pow(2.0,s)*A;
00241 
00242   X = AA;
00243   df1b2variable c = 0.5;
00244 
00245   E = I+c*AA;
00246   D = I-c*AA;
00247   int q = 6, p = 1;
00248   for(int k = 2;  k<=q; ++k){
00249     c*=((double)q-k+1.0)/((double)k*(2*q-k+1));
00250     X = AA*X;
00251     cX = c*X;
00252     E+=cX;
00253     if(p==1){D+=cX;}else{D-=cX;}
00254     p = (p==1) ? 0 : 1;
00255   }
00256   // E = inv(D)*E;
00257   E = solve(D,E);
00258   for(int k = 1; k<=s; ++k){
00259     E = E*E;
00260   }
00261   RETURN_ARRAYS_DECREMENT();
00262   return E;
00263 }
00264 
00265 #undef TINY