dmat3.cpp

Go to the documentation of this file.

#include "fvar.hpp"
#include <math.h>
#ifndef OPT_LIB
  #include <cassert>
  #include <climits>
#endif

#ifdef ISZERO
  #undef ISZERO
#endif
#define ISZERO(d) ((d)==0.0)

#define TINY 1.0e-20;

void lubksb(dmatrix a, const ivector&  indx,dvector b);
void ludcmp(const dmatrix& a, const ivector& indx, const double& d);

dmatrix inv(const dmatrix& m1)
{
  int min = m1.rowmin();
  int max = m1.rowmax();

#ifndef OPT_LIB
  if (min != m1.colmin() || max != m1.colmax())
  {
    cerr << " Error in dmatrix inv(const dmatrix&) -- matrix not square \n";
  }
#endif

  dmatrix a(min, max, min, max);

  dvector* pai = &a(min);
  const dvector* pm1i = &m1(min);
  for (int i = min; i <= max; ++i)
  {
    double* pm1ij = pm1i->get_v() + min;
    double* paij = pai->get_v() + min;
    for (int j = min; j <= max; ++j)
    {
      *paij = *pm1ij;

      ++pm1ij;
      ++paij;
    }
    ++pm1i;
    ++pai;
  }
  ivector indx(min, max);
  //int indx[30];

  double d;
  ludcmp(a,indx,d);

  dmatrix y(min, max, min, max);
  dvector col(min, max);

  double* pcolj = col.get_v() + min;
  for (int j = min; j <= max; ++j)
  {
    double* pcoli = col.get_v() + min;
    for (int i= min; i <= max; ++i)
    {
      *pcoli = 0.0;

      ++pcoli;
    }
    *pcolj = 1.0;

    lubksb(a,indx,col);

    dvector* pyi = &y(min);
    pcoli = col.get_v() + min;
    for (int i= min; i <= max; ++i)
    {
      *(pyi->get_v() + j)= *pcoli;

      ++pyi;
      ++pcoli;
    }

    ++pcolj;
  }
  return y;
}

dmatrix inv(const dmatrix& m1,const double& _ln_det, const int& _sgn)
{
  int min = m1.rowmin();
  int max = m1.rowmax();

#ifndef OPT_LIB
  if (min != m1.colmin() || max != m1.colmax())
  {
    cerr << " Error in dmatrix inv(const dmatrix&) -- matrix not square \n";
  }
#endif

  dmatrix a(min, max, min, max);

  dvector* pai = &a(min);
  const dvector* pm1i = &m1(min);
  for (int i = min; i <= max; ++i)
  {
    double* pm1ij = pm1i->get_v() + min;
    double* paij = pai->get_v() + min;
    for (int j = min; j <= max; ++j)
    {
      *paij = *pm1ij;

      ++pm1ij;
      ++paij;
    }
    ++pm1i;
    ++pai;
  }
  ivector indx(min, max);
  //int indx[30];

  double d = 0.0;
  ludcmp(a,indx,d);

  int& sgn=(int&)(_sgn);
  if (d>.1)
  {
    sgn=1;
  }
  else if (d<-0.1)
  {
    sgn=-1;
  }
  else
  {
    sgn=0;
  }
  double& ln_det=(double&)(_ln_det);
  ln_det=0.0;
  for (int j = min; j <= max; ++j)
  {
    double ajj = a(j, j);
    if (ajj > 0)
    {
      ln_det += log(ajj);
    }
    else if (ajj < 0)
    {
      sgn = -sgn;
      ln_det += log(-ajj);
    }
    else
    {
      sgn = 0;
    }
  }

  dmatrix y(min, max, min, max);
  dvector col(min, max);

  double* pcolj = col.get_v() + min;
  for (int j = min; j <= max; ++j)
  {
    double* pcoli = col.get_v() + min;
    for (int i= min; i <= max; ++i)
    {
      *pcoli = 0.0;

      ++pcoli;
    }
    *pcolj = 1.0;

    lubksb(a,indx,col);

    dvector* pyi = &y(min);
    pcoli = col.get_v() + min;
    for (int i = min; i <= max; ++i)
    {
      *(pyi->get_v() + j)= *pcoli;

      ++pcoli;
      ++pyi;
    }
  }
  return y;
}

