qmatvec.cpp

来自「算断裂的」· C++ 代码 · 共 306 行

// ------------------------------------------------------------------
// qmatvec.cpp
//
// This file contains member functions for Matrix.
// ------------------------------------------------------------------
// Author: Stephen A. Vavasis
// Copyright (c) 1999 by Cornell University.  All rights reserved.
// 
// See the accompanying file 'Copyright' for authorship information,
// the terms of the license governing this software, and disclaimers
// concerning this software.
// ------------------------------------------------------------------
// This file is part of the QMG software.  
// Version 2.0 of QMG, release date September 3, 1999.
// ------------------------------------------------------------------

#include "qmatvec.h"


// Solve a linear system in place; return estimate of inverse norm.

QMG::Real QMG::Matrix::linear_solve(Real rhs[], Real x[]) {
  Matrix& m = *this;
  int n = m.numrows_;
  
#ifdef DEBUGGING
  if (n != m.numcols_ || n == 0) {
    throw_error("Attempt to solve nonsquare or empty system");
    return 0.0;
  }
#endif
  
  
  // If dim = 1 solve directly.
  
  if (n == 1) {
    if (m(0,0) == 0.0)
      return BIG_REAL;
    x[0] = rhs[0] / m(0,0);
    return 1.0 / fabs(m(0,0));
  }
  else if (n == 2) {
    // Use Cramer's rule for x[1] and backsub for x[0].
    Real det = m(0,0) * m(1,1) - m(1,0) * m(0,1);
    if (det == 0.0) 
      return BIG_REAL;
    x[1] = (m(0,0) * rhs[1] - m(1,0) * rhs[0]) / det;
    if (fabs(m(0,0)) >= fabs(m(1,0))) {
      x[0] = (rhs[0] - m(0,1) * x[1]) / m(0,0);
    }
    else {
      x[0] = (rhs[1] - m(1,1) * x[1]) / m(1,0);
    }
    return fabs(sqrt(m(0,0) * m(0,0) + m(0,1) * m(0,1)
      + m(1,0) * m(1,0) + m(1,1) * m(1,1)) / det);
  }
  
  // Use GEPP with partial pivoting.
  
  {
    for (int i = 0; i < n - 1; ++i) {
      // Scan for the pivot in col j.
      Real piv = m(i,i);
      Real abspiv = fabs(piv);
      int pivrow = i;
      {
        for (int i1 = i + 1; i1 < n; ++i1) {
          if (fabs(m(i1,i)) > abspiv) {
            pivrow  = i1;
            piv = m(i1,i);
            abspiv = fabs(piv);
          }
        }
      }
      if (piv == 0.0)
        return BIG_REAL;
      if (pivrow != i) {
        Real tmp;
        for (int j = i; j < n; ++j) {
          tmp = m(i,j);
          m(i,j) = m(pivrow,j);
          m(pivrow,j) = tmp;
        }
        tmp = rhs[i];
        rhs[i] = rhs[pivrow];
        rhs[pivrow] = tmp;
      }
      Real mult;
      {
        for (int i1 = i + 1; i1 < n; ++i1) {
          mult = m(i1,i) / piv;
          for (int j = i + 1; j < n; ++j) {
            m(i1,j) -= m(i,j) * mult;
          }
          rhs[i1] = rhs[i1] - rhs[i] * mult;
        }
      }
    }
    if (m(n-1, n-1) == 0.0)
      return BIG_REAL;
  }
    // Backsubstitution to get x.
  {
    
    for (int i = n - 1; i >= 0; --i) {
      Real t = rhs[i];
      for (int j = i + 1; j < n; ++j) {
        t -= m(i,j) * x[j];
      }
      x[i] = t / m(i,i);
    }
  }
  // Assume norm(inv(A)) == n * norminv(U) using backsubstitution.
  
  Real norminvU = 0.0;
  
  // Solve Ux = e_k
  // Store answer in rhs[j],
  {
    for (int k = 0; k < n; ++k) {
      Real t = 1.0;
      {
        for (int i = k; i >= 0; --i) {
          for (int j = i + 1; j <= k; ++j) {
            t -= m(i,j) * rhs[j];
          }
          rhs[i] = t / m(i,i);
          t = 0.0;
        }
      }
      {
        for (int i = 0; i <= k; ++i)
          norminvU += rhs[i] * rhs[i];
      }
    }
    // Now compute norm(U).
  }
  return n * sqrt(norminvU);
}


// Compute the determinant of a matrix.

