ADMB Documentation  Fournier-pthread.1088
 All Classes Namespaces Files Functions Variables Typedefs Friends Defines
df1b2ludcmp.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: df1b2ludcmp.cpp 494 2012-06-13 20:41:16Z johnoel $
00003  *
00004  * Copyright (c) 2010-2012 ADMB Foundation
00005  */
00011   #include <df1b2ludcmp.hpp>
00012 
00021   df1b2ludecomp ludecomp_pivot(const df1b2matrix & M)
00022   {
00023      int i = 0;
00024      int j = 0;
00025      int k = 0;
00026      int mmin = M.indexmin();
00027      int mmax = M.indexmax();
00028      int imax = mmax - 1;
00029      df1b2ludecomp clu(mmin, mmax);
00030      df1b2vector scale(mmin, mmax);
00031   
00032      // get terms for implicit scaling
00033      for (i = mmin; i <= mmax; i++)
00034      {
00035         df1b2variable tmp = 0.0;
00036         tmp = 1.0 / max(fabs(M(i)));
00037         if (value(tmp) == 0.0)
00038         {
00039      cerr << "Error -- trying to take LU decomp of a singular matrix"
00040         << endl;
00041      // use ad_exit() so that debugger can trace this
00042      ad_exit(1);
00043         }
00044         scale(i) = 0.0;
00045         scale(i) = 1.0 / max(fabs(M(i)));
00046      }
00047      // get upper and lower parts of LU
00048      df1b2matrix & alpha = clu.get_L();
00049      df1b2matrix & gamma = clu.get_U(); // gamma is the transpose of beta
00050      ivector & index = clu.get_index();
00051      ivector & index2 = clu.get_index2();
00052      int sign = clu.get_sign();
00053      // copy M into alpha and gamma
00054      for (i = mmin; i <= mmax; i++)
00055      {
00056         for (int j = mmin; j <= mmax; j++)
00057         {
00058      clu(i, j) = M(i, j);
00059         }
00060      }
00061      for (j = mmin; j <= mmax; j++)
00062      {
00063         int i = 0;
00064         for (i = mmin + 1; i < j; i++)
00065         {
00066      // using subvector here
00067      clu(i, j) -= alpha(i) (mmin, i - 1) * gamma(j) (mmin, i - 1);
00068         }
00069         df1b2variable maxterm = 0.0;
00070 
00071         for (i = j; i <= mmax; i++)
00072         {
00073      // using subvector here
00074      if (j > 1)
00075      {
00076         clu(i, j) -= alpha(i) (mmin, j - 1) * gamma(j) (mmin, j - 1);
00077      }
00078          df1b2variable tmp = 0.0;
00079      tmp = scale(i) * fabs(clu(i, j));
00080 
00081      if (value(tmp) > value(maxterm))
00082      {
00083         maxterm = tmp;
00084         imax = i;
00085      }
00086         }
00087 
00088         if (j != imax)
00089         {
00090      // have to do this element-wise
00091      for (k = mmin; k <= mmax; k++)
00092      {
00093         df1b2variable tmp = 0.0;
00094             tmp = clu(imax, k);
00095         clu(imax, k) = clu(j,k);
00096         clu(j, k) = tmp;
00097      }
00098 
00099      scale(imax) = scale(j);
00100      int itmp = index2(imax);
00101      index2(imax) = index2(j);
00102      index2(j) = itmp;
00103      sign = -sign;
00104         }
00105         index(j) = imax;
00106 
00107         if (value(clu(j, j)) == 0.0)
00108       clu(j, j) = 1.e-25;
00109         if (j != mmax)
00110         {
00111          df1b2variable z = 0.0;
00112      z = 1.0 / gamma(j, j);
00113      for (i = j + 1; i <= mmax; i++)
00114      {
00115         alpha(i, j) *= z;
00116      }
00117         }
00118      }
00119      return clu;
00120   }
00121   //end ludcmp pivoting
00122 
00129 df1b2vector solve(const df1b2matrix& aa,const df1b2vector& z)
00130 {
00131    int lb = aa.colmin();
00132    int ub = aa.colmax();
00133    if (lb != aa.rowmin() || ub != aa.colmax())
00134    {
00135       cerr << "Error matrix not square in solve(df1b2matrix)" << endl;
00136       ad_exit(1);
00137    }
00138    
00139    df1b2vector x(lb, ub);
00140    x.initialize();
00141 
00142    if (ub == lb)
00143    {
00144       x(lb) = z(lb) / aa(lb, lb);
00145       return (x);
00146    }
00147 
00148    df1b2matrix MC(lb,ub,lb,ub);
00149    MC=aa;
00150    df1b2ludecomp dcmp = ludecomp_pivot(MC);
00151    ivector index2 = dcmp.get_index2();
00152    df1b2matrix & gamma = dcmp.get_U();
00153    df1b2matrix & alpha = dcmp.get_L();
00154 
00155    //check if invertable
00156   df1b2variable ln_det = 0.0;
00157    for (int i = lb; i <= ub; i++)
00158    {
00159      ln_det += log(dcmp(i, i));
00160    }
00161    if (exp(value(ln_det)) == 0.0)
00162    {
00163       cerr <<
00164    "Error in matrix inverse -- matrix singular in solve(df1b2matrix)\n";
00165       ad_exit(1);
00166    }
00167 
00168    //Solve L*y=b with forward-substitution (before solving Ux=y)
00169    df1b2vector y(lb, ub);
00170    y.initialize();
00171 
00172    for (int i = lb; i <= ub; i++)
00173    {
00174       df1b2variable tmp = 0.0;
00175       for (int j = lb; j < i; j++)
00176       {
00177    tmp += alpha(i, j) * y(j);
00178       }
00179       y(i) = z(index2(i)) - tmp;
00180    }
00181 
00182    //Now solve U*x=y with back substitution
00183    for (int i = ub; i >= lb; i--)
00184    {
00185       df1b2variable tmp = 0.0;
00186       for (int j = ub; j > i; j--)
00187       {
00188    tmp += gamma(j, i) * x(j);
00189       }
00190       x(i) = (y(i) - tmp) / gamma(i, i);
00191    }
00192 
00193    return x;
00194 }
00195 
00202 df1b2vector solve(const df1b2matrix& aa,const dvector& z)
00203 {
00204   df1b2vector zz(z.indexmin(),z.indexmax());
00205   zz.initialize();
00206   /*for(int i=z.indexmin();i<=z.indexmax();i++)
00207   {
00208     zz(i) = 0.0;
00209     zz(i) = z(i);
00210   }*/
00211   zz = z;
00212   return solve(aa,zz);
00213 }
00214 
00219 df1b2variable ln_det(const df1b2matrix & m1)
00220 {
00221    int mmin = m1.indexmin();
00222    int mmax = m1.indexmax();
00223 
00224    df1b2matrix M(mmin,mmax,mmin,mmax);
00225    M = m1;
00226    df1b2ludecomp clu1 = ludecomp_pivot(M);
00227 
00228    int sign = clu1.get_sign();
00229    ivector index2 = clu1.get_index2();
00230    df1b2variable lndet = 0.0;
00231    df1b2matrix & gamma = clu1.get_U();
00232 
00233    // only need to save the diagonal of gamma
00234    for (int i = mmin; i <= mmax; i++)
00235    {
00236       if (value(gamma(i, i)) < 0.0)
00237       {
00238    sign = -sign;
00239    lndet += log(-gamma(i, i));
00240       } else
00241       {
00242    lndet += log(gamma(i, i));
00243       }
00244    }
00245    return lndet;
00246 }
00247 
00256 df1b2vector solve(const df1b2matrix & aa, const df1b2vector & z,
00257       df1b2variable & ln_unsigned_det,
00258       const df1b2variable & _sign)
00259 {
00260    ADUNCONST(df1b2variable, sign)
00261    sign = 0.0;
00262    df1b2variable lndet = ln_det(aa);
00263    ln_unsigned_det = lndet;
00264    //df1b2variable sign = 0.0;
00265    //_sign = sign;
00266 
00267    df1b2vector sol = solve(aa, z);
00268    return sol;
00269 }
00270 
00271 df1b2matrix inv(const df1b2matrix& aa)
00272 {
00273    int lb = aa.colmin();
00274    int ub = aa.colmax();
00275    if (lb != aa.rowmin() || ub != aa.colmax())
00276    {
00277       cerr << "Error matrix not square in solve(df1b2matrix)" << endl;
00278       ad_exit(1);
00279    }
00280    
00281    df1b2matrix x(lb, ub,lb,ub);
00282    x.initialize();
00283 
00284    if (ub == lb)
00285    {
00286       x(lb,lb) = 1.0 / aa(lb, lb);
00287       return (x);
00288    }
00289 
00290    df1b2matrix MC(lb,ub,lb,ub);
00291    MC=aa;
00292    df1b2ludecomp dcmp = ludecomp_pivot(MC);
00293    ivector index2 = dcmp.get_index2();
00294    df1b2matrix & gamma = dcmp.get_U();
00295    df1b2matrix & alpha = dcmp.get_L();
00296 
00297    //check if invertable
00298   df1b2variable ln_det = 0.0;
00299    int sgn=1;
00300 
00301    for (int i = lb; i <= ub; i++)
00302    {
00303      if (value(dcmp(i,i)) == 0.0)
00304      {
00305         cerr <<
00306      "Error in matrix inverse -- matrix singular in solve(df1b2matrix)\n";
00307         ad_exit(1);
00308      }
00309      if (value(dcmp(i,i))<0.0)
00310      {
00311        sgn*=-1;
00312        ln_det += log(-dcmp(i, i));
00313      }
00314      else
00315      {
00316        sgn*=-1;
00317        ln_det += log(dcmp(i, i));
00318      }
00319    }
00320    for (int k = lb; k <= ub; k++)
00321    {
00322      //Solve L*y=b with forward-substitution (before solving Ux=y)
00323      df1b2vector y(lb, ub);
00324      df1b2vector z(lb, ub);
00325      y.initialize();
00326      z.initialize();
00327      z(k)=1.0;
00328   
00329      for (int i = lb; i <= ub; i++)
00330      {
00331         df1b2variable tmp = 0.0;
00332         for (int j = lb; j < i; j++)
00333         {
00334      tmp += alpha(i, j) * y(j);
00335         }
00336         y(i) = z(index2(i)) - tmp;
00337      }
00338   
00339      //Now solve U*x=y with back substitution
00340      for (int i = ub; i >= lb; i--)
00341      {
00342         df1b2variable tmp = 0.0;
00343         for (int j = ub; j > i; j--)
00344         {
00345      tmp += gamma(j, i) * x(k,j);
00346         }
00347         x(k,i) = (y(i) - tmp) / gamma(i, i);
00348      }
00349      return x;
00350   }
00351 }
00352