void ludcmp(const dmatrix& _a, const ivector& _indx, const double& _d)
{
  int i=0;
  int j=0;
  int k=0;
  double& d=(double&)_d;
  dmatrix& a=(dmatrix&)_a;
  ivector& indx=(ivector&)_indx;

#if !defined(OPT_LIB) && (__cplusplus >= 201103L)
  int n = [](unsigned int colsize) -> int
  {
    assert(colsize <= INT_MAX);
    return static_cast<int>(colsize);
  } (a.colsize());
#else
  int n = static_cast<int>(a.colsize());
#endif
  int lb=a.colmin();
  int ub=a.colmax();

  dvector vv(lb,ub);

  d=1.0;

  dvector* pai = &a(lb);
  double* pvvi = vv.get_v() + lb;
  for (i=lb;i<=ub;++i)
  {
    double big=0.0;

    double* paij = pai->get_v() + lb;
    for (j=lb;j<=ub;++j)
    {
      double temp=fabs(*paij);
      if (temp > big)
      {
        big=temp;
      }

      ++paij;
    }
    if (big == 0.0)
    {
      std::ostream& output_stream = get_output_stream();
      output_stream << "Error: division by zero in ludcmp\n";
    }
    *pvvi = 1.0 / big;

    ++pai;
    ++pvvi;
  }



  for (j=lb;j<=ub;j++)
  {
    for (i=lb;i<j;i++)
    {
      double sum=a[i][j];
      for (k=lb;k<i;k++)
      {
        sum = sum - a[i][k]*a[k][j];
      }
      a[i][j]=sum;
    }
    int imax=j;
    double big=0.0;
    for (i=j;i<=ub;i++)
    {
      double sum=a[i][j];
      for (k=lb;k<j;k++)
      {
        sum = sum - a[i][k]*a[k][j];
      }
      a[i][j]=sum;
      double dum=vv[i]*fabs(sum);
      if ( dum >= big)
      {
        big=dum;
        imax=i;
      }
    }
    if (j != imax)
    {
      for (k=lb;k<=ub;k++)
      {
        double dum=a[imax][k];
        a[imax][k]=a[j][k];
        a[j][k]=dum;
      }
      d = -d;
      vv[imax]=vv[j];
    }
    indx[j]=imax;

    if (a[j][j] == 0.0)
    {
      a[j][j]=TINY;
    }

    if (j != n)
    {
      double dum=1.0/(a[j][j]);
      for (i=j+1;i<=ub;i++)
      {
        a[i][j] = a[i][j] * dum;
      }
    }
  }
}
#undef TINY

#define TINY 1.0e-50;

void ludcmp_det(const dmatrix& _a, const ivector& _indx, const double& _d)
{
  int i,j,k;
  double& d=(double&)_d;
  dmatrix& a=(dmatrix&)_a;
  ivector& indx=(ivector&)_indx;

#if !defined(OPT_LIB) && (__cplusplus >= 201103L)
  int n = [](unsigned int colsize) -> int
  {
    assert(colsize <= INT_MAX);
    return static_cast<int>(colsize);
  } (a.colsize());
#else
  int n = static_cast<int>(a.colsize());
#endif
  int lb=a.colmin();
  int ub=a.colmax();

  dvector vv(lb,ub);

  d=1.0;

  for (i=lb;i<=ub;i++)
  {
    double big=0.0;
    for (j=lb;j<=ub;j++)
    {
      double temp=fabs(a[i][j]);
      if (temp > big)
      {
        big=temp;
      }
    }
    if (big == 0.0)
    {
      d=0.;
    }
    vv[i]=1.0/big;
  }



  for (j=lb;j<=ub;j++)
  {
    for (i=lb;i<j;i++)
    {
      double sum=a[i][j];
      for (k=lb;k<i;k++)
      {
        sum = sum - a[i][k]*a[k][j];
      }
      a[i][j]=sum;
    }
    int imax = j;
    double big=0.0;
    for (i=j;i<=ub;i++)
    {
      double sum=a[i][j];
      for (k=lb;k<j;k++)
      {
        sum = sum - a[i][k]*a[k][j];
      }
      a[i][j]=sum;
      double dum=vv[i]*fabs(sum);
      if ( dum >= big)
      {
        big=dum;
        imax=i;
      }
    }
    if (j != imax)
    {
      for (k=lb;k<=ub;k++)
      {
        double dum=a[imax][k];
        a[imax][k]=a[j][k];
        a[j][k]=dum;
      }
      d = -d;
      vv[imax]=vv[j];
    }
    indx[j]=imax;

    if (a[j][j] == 0.0)
    {
      a[j][j]=TINY;
    }

    if (j != n)
    {
      double dum=1.0/(a[j][j]);
      for (i=j+1;i<=ub;i++)
      {
        a[i][j] = a[i][j] * dum;
      }
    }
  }
}


void lubksb(dmatrix a, const ivector& indx, dvector b)
{
  int i,ii=0,ip,j,iiflag=0;
  int lb=a.colmin();
  int ub=a.colmax();
  for (i=lb;i<=ub;i++)
  {
    ip=indx[i];
    double sum=b[ip];
    b[ip]=b[i];
    if (iiflag)
    {
      for (j=ii;j<=i-1;j++)
      {
        sum -= a[i][j]*b[j];
      }
    }
    else if (!ISZERO(sum))
    {
      ii=i;
      iiflag=1;
    }
    b[i]=sum;
  }

  for (i=ub;i>=lb;i--)
  {
    double sum=b[i];
    for (j=i+1;j<=ub;j++)
    {                        // !!! remove to show bug
      sum -= a[i][j]*b[j];
    }                        // !!! remove to show bug
    b[i]=sum/a[i][i];
  }
}

