ADMB Documentation  11.1.2192
 All Classes Files Functions Variables Typedefs Friends Defines
dmat3.cpp
Go to the documentation of this file.
00001 
00007 #include "fvar.hpp"
00008 #include <math.h>
00009 #ifndef __ZTC__
00010 //#include <iomanip.h>
00011 #endif
00012 
00017 #define TINY 1.0e-20;
00018 
00019 void lubksb(dmatrix a, const ivector&  indx,dvector b);
00020 void ludcmp(const dmatrix& a, const ivector& indx, const double& d);
00021 
00030 dmatrix inv(const dmatrix& m1)
00031 {
00032   double d;
00033   if (m1.rowmin()!=m1.colmin() || m1.rowmax() != m1.colmax())
00034   {
00035     cerr << " Error in dmatrix inv(const dmatrix&) -- matrix not square \n";
00036   }
00037 
00038   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00039 
00040   int i;
00041   for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00042   {
00043     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00044     {
00045       a[i][j]=m1[i][j];
00046     }
00047   }
00048   ivector indx(m1.rowmin(),m1.rowmax());
00049   //int indx[30];
00050 
00051   ludcmp(a,indx,d);
00052 
00053   dmatrix y(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00054   dvector col(m1.rowmin(),m1.rowmax());
00055 
00056   for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00057   {
00058     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00059     {
00060       col[i]=0;
00061     }
00062     col[j]=1;
00063 
00064     lubksb(a,indx,col);
00065 
00066     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00067     {
00068       y[i][j]=col[i];
00069     }
00070   }
00071   return(y);
00072 }
00073 
00083 dmatrix inv(const dmatrix& m1,const double& _ln_det, const int& _sgn)
00084 {
00085   double d = 0.0;
00086   double& ln_det=(double&)(_ln_det);
00087   ln_det=0.0;
00088   int& sgn=(int&)(_sgn);
00089 
00090   if (m1.rowmin()!=m1.colmin() || m1.rowmax() != m1.colmax())
00091   {
00092     cerr << " Error in dmatrix inv(const dmatrix&) -- matrix not square \n";
00093   }
00094 
00095   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00096 
00097   int i;
00098   for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00099   {
00100     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00101     {
00102       a[i][j]=m1[i][j];
00103     }
00104   }
00105   ivector indx(m1.rowmin(),m1.rowmax());
00106   //int indx[30];
00107 
00108   ludcmp(a,indx,d);
00109   if (d>.1)
00110   {
00111     sgn=1;
00112   }
00113   else if (d<-0.1)
00114   {
00115     sgn=-1;
00116   }
00117   else
00118   {
00119     sgn=0;
00120   }
00121   int j;
00122   for (j=m1.rowmin();j<=m1.rowmax();j++)
00123   {
00124     if (a(j,j)>0)
00125     {
00126       ln_det+=log(a[j][j]);
00127     }
00128     else if (a(j,j)<0)
00129     {
00130       sgn=-sgn;
00131       ln_det+=log(-a[j][j]);
00132     }
00133     else
00134     {
00135       sgn=0;
00136     }
00137   }
00138 
00139   dmatrix y(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00140   dvector col(m1.rowmin(),m1.rowmax());
00141 
00142   for (j=m1.rowmin(); j<=m1.rowmax(); j++)
00143   {
00144     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00145     {
00146       col[i]=0;
00147     }
00148     col[j]=1;
00149 
00150     lubksb(a,indx,col);
00151 
00152     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00153     {
00154       y[i][j]=col[i];
00155     }
00156   }
00157   return(y);
00158 }
00159 
00168 void ludcmp(const dmatrix& _a, const ivector& _indx, const double& _d)
00169 {
00170   int i=0;
00171   int imax=0;
00172   int j=0;
00173   int k=0;
00174   int n=0;
00175   double& d=(double&)_d;
00176   dmatrix& a=(dmatrix&)_a;
00177   ivector& indx=(ivector&)_indx;
00178 
00179   n=a.colsize();
00180   int lb=a.colmin();
00181   int ub=a.colmax();
00182 
00183   double big,dum,sum,temp;
00184 
00185   dvector vv(lb,ub);
00186 
00187 
00188   d=1.0;
00189 
00190   for (i=lb;i<=ub;i++)
00191   {
00192     big=0.0;
00193     for (j=lb;j<=ub;j++)
00194     {
00195       temp=fabs(a[i][j]);
00196       if (temp > big)
00197       {
00198         big=temp;
00199       }
00200     }
00201     if (big == 0.0)
00202     {
00203       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00204     }
00205     vv[i]=1.0/big;
00206   }
00207 
00208 
00209 
00210   for (j=lb;j<=ub;j++)
00211   {
00212     for (i=lb;i<j;i++)
00213     {
00214       sum=a[i][j];
00215       for (k=lb;k<i;k++)
00216       {
00217         sum = sum - a[i][k]*a[k][j];
00218       }
00219       a[i][j]=sum;
00220     }
00221     big=0.0;
00222     for (i=j;i<=ub;i++)
00223     {
00224       sum=a[i][j];
00225       for (k=lb;k<j;k++)
00226       {
00227         sum = sum - a[i][k]*a[k][j];
00228       }
00229       a[i][j]=sum;
00230       dum=vv[i]*fabs(sum);
00231       if ( dum >= big)
00232       {
00233         big=dum;
00234         imax=i;
00235       }
00236     }
00237     if (j != imax)
00238     {
00239       for (k=lb;k<=ub;k++)
00240       {
00241         dum=a[imax][k];
00242         a[imax][k]=a[j][k];
00243         a[j][k]=dum;
00244       }
00245       d = -d;
00246       vv[imax]=vv[j];
00247     }
00248     indx[j]=imax;
00249 
00250     if (a[j][j] == 0.0)
00251     {
00252       a[j][j]=TINY;
00253     }
00254 
00255     if (j != n)
00256     {
00257       dum=1.0/(a[j][j]);
00258       for (i=j+1;i<=ub;i++)
00259       {
00260         a[i][j] = a[i][j] * dum;
00261       }
00262     }
00263   }
00264 }
00265 #undef TINY
00266 
00267 #define TINY 1.0e-50;
00268 
00277 void ludcmp_det(const dmatrix& _a, const ivector& _indx, const double& _d)
00278 {
00279   int i,imax,j,k,n;
00280   double& d=(double&)_d;
00281   dmatrix& a=(dmatrix&)_a;
00282   ivector& indx=(ivector&)_indx;
00283 
00284   n=a.colsize();
00285   int lb=a.colmin();
00286   int ub=a.colmax();
00287 
00288   double big,dum,sum,temp;
00289 
00290   dvector vv(lb,ub);
00291 
00292 
00293   d=1.0;
00294 
00295   for (i=lb;i<=ub;i++)
00296   {
00297     big=0.0;
00298     for (j=lb;j<=ub;j++)
00299     {
00300       temp=fabs(a[i][j]);
00301       if (temp > big)
00302       {
00303         big=temp;
00304       }
00305     }
00306     if (big == 0.0)
00307     {
00308       d=0.;
00309     }
00310     vv[i]=1.0/big;
00311   }
00312 
00313 
00314 
00315   for (j=lb;j<=ub;j++)
00316   {
00317     for (i=lb;i<j;i++)
00318     {
00319       sum=a[i][j];
00320       for (k=lb;k<i;k++)
00321       {
00322         sum = sum - a[i][k]*a[k][j];
00323       }
00324       a[i][j]=sum;
00325     }
00326     big=0.0;
00327     for (i=j;i<=ub;i++)
00328     {
00329       sum=a[i][j];
00330       for (k=lb;k<j;k++)
00331       {
00332         sum = sum - a[i][k]*a[k][j];
00333       }
00334       a[i][j]=sum;
00335       dum=vv[i]*fabs(sum);
00336       if ( dum >= big)
00337       {
00338         big=dum;
00339         imax=i;
00340       }
00341     }
00342     if (j != imax)
00343     {
00344       for (k=lb;k<=ub;k++)
00345       {
00346         dum=a[imax][k];
00347         a[imax][k]=a[j][k];
00348         a[j][k]=dum;
00349       }
00350       d = -d;
00351       vv[imax]=vv[j];
00352     }
00353     indx[j]=imax;
00354 
00355     if (a[j][j] == 0.0)
00356     {
00357       a[j][j]=TINY;
00358     }
00359 
00360     if (j != n)
00361     {
00362       dum=1.0/(a[j][j]);
00363       for (i=j+1;i<=ub;i++)
00364       {
00365         a[i][j] = a[i][j] * dum;
00366       }
00367     }
00368   }
00369 }
00370 
00371 
00384 void lubksb(dmatrix a, const ivector& indx, dvector b)
00385 {
00386   int i,ii=0,ip,j,iiflag=0;
00387   double sum;
00388   int lb=a.colmin();
00389   int ub=a.colmax();
00390   for (i=lb;i<=ub;i++)
00391   {
00392     ip=indx[i];
00393     sum=b[ip];
00394     b[ip]=b[i];
00395     if (iiflag)
00396     {
00397       for (j=ii;j<=i-1;j++)
00398       {
00399         sum -= a[i][j]*b[j];
00400       }
00401     }
00402     else if ( sum )
00403     {
00404       ii=i;
00405       iiflag=1;
00406     }
00407     b[i]=sum;
00408   }
00409 
00410   for (i=ub;i>=lb;i--)
00411   {
00412     sum=b[i];
00413     for (j=i+1;j<=ub;j++)
00414     {                        // !!! remove to show bug
00415       sum -= a[i][j]*b[j];
00416     }                        // !!! remove to show bug
00417     b[i]=sum/a[i][i];
00418   }
00419 }
00420 
00431 double det(const dmatrix& m1)
00432 {
00433   double d = 0.0;
00434   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00435 
00436   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00437   {
00438     cerr << "Matrix not square in routine det()" << endl;
00439     ad_exit(1);
00440   }
00441 
00442   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00443   {
00444     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00445     {
00446       a[i][j]=m1[i][j];
00447     }
00448   }
00449 
00450   ivector indx(m1.rowmin(),m1.rowmax());
00451   ludcmp_det(a,indx,d);
00452   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00453   {
00454       d*=a[j][j];
00455   }
00456 
00457   return(d);
00458 }
00459 
00470 double ln_det(const dmatrix& m1, const int& _sgn)
00471 {
00472   double d = 0.0;
00473   int& sgn=(int&)_sgn;
00474   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00475 
00476   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00477   {
00478     cerr << "Matrix not square in routine det()" << endl;
00479     ad_exit(1);
00480   }
00481 
00482   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00483   {
00484     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00485     {
00486       a[i][j]=m1[i][j];
00487     }
00488   }
00489 
00490   ivector indx(m1.rowmin(),m1.rowmax());
00491   ludcmp_det(a,indx,d);
00492   double ln_det=0.0;
00493 
00494   if (d>.1)
00495   {
00496     sgn=1;
00497   }
00498   else if (d<-0.1)
00499   {
00500     sgn=-1;
00501   }
00502   else
00503   {
00504     sgn=0;
00505   }
00506   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00507   {
00508     if (a(j,j)>0)
00509     {
00510       ln_det+=log(a[j][j]);
00511     }
00512     else if (a(j,j)<0)
00513     {
00514       sgn=-sgn;
00515       ln_det+=log(-a[j][j]);
00516     }
00517     else
00518     {
00519       sgn=0;
00520     }
00521   }
00522   return(ln_det);
00523 }
00524 
00532 void ludcmp_index(const dmatrix& _a, const ivector& _indx, const double& _d)
00533 {
00534   int i=0;
00535   int imax=0;
00536   int j=0;
00537   int k=0;
00538   int n=0;
00539   double& d=(double&)_d;
00540   dmatrix& a=(dmatrix&)_a;
00541   ivector& indx=(ivector&)_indx;
00542 
00543   n=a.colsize();
00544   int lb=a.colmin();
00545   int ub=a.colmax();
00546   indx.fill_seqadd(lb,1);
00547 
00548   double big,dum,sum,temp;
00549 
00550   dvector vv(lb,ub);
00551 
00552 
00553   d=1.0;
00554 
00555   for (i=lb;i<=ub;i++)
00556   {
00557     big=0.0;
00558     for (j=lb;j<=ub;j++)
00559     {
00560       temp=fabs(a[i][j]);
00561       if (temp > big)
00562       {
00563         big=temp;
00564       }
00565     }
00566     if (big == 0.0)
00567     {
00568       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00569     }
00570     vv[i]=1.0/big;
00571   }
00572 
00573 
00574 
00575   for (j=lb;j<=ub;j++)
00576   {
00577     for (i=lb;i<j;i++)
00578     {
00579       sum=a[i][j];
00580       for (k=lb;k<i;k++)
00581       {
00582         sum = sum - a[i][k]*a[k][j];
00583       }
00584       a[i][j]=sum;
00585     }
00586     big=0.0;
00587 
00588     for (i=j;i<=ub;i++)
00589     {
00590       sum=a[i][j];
00591       for (k=lb;k<j;k++)
00592       {
00593         sum = sum - a[i][k]*a[k][j];
00594       }
00595       a[i][j]=sum;
00596       dum=vv[i]*fabs(sum);
00597       if ( dum >= big)
00598       {
00599         big=dum;
00600         imax=i;
00601       }
00602     }
00603     if (j != imax)
00604     {
00605       for (k=lb;k<=ub;k++)
00606       {
00607         dum=a[imax][k];
00608         a[imax][k]=a[j][k];
00609         a[j][k]=dum;
00610       }
00611       d = -d;
00612       vv[imax]=vv[j];
00613       int itemp=indx.elem(imax);
00614       indx.elem(imax)=indx.elem(j);
00615       indx.elem(j)=itemp;
00616     }
00617 
00618     if (a[j][j] == 0.0)
00619     {
00620       a[j][j]=TINY;
00621     }
00622 
00623     if (j != n)
00624     {
00625       dum=1.0/(a[j][j]);
00626       for (i=j+1;i<=ub;i++)
00627       {
00628         a[i][j] = a[i][j] * dum;
00629       }
00630     }
00631   }
00632 }
00633 #undef TINY
00634 
00635