ADMB Documentation  11.1x.2738
 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 
00013 #ifdef ISZERO
00014   #undef ISZERO
00015 #endif
00016 #define ISZERO(d) ((d)==0.0)
00017 
00023 #define TINY 1.0e-20;
00024 
00025 void lubksb(dmatrix a, const ivector&  indx,dvector b);
00026 void ludcmp(const dmatrix& a, const ivector& indx, const double& d);
00027 
00036 dmatrix inv(const dmatrix& m1)
00037 {
00038   double d;
00039   if (m1.rowmin()!=m1.colmin() || m1.rowmax() != m1.colmax())
00040   {
00041     cerr << " Error in dmatrix inv(const dmatrix&) -- matrix not square \n";
00042   }
00043 
00044   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00045 
00046   int i;
00047   for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00048   {
00049     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00050     {
00051       a[i][j]=m1[i][j];
00052     }
00053   }
00054   ivector indx(m1.rowmin(),m1.rowmax());
00055   //int indx[30];
00056 
00057   ludcmp(a,indx,d);
00058 
00059   dmatrix y(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00060   dvector col(m1.rowmin(),m1.rowmax());
00061 
00062   for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00063   {
00064     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00065     {
00066       col[i]=0;
00067     }
00068     col[j]=1;
00069 
00070     lubksb(a,indx,col);
00071 
00072     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00073     {
00074       y[i][j]=col[i];
00075     }
00076   }
00077   return(y);
00078 }
00079 
00089 dmatrix inv(const dmatrix& m1,const double& _ln_det, const int& _sgn)
00090 {
00091   double d = 0.0;
00092   double& ln_det=(double&)(_ln_det);
00093   ln_det=0.0;
00094   int& sgn=(int&)(_sgn);
00095 
00096   if (m1.rowmin()!=m1.colmin() || m1.rowmax() != m1.colmax())
00097   {
00098     cerr << " Error in dmatrix inv(const dmatrix&) -- matrix not square \n";
00099   }
00100 
00101   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00102 
00103   int i;
00104   for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00105   {
00106     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00107     {
00108       a[i][j]=m1[i][j];
00109     }
00110   }
00111   ivector indx(m1.rowmin(),m1.rowmax());
00112   //int indx[30];
00113 
00114   ludcmp(a,indx,d);
00115   if (d>.1)
00116   {
00117     sgn=1;
00118   }
00119   else if (d<-0.1)
00120   {
00121     sgn=-1;
00122   }
00123   else
00124   {
00125     sgn=0;
00126   }
00127   int j;
00128   for (j=m1.rowmin();j<=m1.rowmax();j++)
00129   {
00130     if (a(j,j)>0)
00131     {
00132       ln_det+=log(a[j][j]);
00133     }
00134     else if (a(j,j)<0)
00135     {
00136       sgn=-sgn;
00137       ln_det+=log(-a[j][j]);
00138     }
00139     else
00140     {
00141       sgn=0;
00142     }
00143   }
00144 
00145   dmatrix y(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00146   dvector col(m1.rowmin(),m1.rowmax());
00147 
00148   for (j=m1.rowmin(); j<=m1.rowmax(); j++)
00149   {
00150     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00151     {
00152       col[i]=0;
00153     }
00154     col[j]=1;
00155 
00156     lubksb(a,indx,col);
00157 
00158     for (i=m1.rowmin(); i<=m1.rowmax(); i++)
00159     {
00160       y[i][j]=col[i];
00161     }
00162   }
00163   return(y);
00164 }
00165 
00174 void ludcmp(const dmatrix& _a, const ivector& _indx, const double& _d)
00175 {
00176   int i=0;
00177   int j=0;
00178   int k=0;
00179   int n=0;
00180   double& d=(double&)_d;
00181   dmatrix& a=(dmatrix&)_a;
00182   ivector& indx=(ivector&)_indx;
00183 
00184   n=a.colsize();
00185   int lb=a.colmin();
00186   int ub=a.colmax();
00187 
00188   double big,dum,sum,temp;
00189 
00190   dvector vv(lb,ub);
00191 
00192 
00193   d=1.0;
00194 
00195   for (i=lb;i<=ub;i++)
00196   {
00197     big=0.0;
00198     for (j=lb;j<=ub;j++)
00199     {
00200       temp=fabs(a[i][j]);
00201       if (temp > big)
00202       {
00203         big=temp;
00204       }
00205     }
00206     if (big == 0.0)
00207     {
00208       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00209     }
00210     vv[i]=1.0/big;
00211   }
00212 
00213 
00214 
00215   for (j=lb;j<=ub;j++)
00216   {
00217     for (i=lb;i<j;i++)
00218     {
00219       sum=a[i][j];
00220       for (k=lb;k<i;k++)
00221       {
00222         sum = sum - a[i][k]*a[k][j];
00223       }
00224       a[i][j]=sum;
00225     }
00226     int imax=j;
00227     big=0.0;
00228     for (i=j;i<=ub;i++)
00229     {
00230       sum=a[i][j];
00231       for (k=lb;k<j;k++)
00232       {
00233         sum = sum - a[i][k]*a[k][j];
00234       }
00235       a[i][j]=sum;
00236       dum=vv[i]*fabs(sum);
00237       if ( dum >= big)
00238       {
00239         big=dum;
00240         imax=i;
00241       }
00242     }
00243     if (j != imax)
00244     {
00245       for (k=lb;k<=ub;k++)
00246       {
00247         dum=a[imax][k];
00248         a[imax][k]=a[j][k];
00249         a[j][k]=dum;
00250       }
00251       d = -d;
00252       vv[imax]=vv[j];
00253     }
00254     indx[j]=imax;
00255 
00256     if (a[j][j] == 0.0)
00257     {
00258       a[j][j]=TINY;
00259     }
00260 
00261     if (j != n)
00262     {
00263       dum=1.0/(a[j][j]);
00264       for (i=j+1;i<=ub;i++)
00265       {
00266         a[i][j] = a[i][j] * dum;
00267       }
00268     }
00269   }
00270 }
00271 #undef TINY
00272 
00273 #define TINY 1.0e-50;
00274 
00283 void ludcmp_det(const dmatrix& _a, const ivector& _indx, const double& _d)
00284 {
00285   int i,j,k,n;
00286   double& d=(double&)_d;
00287   dmatrix& a=(dmatrix&)_a;
00288   ivector& indx=(ivector&)_indx;
00289 
00290   n=a.colsize();
00291   int lb=a.colmin();
00292   int ub=a.colmax();
00293 
00294   double big,dum,sum,temp;
00295 
00296   dvector vv(lb,ub);
00297 
00298 
00299   d=1.0;
00300 
00301   for (i=lb;i<=ub;i++)
00302   {
00303     big=0.0;
00304     for (j=lb;j<=ub;j++)
00305     {
00306       temp=fabs(a[i][j]);
00307       if (temp > big)
00308       {
00309         big=temp;
00310       }
00311     }
00312     if (big == 0.0)
00313     {
00314       d=0.;
00315     }
00316     vv[i]=1.0/big;
00317   }
00318 
00319 
00320 
00321   for (j=lb;j<=ub;j++)
00322   {
00323     for (i=lb;i<j;i++)
00324     {
00325       sum=a[i][j];
00326       for (k=lb;k<i;k++)
00327       {
00328         sum = sum - a[i][k]*a[k][j];
00329       }
00330       a[i][j]=sum;
00331     }
00332     int imax = j;
00333     big=0.0;
00334     for (i=j;i<=ub;i++)
00335     {
00336       sum=a[i][j];
00337       for (k=lb;k<j;k++)
00338       {
00339         sum = sum - a[i][k]*a[k][j];
00340       }
00341       a[i][j]=sum;
00342       dum=vv[i]*fabs(sum);
00343       if ( dum >= big)
00344       {
00345         big=dum;
00346         imax=i;
00347       }
00348     }
00349     if (j != imax)
00350     {
00351       for (k=lb;k<=ub;k++)
00352       {
00353         dum=a[imax][k];
00354         a[imax][k]=a[j][k];
00355         a[j][k]=dum;
00356       }
00357       d = -d;
00358       vv[imax]=vv[j];
00359     }
00360     indx[j]=imax;
00361 
00362     if (a[j][j] == 0.0)
00363     {
00364       a[j][j]=TINY;
00365     }
00366 
00367     if (j != n)
00368     {
00369       dum=1.0/(a[j][j]);
00370       for (i=j+1;i<=ub;i++)
00371       {
00372         a[i][j] = a[i][j] * dum;
00373       }
00374     }
00375   }
00376 }
00377 
00378 
00391 void lubksb(dmatrix a, const ivector& indx, dvector b)
00392 {
00393   int i,ii=0,ip,j,iiflag=0;
00394   double sum;
00395   int lb=a.colmin();
00396   int ub=a.colmax();
00397   for (i=lb;i<=ub;i++)
00398   {
00399     ip=indx[i];
00400     sum=b[ip];
00401     b[ip]=b[i];
00402     if (iiflag)
00403     {
00404       for (j=ii;j<=i-1;j++)
00405       {
00406         sum -= a[i][j]*b[j];
00407       }
00408     }
00409     else if (!ISZERO(sum))
00410     {
00411       ii=i;
00412       iiflag=1;
00413     }
00414     b[i]=sum;
00415   }
00416 
00417   for (i=ub;i>=lb;i--)
00418   {
00419     sum=b[i];
00420     for (j=i+1;j<=ub;j++)
00421     {                        // !!! remove to show bug
00422       sum -= a[i][j]*b[j];
00423     }                        // !!! remove to show bug
00424     b[i]=sum/a[i][i];
00425   }
00426 }
00427 
00438 double det(const dmatrix& m1)
00439 {
00440   double d = 0.0;
00441   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00442 
00443   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00444   {
00445     cerr << "Matrix not square in routine det()" << endl;
00446     ad_exit(1);
00447   }
00448 
00449   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00450   {
00451     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00452     {
00453       a[i][j]=m1[i][j];
00454     }
00455   }
00456 
00457   ivector indx(m1.rowmin(),m1.rowmax());
00458   ludcmp_det(a,indx,d);
00459   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00460   {
00461       d*=a[j][j];
00462   }
00463 
00464   return(d);
00465 }
00466 
00477 double ln_det(const dmatrix& m1, const int& _sgn)
00478 {
00479   double d = 0.0;
00480   int& sgn=(int&)_sgn;
00481   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00482 
00483   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00484   {
00485     cerr << "Matrix not square in routine det()" << endl;
00486     ad_exit(1);
00487   }
00488 
00489   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00490   {
00491     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00492     {
00493       a[i][j]=m1[i][j];
00494     }
00495   }
00496 
00497   ivector indx(m1.rowmin(),m1.rowmax());
00498   ludcmp_det(a,indx,d);
00499   double ln_det=0.0;
00500 
00501   if (d>.1)
00502   {
00503     sgn=1;
00504   }
00505   else if (d<-0.1)
00506   {
00507     sgn=-1;
00508   }
00509   else
00510   {
00511     sgn=0;
00512   }
00513   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00514   {
00515     if (a(j,j)>0)
00516     {
00517       ln_det+=log(a[j][j]);
00518     }
00519     else if (a(j,j)<0)
00520     {
00521       sgn=-sgn;
00522       ln_det+=log(-a[j][j]);
00523     }
00524     else
00525     {
00526       sgn=0;
00527     }
00528   }
00529   return(ln_det);
00530 }
00531 
00539 void ludcmp_index(const dmatrix& _a, const ivector& _indx, const double& _d)
00540 {
00541   int i=0;
00542   int j=0;
00543   int k=0;
00544   int n=0;
00545   double& d=(double&)_d;
00546   dmatrix& a=(dmatrix&)_a;
00547   ivector& indx=(ivector&)_indx;
00548 
00549   n=a.colsize();
00550   int lb=a.colmin();
00551   int ub=a.colmax();
00552   indx.fill_seqadd(lb,1);
00553 
00554   double big,dum,sum,temp;
00555 
00556   dvector vv(lb,ub);
00557 
00558 
00559   d=1.0;
00560 
00561   for (i=lb;i<=ub;i++)
00562   {
00563     big=0.0;
00564     for (j=lb;j<=ub;j++)
00565     {
00566       temp=fabs(a[i][j]);
00567       if (temp > big)
00568       {
00569         big=temp;
00570       }
00571     }
00572     if (big == 0.0)
00573     {
00574       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00575     }
00576     vv[i]=1.0/big;
00577   }
00578 
00579 
00580 
00581   for (j=lb;j<=ub;j++)
00582   {
00583     for (i=lb;i<j;i++)
00584     {
00585       sum=a[i][j];
00586       for (k=lb;k<i;k++)
00587       {
00588         sum = sum - a[i][k]*a[k][j];
00589       }
00590       a[i][j]=sum;
00591     }
00592     int imax=j;
00593     big=0.0;
00594     for (i=j;i<=ub;i++)
00595     {
00596       sum=a[i][j];
00597       for (k=lb;k<j;k++)
00598       {
00599         sum = sum - a[i][k]*a[k][j];
00600       }
00601       a[i][j]=sum;
00602       dum=vv[i]*fabs(sum);
00603       if ( dum >= big)
00604       {
00605         big=dum;
00606         imax=i;
00607       }
00608     }
00609     if (j != imax)
00610     {
00611       for (k=lb;k<=ub;k++)
00612       {
00613         dum=a[imax][k];
00614         a[imax][k]=a[j][k];
00615         a[j][k]=dum;
00616       }
00617       d = -d;
00618       vv[imax]=vv[j];
00619       int itemp=indx.elem(imax);
00620       indx.elem(imax)=indx.elem(j);
00621       indx.elem(j)=itemp;
00622     }
00623 
00624     if (a[j][j] == 0.0)
00625     {
00626       a[j][j]=TINY;
00627     }
00628 
00629     if (j != n)
00630     {
00631       dum=1.0/(a[j][j]);
00632       for (i=j+1;i<=ub;i++)
00633       {
00634         a[i][j] = a[i][j] * dum;
00635       }
00636     }
00637   }
00638 }
00639 #undef TINY
00640 
00641