QMG::Real
QMG::Matrix::determinant() const {
  const Matrix& a = *this;
  int n = a.numrows_;
#ifdef DEBUGGING
  if (n == 0 || n != a.numcols_) {
    throw_error("Nonsquare or zero matrix for determinant");
    return 0;
  }
#endif
  if (n == 1)
    return a(0,0);
  else if (n == 2)
    return a(0,0) * a(1,1) - a(1,0) * a(0,1);
  else if (n == 3)
    return a(0,0) * (a(1,1) * a(2,2) - a(1,2) * a(2,1)) + 
    a(1,0) * (a(0,2) * a(2,1) - a(0,1) * a(2,2)) +
    a(2,0) * (a(0,1) * a(1,2) - a(0,2) * a(1,1));
  // Do GEPP for the remaining cases.  Must make a temp copy of
  // a since we promised not to overwite a.

  Matrix m(n,n);
  {
  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < n; ++j) {
      m(i,j) = a(i,j);
    }
  }
  }
  Real det = 1.0;

  // Use GEPP with partial pivoting.
  {
    for (int i = 0; i < n; ++i) {
      // Scan for the pivot in col j.
      Real piv = m(i,i);
      Real abspiv = fabs(piv);
      int pivrow = i;
      {
        for (int i1 = i + 1; i1 < n; ++i1) {
          if (fabs(m(i1,i)) > abspiv) {
            pivrow  = i1;
            piv = m(i1,i);
            abspiv = fabs(piv);
          }
        }
      }
      if (pivrow != i) {
        Real tmp;
        for (int j = i; j < n; ++j) {
          tmp = m(i,j);
          m(i,j) = m(pivrow,j);
          m(pivrow,j) = tmp;
        }
        det *= -1.0;
      }
      Real mult;
      {
        for (int i1 = i + 1; i1 < n; ++i1) {
          mult = m(i1,i) / piv;
          for (int j = i + 1; j < n; ++j) {
            m(i1,j) -= m(i,j) * mult;
          }
        }
      }
      det *= piv;
    }
  }
  return det;
}

QMG::ostream& operator<<(QMG::ostream& os, const QMG::Matrix& mtx) {
  os << '[';
  for (int i = 0; i < mtx.numrows(); ++i) {
    for (int j = 0; j < mtx.numcols(); ++j) {
      os << mtx(i,j);
      if (j < mtx.numcols() - 1)
        os << ',';
    }
    if (i < mtx.numrows() - 1)
      os << ';';
  }
  return os << ']';
}

// Copying copies a pointer for heap arrays and sets the source to zero.
// (Like autoptr).

void QMG::Matrix::init_from_mtx_(Matrix& m) {
  if (m.heap_allocated_) {
    entries_ = m.entries_;
    numrows_ = m.numrows_;
    numcols_ = m.numcols_;
    heap_allocated_ = true;
    m.entries_ = 0;
    m.numrows_ = 0;
    m.entries_ = 0;
    m.heap_allocated_ = false;
  }
  else {
    entries_ = new Real[m.numrows_ * m.numcols_];
    numrows_ = m.numrows_;
    numcols_ = m.numcols_;
    heap_allocated_ = true;
    for (int i = 0; i < m.numrows_; ++i)
      for (int j = 0; j < m.numcols_; ++j)
        (*this)(i,j) = m(i,j);
  }
}

void
QMG::IntMatrix::init_from_mtx_(IntMatrix& m) {
  if (m.heap_allocated_) {
    entries_ = m.entries_;
    numrows_ = m.numrows_;
    numcols_ = m.numcols_;
    heap_allocated_ = true;
    m.entries_ = 0;
    m.numrows_ = 0;
    m.entries_ = 0;
    m.heap_allocated_ = false;
  }
  else {
    entries_ = new int[m.numrows_ * m.numcols_];
    numrows_ = m.numrows_;
    numcols_ = m.numcols_;
    heap_allocated_ = true;
    for (int i = 0; i < m.numrows_; ++i)
      for (int j = 0; j < m.numcols_; ++j)
        (*this)(i,j) = m(i,j);
  }
}


// Compute a householder transform that zeros out entries
// 2..n of the input vector vec.  Put result in hhtrans.


QMG::Real QMG::Matrix::compute_hh_transform(Real* hhtrans, 
                                            const Real* vec, 
                                            int vec_stride, 
                                            int n) {
  Real sigma = 0.0;
  for (int i = 1; i < n; ++i) {
    hhtrans[i] = vec[i * vec_stride];
    sigma += hhtrans[i] * hhtrans[i];
  }
  Real beta;
  if (sigma == 0.0) {
    hhtrans[0] = 0.0;
    beta = 0.0;
  }
  else {
    Real mu = sqrt(vec[0] * vec[0] + sigma);
    if (vec[0] < 0.0)
      hhtrans[0] = vec[0] - mu;
    else
      hhtrans[0] = -sigma / (vec[0] + mu);
    beta = -1.0 / (mu * hhtrans[0]);
  }
  return beta;
}