ADMB Documentation  11.1x.2711
 All Classes Files Functions Variables Typedefs Friends Defines
dmat34.cpp
Go to the documentation of this file.
00001 
00007 #include <fvar.hpp>
00008 
00009 #ifdef __TURBOC__
00010   #pragma hdrstop
00011   #include <iostream.h>
00012 #endif
00013 
00014 #if defined (__WAT32__)
00015   #include <iostream.h>
00016   #include <strstrea.h>
00017 #endif
00018 
00019 #ifdef __ZTC__
00020   #include <iostream.hpp>
00021 #endif
00022 
00023 #define TINY 1.0e-20;
00024 void dmdv_solve(void);
00025 
00026 
00028 dvector csolve(const dmatrix& aa,const dvector& z)
00029 {
00030   double ln_unsigned_det;
00031   double sign;
00032   dvector sol=solve(aa,z,ln_unsigned_det,sign);
00033   return sol;
00034 }
00035 
00042 dvector solve(const dmatrix& aa,const dvector& z)
00043 {
00044   double ln_unsigned_det;
00045   double sign;
00046   dvector sol=solve(aa,z,ln_unsigned_det,sign);
00047   return sol;
00048 }
00049 
00062 dvector solve(const dmatrix& aa,const dvector& z,
00063   const double& _ln_unsigned_det,double& sign)
00064 {
00065   double& ln_unsigned_det=(double&) (_ln_unsigned_det);
00066   int n=aa.colsize();
00067   int lb=aa.colmin();
00068   int ub=aa.colmax();
00069   if (lb!=aa.rowmin()||ub!=aa.rowmax())
00070   {
00071     cerr << "Error matrix not square in solve()"<<endl;
00072     ad_exit(1);
00073   }
00074   dmatrix bb(lb,ub,lb,ub);
00075   bb=aa;
00076   ivector indx(lb,ub);
00077   int One=1;
00078   indx.fill_seqadd(lb,One);
00079   double d;
00080   double big,dum,sum,temp;
00081   dvector vv(lb,ub);
00082 
00083   d=1.0;
00084   for (int i=lb;i<=ub;i++)
00085   {
00086     big=0.0;
00087     for (int j=lb;j<=ub;j++)
00088     {
00089       temp=fabs(bb(i,j));
00090       if (temp > big)
00091       {
00092         big=temp;
00093       }
00094     }
00095     if (big == 0.0)
00096     {
00097       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00098     }
00099     vv[i]=1.0/big;
00100   }
00101 
00102   for (int j=lb;j<=ub;j++)
00103   {
00104     for (int i=lb;i<j;i++)
00105     {
00106       sum=bb(i,j);
00107       for (int k=lb;k<i;k++)
00108       {
00109         sum -= bb(i,k)*bb(k,j);
00110       }
00111       //a[i][j]=sum;
00112       bb(i,j)=sum;
00113     }
00114     int imax = j;
00115     big=0.0;
00116     for (int i=j;i<=ub;i++)
00117     {
00118       sum=bb(i,j);
00119       for (int k=lb;k<j;k++)
00120       {
00121         sum -= bb(i,k)*bb(k,j);
00122       }
00123       bb(i,j)=sum;
00124       dum=vv[i]*fabs(sum);
00125       if ( dum >= big)
00126       {
00127         big=dum;
00128         imax=i;
00129       }
00130     }
00131     if (j != imax)
00132     {
00133       for (int k=lb;k<=ub;k++)
00134       {
00135         dum=bb(imax,k);
00136         bb(imax,k)=bb(j,k);
00137         bb(j,k)=dum;
00138       }
00139       d = -1.*d;
00140       vv[imax]=vv[j];
00141 
00142       //if (j<ub)
00143       {
00144         int itemp=indx(imax);
00145         indx(imax)=indx(j);
00146         indx(j)=itemp;
00147       }
00148       //cout << "indx= " <<indx<<endl;
00149     }
00150 
00151     if (bb(j,j) == 0.0)
00152     {
00153       bb(j,j)=TINY;
00154     }
00155 
00156     if (j != n)
00157     {
00158       dum=1.0/bb(j,j);
00159       for (int i=j+1;i<=ub;i++)
00160       {
00161         bb(i,j) = bb(i,j) * dum;
00162       }
00163     }
00164   }
00165 
00166   // get the determinant
00167   sign=d;
00168   dvector part_prod(lb,ub);
00169   part_prod(lb)=log(fabs(bb(lb,lb)));
00170   if (bb(lb,lb)<0) sign=-sign;
00171   for (int j=lb+1;j<=ub;j++)
00172   {
00173     if (bb(j,j)<0) sign=-sign;
00174     part_prod(j)=part_prod(j-1)+log(fabs(bb(j,j)));
00175   }
00176   ln_unsigned_det=part_prod(ub);
00177 
00178   dvector x(lb,ub);
00179   dvector y(lb,ub);
00180   //int lb=rowmin;
00181   //int ub=rowmax;
00182   dmatrix& b=bb;
00183   ivector indxinv(lb,ub);
00184   for (int i=lb;i<=ub;i++)
00185   {
00186     indxinv(indx(i))=i;
00187   }
00188 
00189   for (int i=lb;i<=ub;i++)
00190   {
00191     y(indxinv(i))=z(i);
00192   }
00193 
00194   for (int i=lb;i<=ub;i++)
00195   {
00196     sum=y(i);
00197     for (int j=lb;j<=i-1;j++)
00198     {
00199       sum-=b(i,j)*y(j);
00200     }
00201     y(i)=sum;
00202   }
00203   for (int i=ub;i>=lb;i--)
00204   {
00205     sum=y(i);
00206     for (int j=i+1;j<=ub;j++)
00207     {
00208       sum-=b(i,j)*x(j);
00209     }
00210     x(i)=sum/b(i,i);
00211   }
00212 
00213   return x;
00214 }
00215 #undef TINY