ADMB Documentation  11.6rc.3396
 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 
00389 void lubksb(dmatrix a, const ivector& indx, dvector b)
00390 {
00391   int i,ii=0,ip,j,iiflag=0;
00392   double sum;
00393   int lb=a.colmin();
00394   int ub=a.colmax();
00395   for (i=lb;i<=ub;i++)
00396   {
00397     ip=indx[i];
00398     sum=b[ip];
00399     b[ip]=b[i];
00400     if (iiflag)
00401     {
00402       for (j=ii;j<=i-1;j++)
00403       {
00404         sum -= a[i][j]*b[j];
00405       }
00406     }
00407     else if (!ISZERO(sum))
00408     {
00409       ii=i;
00410       iiflag=1;
00411     }
00412     b[i]=sum;
00413   }
00414 
00415   for (i=ub;i>=lb;i--)
00416   {
00417     sum=b[i];
00418     for (j=i+1;j<=ub;j++)
00419     {                        // !!! remove to show bug
00420       sum -= a[i][j]*b[j];
00421     }                        // !!! remove to show bug
00422     b[i]=sum/a[i][i];
00423   }
00424 }
00425 
00434 double det(const dmatrix& m1)
00435 {
00436   double d = 0.0;
00437   dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());
00438 
00439   if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
00440   {
00441     cerr << "Matrix not square in routine det()" << endl;
00442     ad_exit(1);
00443   }
00444 
00445   for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
00446   {
00447     for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
00448     {
00449       a[i][j]=m1[i][j];
00450     }
00451   }
00452 
00453   ivector indx(m1.rowmin(),m1.rowmax());
00454   ludcmp_det(a,indx,d);
00455   for (int j=m1.rowmin();j<=m1.rowmax();j++)
00456   {
00457       d*=a[j][j];
00458   }
00459 
00460   return(d);
00461 }
00462 
00471 double ln_det(const dmatrix& m1, int& sgn)
00472 {
00473   double d = 0.0;
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 j=0;
00536   int k=0;
00537   int n=0;
00538   double& d=(double&)_d;
00539   dmatrix& a=(dmatrix&)_a;
00540   ivector& indx=(ivector&)_indx;
00541 
00542   n=a.colsize();
00543   int lb=a.colmin();
00544   int ub=a.colmax();
00545   indx.fill_seqadd(lb,1);
00546 
00547   double big,dum,sum,temp;
00548 
00549   dvector vv(lb,ub);
00550 
00551 
00552   d=1.0;
00553 
00554   for (i=lb;i<=ub;i++)
00555   {
00556     big=0.0;
00557     for (j=lb;j<=ub;j++)
00558     {
00559       temp=fabs(a[i][j]);
00560       if (temp > big)
00561       {
00562         big=temp;
00563       }
00564     }
00565     if (big == 0.0)
00566     {
00567       cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
00568     }
00569     vv[i]=1.0/big;
00570   }
00571 
00572 
00573 
00574   for (j=lb;j<=ub;j++)
00575   {
00576     for (i=lb;i<j;i++)
00577     {
00578       sum=a[i][j];
00579       for (k=lb;k<i;k++)
00580       {
00581         sum = sum - a[i][k]*a[k][j];
00582       }
00583       a[i][j]=sum;
00584     }
00585     int imax=j;
00586     big=0.0;
00587     for (i=j;i<=ub;i++)
00588     {
00589       sum=a[i][j];
00590       for (k=lb;k<j;k++)
00591       {
00592         sum = sum - a[i][k]*a[k][j];
00593       }
00594       a[i][j]=sum;
00595       dum=vv[i]*fabs(sum);
00596       if ( dum >= big)
00597       {
00598         big=dum;
00599         imax=i;
00600       }
00601     }
00602     if (j != imax)
00603     {
00604       for (k=lb;k<=ub;k++)
00605       {
00606         dum=a[imax][k];
00607         a[imax][k]=a[j][k];
00608         a[j][k]=dum;
00609       }
00610       d = -d;
00611       vv[imax]=vv[j];
00612       int itemp=indx.elem(imax);
00613       indx.elem(imax)=indx.elem(j);
00614       indx.elem(j)=itemp;
00615     }
00616 
00617     if (a[j][j] == 0.0)
00618     {
00619       a[j][j]=TINY;
00620     }
00621 
00622     if (j != n)
00623     {
00624       dum=1.0/(a[j][j]);
00625       for (i=j+1;i<=ub;i++)
00626       {
00627         a[i][j] = a[i][j] * dum;
00628       }
00629     }
00630   }
00631 }
00632 #undef TINY
00633 
00634