dmat34.cpp

Go to the documentation of this file.

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

#ifdef __TURBOC__
  #pragma hdrstop
  #include <iostream.h>
#endif

#if defined (__WAT32__)
  #include <iostream.h>
  #include <strstrea.h>
#endif

#ifdef __ZTC__
  #include <iostream.hpp>
#endif

#define TINY 1.0e-20;
void dmdv_solve(void);


dvector csolve(const dmatrix& aa,const dvector& z)
{
  double ln_unsigned_det;
  double sign;
  dvector sol=solve(aa,z,ln_unsigned_det,sign);
  return sol;
}

dvector solve(const dmatrix& aa,const dvector& z)
{
  double ln_unsigned_det;
  double sign;
  dvector sol=solve(aa,z,ln_unsigned_det,sign);
  return sol;
}

dvector solve(const dmatrix& aa,const dvector& z,
  const double& _ln_unsigned_det,double& sign)
{
  double& ln_unsigned_det=(double&) (_ln_unsigned_det);
#if !defined(OPT_LIB) && (__cplusplus >= 201103L)
  int n = [](unsigned int colsize) -> int
  {
    assert(colsize <= INT_MAX);
    return static_cast<int>(colsize);
  } (aa.colsize());
#else
  int n = static_cast<int>(aa.colsize());
#endif
  int lb=aa.colmin();
  int ub=aa.colmax();
  if (lb!=aa.rowmin()||ub!=aa.rowmax())
  {
    cerr << "Error matrix not square in solve()"<<endl;
    ad_exit(1);
  }
  dmatrix bb(lb,ub,lb,ub);
  bb=aa;
  ivector indx(lb,ub);
  int One=1;
  indx.fill_seqadd(lb,One);
  double d;
  double big,dum,sum,temp;
  dvector vv(lb,ub);

  d=1.0;
  dvector* pbbi = &bb(lb);
  double* pvvi = &vv(lb);
  for (int i=lb;i<=ub;i++)
  {
    big=0.0;
    double* pbbij = pbbi->get_v() + lb;
    for (int j=lb;j<=ub;j++)
    {
      temp=fabs(*pbbij);
      if (temp > big)
      {
        big=temp;
      }
      ++pbbij;
    }
    if (big == 0.0)
    {
      cerr << "Error in matrix inverse -- matrix singular in inv(dmatrix)\n";
    }
    *pvvi = 1.0 / big;

    ++pvvi;
    ++pbbi;
  }

  dvector* pbbj = &bb(lb);
  double* pvvj = &vv(lb);
  for (int j=lb;j<=ub;j++)
  {
    dvector* pbbi = &bb(lb);
    for (int i=lb;i<j;i++)
    {
      sum = *(pbbi->get_v() + j);
      double* pbbik = pbbi->get_v() + lb;
      dvector* pbbk = &bb(lb);
      for (int k=lb;k<i;k++)
      {
        sum -= *pbbik * *(pbbk->get_v() + j);

        ++pbbik;
        ++pbbk;
      }
      //a[i][j]=sum;
      *(pbbi->get_v() + j) = sum;

      ++pbbi;
    }
    int imax = j;
    big=0.0;

    pbbi = &bb(j);
    pvvi = &vv(j);
    for (int i=j;i<=ub;i++)
    {
      sum = *(pbbi->get_v() + j);
      double* pbbik = pbbi->get_v() + lb;
      dvector* pbbk = &bb(lb);
      for (int k=lb;k<j;k++)
      {
        sum -= *pbbik * *(pbbk->get_v() + j);

        ++pbbik;
        ++pbbk;
      }
      *(pbbi->get_v() + j) = sum;
      dum = *pvvi * fabs(sum);
      if (dum >= big)
      {
        big = dum;
        imax = i;
      }
      ++pbbi;
      ++pvvi;
    }
    if (j != imax)
    {
      double* pbbjk = pbbj->get_v() + lb;
      double* pbbimaxk = bb(imax).get_v() + lb;
      for (int k=lb;k<=ub;k++)
      {
        dum = *pbbimaxk;
        *pbbimaxk = *pbbjk;
        *pbbjk = dum;

        ++pbbjk;
        ++pbbimaxk;
      }
      d = -1.*d;
      vv[imax] = *pvvj;

      //if (j<ub)
      {
        int itemp=indx(imax);
        indx(imax)=indx(j);
        indx(j)=itemp;
      }
      //cout << "indx= " <<indx<<endl;
    }

    double* pbbjj = pbbj->get_v() + j;
    if (*pbbjj == 0.0)
    {
      *pbbjj = TINY;
    }

    if (j != n)
    {
      dum = 1.0 / *pbbjj;
      pbbi = &bb(j + 1);
      for (int i=j+1;i<=ub;i++)
      {
        *(pbbi->get_v() + j) = *(pbbi->get_v() + j) * dum;

        ++pbbi;
      }
    }
    ++pbbj;
    ++pvvj;
  }

  // get the determinant
  sign=d;
  dvector part_prod(lb,ub);
  part_prod(lb)=log(fabs(bb(lb,lb)));
  if (bb(lb,lb)<0) sign=-sign;

  for (int j=lb+1;j<=ub;j++)
  {
    double bbjj = bb(j, j);
    if (bbjj < 0) sign=-sign;
    part_prod(j)=part_prod(j-1)+log(fabs(bbjj));
  }
  ln_unsigned_det=part_prod(ub);

  dvector x(lb,ub);
  dvector y(lb,ub);
  //int lb=rowmin;
  //int ub=rowmax;
  //dmatrix& b=bb;
  ivector indxinv(lb,ub);

  int* pindxi = indx.get_v() + lb;
  for (int i=lb;i<=ub;i++)
  {
    indxinv(*pindxi) = i;
    ++pindxi;
  }

  int* pindxinvi = indxinv.get_v() + lb;
  for (int i=lb;i<=ub;i++)
  {
    y(*pindxinvi)=z(i);
    ++pindxinvi;
  }

  pbbi = &bb(lb);
  double* pyi = &y(lb);
  for (int i=lb;i<=ub;i++)
  {
    sum = *pyi;

    double* pbbij = pbbi->get_v() + lb;
    double* pyj = &y(lb);
    for (int j=lb;j<=i-1;j++)
    {
      sum -= *pbbij * *pyj;

      ++pyj;
      ++pbbij;
    }
    *pyi = sum;

    ++pyi;
    ++pbbi;
  }
  pbbi = &bb(ub);
  pyi = &y(ub);
  double* pxi = x.get_v() + ub;
  for (int i=ub;i>=lb;i--)
  {
    sum = *pyi;

    double* pbbij = pbbi->get_v() + i + 1;
    double* pxj = x.get_v() + i + 1;
    for (int j=i+1;j<=ub;j++)
    {
      sum -= *pbbij * *pxj;

      ++pbbij;
      ++pxj;
    }
    *pxi = sum / *(pbbi->get_v() + i);

    --pbbi;
    --pyi;
    --pxi;
  }

  return x;
}
#undef TINY