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