ADMB Documentation  11.1x.2711
 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 #ifdef ISZERO
00011   #undef ISZERO
00012 #endif
00013 #define ISZERO(d) ((d)==0.0)
00014 
00015 static double eps0=1.e-50;
00016 
00017 void lubksb(dvar_matrix a, const ivector&  indx,dvar_vector b);
00018 void ludcmp(const dvar_matrix& a, const ivector& indx, const prevariable& d);
00019 
00028 void ludcmp(const dvar_matrix& _a, const ivector& _indx, const prevariable& _d)
00029 {
00030   ADUNCONST(dvar_matrix,a)
00031   ADUNCONST(prevariable,d)
00032   ivector& indx= (ivector&) _indx;
00033   int i,j,k,n;
00034 
00035   n=a.colsize();
00036   int lb=a.colmin();
00037   int ub=a.colmax();
00038 
00039   dvariable big,dum,sum,temp;
00040 
00041   dvar_vector vv(lb,ub);
00042 
00043 
00044   d=1.0;
00045 
00046   for (i=lb;i<=ub;i++)
00047   {
00048     big=0.0;
00049     for (j=lb;j<=ub;j++)
00050     {
00051       temp=fabs(a(i,j));
00052       if (temp > big)
00053       {
00054         big=temp;
00055       }
00056     }
00057     if (big == 0.0)
00058     {
00059       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00060     }
00061     vv(i)=1.0/big;
00062   }
00063 
00064   for (j=lb;j<=ub;j++)
00065   {
00066     for (i=lb;i<j;i++)
00067     {
00068       sum=a(i,j);
00069       for (k=lb;k<i;k++)
00070       {
00071         sum = sum - a(i,k)*a(k,j);
00072       }
00073       a(i,j)=sum;
00074     }
00075     int imax = j;
00076     big=0.0;
00077     for (i=j;i<=ub;i++)
00078     {
00079       sum=a(i,j);
00080       for (k=lb;k<j;k++)
00081       {
00082         sum = sum - a(i,k)*a(k,j);
00083       }
00084       a(i,j)=sum;
00085       dum=vv(i)*fabs(sum);
00086       if ( dum >= big)
00087       {
00088         big=dum;
00089         imax=i;
00090       }
00091     }
00092     if (j != imax)
00093     {
00094       for (k=lb;k<=ub;k++)
00095       {
00096         dum=a(imax,k);
00097         a(imax,k)=a(j,k);
00098         a(j,k)=dum;
00099       }
00100       d = -1.*d;
00101       vv(imax)=vv(j);
00102     }
00103     indx(j)=imax;
00104 
00105     if (a(j,j) == 0.0)
00106     {
00107       a(j,j)=eps0;
00108     }
00109 
00110     if (j != n)
00111     {
00112       dum=1.0/(a(j,j));
00113       for (i=j+1;i<=ub;i++)
00114       {
00115         a(i,j) = a(i,j) * dum;
00116       }
00117     }
00118   }
00119 }
00120 
00130 void lubksb(dvar_matrix a, const ivector& indx,dvar_vector b)
00131 {
00132   int i,ii=0,ip,j,iiflag=0;
00133   dvariable sum;
00134   int lb=a.colmin();
00135   int ub=a.colmax();
00136   for (i=lb;i<=ub;i++)
00137   {
00138     ip=indx(i);
00139     sum=b(ip);
00140     b(ip)=b(i);
00141     if (iiflag)
00142     {
00143       for (j=ii;j<=i-1;j++)
00144       {
00145         sum -= a.elem(i,j)*b.elem(j);
00146       }
00147     }
00148     else if (!ISZERO(value(sum)))
00149     {
00150       ii=i;
00151       iiflag=1;
00152     }
00153     b(i)=sum;
00154   }
00155 
00156   for (i=ub;i>=lb;i--)
00157   {
00158     sum=b(i);
00159     for (j=i+1;j<=ub;j++)
00160     {                        // !!! remove to show bug
00161       sum -= a.elem(i,j)*b.elem(j);
00162     }                        // !!! remove to show bug
00163     b.elem(i)=sum/a.elem(i,i);
00164   }
00165 }
00166 
00167