ADMB Documentation  11.1.2532
 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 j=0;
00172   int k=0;
00173   int n=0;
00174   double& d=(double&)_d;
00175   dmatrix& a=(dmatrix&)_a;
00176   ivector& indx=(ivector&)_indx;
00177 
00178   n=a.colsize();
00179   int lb=a.colmin();
00180   int ub=a.colmax();
00181 
00182   double big,dum,sum,temp;
00183 
00184   dvector vv(lb,ub);
00185 
00186 
00187   d=1.0;
00188 
00189   for (i=lb;i<=ub;i++)
00190   {
00191     big=0.0;
00192     for (j=lb;j<=ub;j++)
00193     {
00194       temp=fabs(a[i][j]);
00195       if (temp > big)
00196       {
00197         big=temp;
00198       }
00199     }
00200     if (big == 0.0)
00201     {
00202       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00203     }
00204     vv[i]=1.0/big;
00205   }
00206 
00207 
00208 
00209   for (j=lb;j<=ub;j++)
00210   {
00211     for (i=lb;i<j;i++)
00212     {
00213       sum=a[i][j];
00214       for (k=lb;k<i;k++)
00215       {
00216         sum = sum - a[i][k]*a[k][j];
00217       }
00218       a[i][j]=sum;
00219     }
00220     int imax=j;
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,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     int imax = j;
00327     big=0.0;
00328     for (i=j;i<=ub;i++)
00329     {
00330       sum=a[i][j];
00331       for (k=lb;k<j;k++)
00332       {
00333         sum = sum - a[i][k]*a[k][j];
00334       }
00335       a[i][j]=sum;
00336       dum=vv[i]*fabs(sum);
00337       if ( dum >= big)
00338       {
00339         big=dum;
00340         imax=i;
00341       }
00342     }
00343     if (j != imax)
00344     {
00345       for (k=lb;k<=ub;k++)
00346       {
00347         dum=a[imax][k];
00348         a[imax][k]=a[j][k];
00349         a[j][k]=dum;
00350       }
00351       d = -d;
00352       vv[imax]=vv[j];
00353     }
00354     indx[j]=imax;
00355 
00356     if (a[j][j] == 0.0)
00357     {
00358       a[j][j]=TINY;
00359     }
00360 
00361     if (j != n)
00362     {
00363       dum=1.0/(a[j][j]);
00364       for (i=j+1;i<=ub;i++)
00365       {
00366         a[i][j] = a[i][j] * dum;
00367       }
00368     }
00369   }
00370 }
00371 
00372 
00385 void lubksb(dmatrix a, const ivector& indx, dvector b)
00386 {
00387   int i,ii=0,ip,j,iiflag=0;
00388   double sum;
00389   int lb=a.colmin();
00390   int ub=a.colmax();
00391   for (i=lb;i<=ub;i++)
00392   {
00393     ip=indx[i];
00394     sum=b[ip];
00395     b[ip]=b[i];
00396     if (iiflag)
00397     {
00398       for (j=ii;j<=i-1;j++)
00399       {
00400         sum -= a[i][j]*b[j];
00401       }
00402     }
00403     else if ( sum )
00404     {
00405       ii=i;
00406       iiflag=1;
00407     }
00408     b[i]=sum;
00409   }
00410 
00411   for (i=ub;i>=lb;i--)
00412   {
00413     sum=b[i];
00414     for (j=i+1;j<=ub;j++)
00415     {                        // !!! remove to show bug
00416       sum -= a[i][j]*b[j];
00417     }                        // !!! remove to show bug
00418     b[i]=sum/a[i][i];
00419   }
00420 }
00421 
00432 double det(const dmatrix& m1)
00433 {
00434   double d = 0.0;
00435   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00436 
00437   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00438   {
00439     cerr << "Matrix not square in routine det()" << endl;
00440     ad_exit(1);
00441   }
00442 
00443   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00444   {
00445     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00446     {
00447       a[i][j]=m1[i][j];
00448     }
00449   }
00450 
00451   ivector indx(m1.rowmin(),m1.rowmax());
00452   ludcmp_det(a,indx,d);
00453   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00454   {
00455       d*=a[j][j];
00456   }
00457 
00458   return(d);
00459 }
00460 
00471 double ln_det(const dmatrix& m1, const int& _sgn)
00472 {
00473   double d = 0.0;
00474   int& sgn=(int&)_sgn;
00475   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00476 
00477   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00478   {
00479     cerr << "Matrix not square in routine det()" << endl;
00480     ad_exit(1);
00481   }
00482 
00483   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00484   {
00485     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00486     {
00487       a[i][j]=m1[i][j];
00488     }
00489   }
00490 
00491   ivector indx(m1.rowmin(),m1.rowmax());
00492   ludcmp_det(a,indx,d);
00493   double ln_det=0.0;
00494 
00495   if (d>.1)
00496   {
00497     sgn=1;
00498   }
00499   else if (d<-0.1)
00500   {
00501     sgn=-1;
00502   }
00503   else
00504   {
00505     sgn=0;
00506   }
00507   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00508   {
00509     if (a(j,j)>0)
00510     {
00511       ln_det+=log(a[j][j]);
00512     }
00513     else if (a(j,j)<0)
00514     {
00515       sgn=-sgn;
00516       ln_det+=log(-a[j][j]);
00517     }
00518     else
00519     {
00520       sgn=0;
00521     }
00522   }
00523   return(ln_det);
00524 }
00525 
00533 void ludcmp_index(const dmatrix& _a, const ivector& _indx, const double& _d)
00534 {
00535   int i=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     int imax=j;
00587     big=0.0;
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