ADMB Documentation  11.5.3197
 All Classes Files Functions Variables Typedefs Friends Defines
linad99/expm.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id$
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 n = aa.colsize();
00199   int lb=aa.colmin();
00200   int ub=aa.colmax();
00201   if (lb!=aa.rowmin()||ub!=aa.colmax())
00202   {
00203     cerr << "Error matrix not square in solve()"<<endl;
00204     ad_exit(1);
00205   }
00206   dvar_matrix bb(lb,ub,lb,ub);
00207   bb=aa;
00208   ivector indx(lb,ub);
00209   int One=1;
00210   indx.fill_seqadd(lb,One);
00211   dvariable d;
00212   dvariable big,dum,sum,temp;
00213   dvar_vector vv(lb,ub);
00214 
00215   d=1.0;
00216   for (int i=lb;i<=ub;i++)
00217   {
00218     big=0.0;
00219     for (int j=lb;j<=ub;j++)
00220     {
00221       temp=fabs(bb(i,j));
00222       if (temp > big)
00223       {
00224         big=temp;
00225       }
00226     }
00227     if (big == 0.0)
00228     {
00229      cerr << "Error in matrix inverse -- matrix singular in inv(dvar_matrix)\n";
00230     }
00231     vv[i]=1.0/big;
00232   }
00233 
00234   for (int j=lb;j<=ub;j++)
00235   {
00236     for (int i=lb;i<j;i++)
00237     {
00238       sum=bb(i,j);
00239       for (int k=lb;k<i;k++)
00240       {
00241         sum -= bb(i,k)*bb(k,j);
00242       }
00243       //a[i][j]=sum;
00244       bb(i,j)=sum;
00245     }
00246     int imax = j;
00247     big=0.0;
00248     for (int i=j;i<=ub;i++)
00249     {
00250       sum=bb(i,j);
00251       for (int k=lb;k<j;k++)
00252       {
00253         sum -= bb(i,k)*bb(k,j);
00254       }
00255       bb(i,j)=sum;
00256       dum=vv[i]*fabs(sum);
00257       if ( dum >= big)
00258       {
00259         big=dum;
00260         imax=i;
00261       }
00262     }
00263     if (j != imax)
00264     {
00265       for (int k=lb;k<=ub;k++)
00266       {
00267         dum=bb(imax,k);
00268         bb(imax,k)=bb(j,k);
00269         bb(j,k)=dum;
00270       }
00271       d = -1.*d;
00272       vv[imax]=vv[j];
00273 
00274       //if (j<ub)
00275       {
00276         int itemp=indx(imax);
00277         indx(imax)=indx(j);
00278         indx(j)=itemp;
00279       }
00280       //cout << "indx= " <<indx<<endl;
00281     }
00282 
00283     if (bb(j,j) == 0.0)
00284     {
00285       bb(j,j)=TINY;
00286     }
00287 
00288     if (j != n)
00289     {
00290       dum=1.0/bb(j,j);
00291       for (int i=j+1;i<=ub;i++)
00292       {
00293         bb(i,j) = bb(i,j) * dum;
00294       }
00295     }
00296   }
00297 
00298   // get the determinant
00299   sign=d;
00300   dvar_vector part_prod(lb,ub);
00301   part_prod(lb)=log(fabs(bb(lb,lb)));
00302   if (bb(lb,lb)<0) sign=-sign;
00303   for (int j=lb+1;j<=ub;j++)
00304   {
00305     if (bb(j,j)<0) sign=-sign;
00306     part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j)));
00307   }
00308   ln_unsigned_det=part_prod(ub);
00309 
00310   dvar_matrix z=trans(tz);
00311   int mmin=z.indexmin();
00312   int mmax=z.indexmax();
00313   dvar_matrix x(mmin,mmax,lb,ub);
00314   //dvar_vector x(lb,ub);
00315 
00316   dvar_vector y(lb,ub);
00317   //int lb=rowmin;
00318   //int ub=rowmax;
00319   dvar_matrix& b=bb;
00320   ivector indxinv(lb,ub);
00321   for (int i=lb;i<=ub;i++)
00322   {
00323     indxinv(indx(i))=i;
00324   }
00325   for (int kk=mmin;kk<=mmax;kk++)
00326   {
00327     for (int i=lb;i<=ub;i++)
00328     {
00329       y(indxinv(i))=z(kk)(i);
00330     }
00331 
00332     for (int i=lb;i<=ub;i++)
00333     {
00334       sum=y(i);
00335       for (int j=lb;j<=i-1;j++)
00336       {
00337         sum-=b(i,j)*y(j);
00338       }
00339       y(i)=sum;
00340     }
00341     for (int i=ub;i>=lb;i--)
00342     {
00343       sum=y(i);
00344       for (int j=i+1;j<=ub;j++)
00345       {
00346         sum-=b(i,j)*x(kk)(j);
00347       }
00348       x(kk)(i)=sum/b(i,i);
00349     }
00350   }
00351   RETURN_ARRAYS_DECREMENT();
00352   return trans(x);
00353 }
00354 #undef TINY