double det(const dmatrix& m1)
{
  double d = 0.0;
  dmatrix a(m1.rowmin(),m1.rowmax(),m1.rowmin(),m1.rowmax());

  if (m1.rowmin()!=m1.colmin()||m1.rowmax()!=m1.colmax())
  {
    cerr << "Matrix not square in routine det()" << endl;
    ad_exit(1);
  }

  for (int i=m1.rowmin(); i<=m1.rowmax(); i++)
  {
    for (int j=m1.rowmin(); j<=m1.rowmax(); j++)
    {
      a[i][j]=m1[i][j];
    }
  }

  ivector indx(m1.rowmin(),m1.rowmax());
  ludcmp_det(a,indx,d);
  for (int j=m1.rowmin();j<=m1.rowmax();j++)
  {
      d*=a[j][j];
  }

  return(d);
}

double ln_det(const dmatrix& m1, int& sgn)
{
  int min = m1.rowmin();
  int max = m1.rowmax();

#ifndef OPT_LIB
  if (min != m1.colmin() || max != m1.colmax())
  {
    cerr << "Matrix not square in routine det()" << endl;
    ad_exit(1);
  }
#endif

  dmatrix a(min, max, min, max);

  dvector* pai = &a(min);
  const dvector* pm1i = &m1(min);
  for (int i = min; i <= max; ++i)
  {
    double* pm1ij = pm1i->get_v() + min;
    double* paij = pai->get_v() + min;
    for (int j = min; j <= max; ++j)
    {
      *paij = *pm1ij;

      ++pm1ij;
      ++paij;
    }
    ++pm1i;
    ++pai;
  }

  ivector indx(min, max);

  double d = 0.0;
  ludcmp_det(a,indx,d);
  double ln_det=0.0;

  if (d>.1)
  {
    sgn=1;
  }
  else if (d<-0.1)
  {
    sgn=-1;
  }
  else
  {
    sgn=0;
  }
  for (int j = min; j <= max; ++j)
  {
    double ajj = a(j, j);
    if (ajj > 0)
    {
      ln_det += log(ajj);
    }
    else if (ajj < 0)
    {
      sgn=-sgn;
      ln_det += log(-ajj);
    }
    else
    {
      sgn = 0;
    }
  }
  return ln_det;
}

void ludcmp_index(const dmatrix& _a, const ivector& _indx, const double& _d)
{
  int i=0;
  int j=0;
  int k=0;
  double& d=(double&)_d;
  dmatrix& a=(dmatrix&)_a;
  ivector& indx=(ivector&)_indx;
#if !defined(OPT_LIB) && (__cplusplus >= 201103L)
  int n = [](unsigned int colsize) -> int
  {
    assert(colsize <= INT_MAX);
    return static_cast<int>(colsize);
  } (a.colsize());
#else
  int n = static_cast<int>(a.colsize());
#endif
  int lb=a.colmin();
  int ub=a.colmax();
  indx.fill_seqadd(lb,1);

  dvector vv(lb,ub);

  d=1.0;

  for (i=lb;i<=ub;i++)
  {
    double big=0.0;
    for (j=lb;j<=ub;j++)
    {
      double temp=fabs(a[i][j]);
      if (temp > big)
      {
        big=temp;
      }
    }
    if (big == 0.0)
    {
      cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
    }
    vv[i]=1.0/big;
  }



  for (j=lb;j<=ub;j++)
  {
    for (i=lb;i<j;i++)
    {
      double sum=a[i][j];
      for (k=lb;k<i;k++)
      {
        sum = sum - a[i][k]*a[k][j];
      }
      a[i][j]=sum;
    }
    int imax=j;
    double big=0.0;
    for (i=j;i<=ub;i++)
    {
      double sum=a[i][j];
      for (k=lb;k<j;k++)
      {
        sum = sum - a[i][k]*a[k][j];
      }
      a[i][j]=sum;
      double dum=vv[i]*fabs(sum);
      if ( dum >= big)
      {
        big=dum;
        imax=i;
      }
    }
    if (j != imax)
    {
      for (k=lb;k<=ub;k++)
      {
        double dum=a[imax][k];
        a[imax][k]=a[j][k];
        a[j][k]=dum;
      }
      d = -d;
      vv[imax]=vv[j];
      int itemp=indx.elem(imax);
      indx.elem(imax)=indx.elem(j);
      indx.elem(j)=itemp;
    }

    if (a[j][j] == 0.0)
    {
      a[j][j]=TINY;
    }

    if (j != n)
    {
      double dum=1.0/(a[j][j]);
      for (i=j+1;i<=ub;i++)
      {
        a[i][j] = a[i][j] * dum;
      }
    }
  }
}
#undef TINY