ADMB Documentation  11.1.2249
 All Classes Files Functions Variables Typedefs Friends Defines
fvar_ma4.cpp
Go to the documentation of this file.
00001 
00007 #include "fvar.hpp"
00008 #include <math.h>
00009 
00010 static double eps0=1.e-50;
00011 
00012 void lubksb(dvar_matrix a, const ivector&  indx,dvar_vector b);
00013 void ludcmp(const dvar_matrix& a, const ivector& indx, const prevariable& d);
00014 
00023 void ludcmp(const dvar_matrix& _a, const ivector& _indx, const prevariable& _d)
00024 {
00025   ADUNCONST(dvar_matrix,a)
00026   ADUNCONST(prevariable,d)
00027   ivector& indx= (ivector&) _indx;
00028   int i,imax,j,k,n;
00029 
00030   n=a.colsize();
00031   int lb=a.colmin();
00032   int ub=a.colmax();
00033 
00034   dvariable big,dum,sum,temp;
00035 
00036   dvar_vector vv(lb,ub);
00037 
00038 
00039   d=1.0;
00040 
00041   for (i=lb;i<=ub;i++)
00042   {
00043     big=0.0;
00044     for (j=lb;j<=ub;j++)
00045     {
00046       temp=fabs(a(i,j));
00047       if (temp > big)
00048       {
00049         big=temp;
00050       }
00051     }
00052     if (big == 0.0)
00053     {
00054       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00055     }
00056     vv(i)=1.0/big;
00057   }
00058 
00059   for (j=lb;j<=ub;j++)
00060   {
00061     for (i=lb;i<j;i++)
00062     {
00063       sum=a(i,j);
00064       for (k=lb;k<i;k++)
00065       {
00066         sum = sum - a(i,k)*a(k,j);
00067       }
00068       a(i,j)=sum;
00069     }
00070     big=0.0;
00071     for (i=j;i<=ub;i++)
00072     {
00073       sum=a(i,j);
00074       for (k=lb;k<j;k++)
00075       {
00076         sum = sum - a(i,k)*a(k,j);
00077       }
00078       a(i,j)=sum;
00079       dum=vv(i)*fabs(sum);
00080       if ( dum >= big)
00081       {
00082         big=dum;
00083         imax=i;
00084       }
00085     }
00086     if (j != imax)
00087     {
00088       for (k=lb;k<=ub;k++)
00089       {
00090         dum=a(imax,k);
00091         a(imax,k)=a(j,k);
00092         a(j,k)=dum;
00093       }
00094       d = -1.*d;
00095       vv(imax)=vv(j);
00096     }
00097     indx(j)=imax;
00098 
00099     if (a(j,j) == 0.0)
00100     {
00101       a(j,j)=eps0;
00102     }
00103 
00104     if (j != n)
00105     {
00106       dum=1.0/(a(j,j));
00107       for (i=j+1;i<=ub;i++)
00108       {
00109         a(i,j) = a(i,j) * dum;
00110       }
00111     }
00112   }
00113 }
00114 
00124 void lubksb(dvar_matrix a, const ivector& indx,dvar_vector b)
00125 {
00126   int i,ii=0,ip,j,iiflag=0;
00127   dvariable sum;
00128   int lb=a.colmin();
00129   int ub=a.colmax();
00130   for (i=lb;i<=ub;i++)
00131   {
00132     ip=indx(i);
00133     sum=b(ip);
00134     b(ip)=b(i);
00135     if (iiflag)
00136     {
00137       for (j=ii;j<=i-1;j++)
00138       {
00139         sum -= a.elem(i,j)*b.elem(j);
00140       }
00141     }
00142     else if ( value(sum) )
00143     {
00144       ii=i;
00145       iiflag=1;
00146     }
00147     b(i)=sum;
00148   }
00149 
00150   for (i=ub;i>=lb;i--)
00151   {
00152     sum=b(i);
00153     for (j=i+1;j<=ub;j++)
00154     {                        // !!! remove to show bug
00155       sum -= a.elem(i,j)*b.elem(j);
00156     }                        // !!! remove to show bug
00157     b.elem(i)=sum/a.elem(i,i);
00158   }
00159 }
00160 
00161