JMatrixNS.hh

Go to the documentation of this file.

#ifndef __JMATH__JMATRIXNS__
#define __JMATH__JMATRIXNS__
 
#include <vector>
 
#include "JMath/JMatrixND.hh"
#include "JLang/JException.hh"
 
 
/**
 * \author mdejong
 */
 
namespace JMATH {}
namespace JPP { using namespace JMATH; }
 
namespace JMATH {
 
  using JLANG::JDivisionByZero;
  using JLANG::JIndexOutOfRange;
 
 
  /**
   * N x N symmetric matrix
   */
  class JMatrixNS :
    public JMatrixND
  {
  public:
    /**
     * Default constructor.
     */
    JMatrixNS() :
      JMatrixND()
    {}
 
 
    /**
     * Constructor (identity matrix).
     *
     * \param  size     dimension
     */
    JMatrixNS(const unsigned int size) :
      JMatrixND(size)
    {}
 
 
    /**
     * Contructor.
     *
     * \param   A      matrix
     */
    JMatrixNS(const JMatrixND& A) :
      JMatrixND(A)
    {}
 
 
    /**
     * Invert matrix.
     */
    void invert()
    {
      // LDU decomposition
 
      unsigned int i, j, k;
      double       val, del;
      row_type     row, root;
      col_type     col, cursor;
 
      JPermutationMatrix P;
 
      for (root = this->begin(), k = 0; root != this->end(); ++root, ++k) {
 
        row_type pivot;
 
        val   = (*root)[k];
        pivot = root;
 
        for (row = root; ++row != this->end(); ) {
          if (fabs((*row)[k]) > fabs(val)) {
            val   = (*row)[k];
            pivot = row;
          }
        }
 
        if (val == 0) {
          throw JDivisionByZero("LDU decomposition zero pivot");
        }
 
        if (pivot != root) {
 
          std::iter_swap(root, pivot);
 
          P.push_back(std::make_pair(std::distance(this->begin(),root),
                                     std::distance(this->begin(),pivot)));
        }
 
        // decompose in LDU
 
        for (row = root; ++row != this->end(); ) {
          
          del = (*row)[k] /= val;
          
          for (col = row->begin() + k, cursor = root->begin() + k; ++col != row->end(); ) {
            (*col) -= del * (*(++cursor));
          }
        }
      }
 
 
      // invert D
 
      for (k = 0; k != this->size(); ++k) {
 
        double& a = this->at(k,k);
        
        if (a == 0) {
          throw JDivisionByZero("LDU decomposition zero pivot");
        }
 
        a = 1.0/a;
      }
 
 
      // invert U
 
      for (root = this->begin(), k = 0; root != this->end(); ++root, ++k) {
 
        col_type pivot = root->begin() + k;
        
        for (col = pivot, j = k + 1; ++col != root->end(); ++j) {
          
          (*col) *= -(*pivot);
          
          for (cursor = pivot, row = root; ++cursor != col; ) {
            (*col) -= (*cursor) * (*(++row))[j];
          }
          
          (*col) *= this->at(j,j);
        }
      }
 
 
      // invert L
 
      for (i = size(); i != 0; ) {
 
        root = this->begin() + --i;
 
        for (j = i; j != 0; ) {
 
          double& a = (*root)[--j];
 
          a = -a;
 
          for (k = j + 1, cursor = root->begin() + k, row = this->begin() + k; k != i; ++cursor, ++row, ++k) {
            a -= (*cursor) * (*row)[j];
          }
        }
      }
 
 
      //  U^-1 x D^-1 x L^-1
 
      for (root = this->begin(), i = 0; root != this->end(); ++root, ++i) {
 
        val = (*root)[i];
        
        for (col = root->begin(), j = 0; j != i; ++col, ++j) {
          
          (*col) *= val;
          
          for (row = root, cursor = root->begin() + i; ++cursor != root->end(); ) {
            (*col) += (*cursor) * (*(++row))[j];
          }
        }
        
        for (j = i, col = root->begin() + j; j != this->size() - 1; ++col, ++j) {
          for (k = j + 1, row = this->begin() + k, cursor = root->begin() + k; k != this->size(); ++k, ++cursor, ++row) {
            (*col) += (*(cursor)) * (*(row))[j];
          }
        }
      }
 
 
      for (JPermutationMatrix::reverse_iterator p = P.rbegin(); p != P.rend(); ++p) {
        for (row = this->begin(); row != this->end(); ++row) {
          std::swap((*row)[p->first], (*row)[p->second]);
        }
      }
    }
 
 
    /**
     * Update inverted matrix at given diagonal element.
     *
     * If A is the original matrix and A' is such that for each (i,j) != (k,k):
     * <pre>
     *       A'[i][j] = A[i][j]          
     *       A'[k][k] = A[k][k] + extra
     * </pre>
     * then JMatrixNS::update(k, extra) will modify the already inverted matrix 
     * of A such that it will be the inverse of A'.
     *
     * \param   k      index of diagonal element
     * \param   extra  value to add
     */
    void update(const unsigned int k, const double extra)
    {
      if (k >= this->size()) {
        throw JIndexOutOfRange("JMatrixNS::invert(): index out of range.");
      }
 
      unsigned int i, j;
      row_type     row, root;
      col_type     col;
 
      const double del  =  this->at(k,k);
      const double din  =  1.0 / (1.0 + extra * del);
 
      root = this->begin() + k;
 
      for (i = 0, row = this->begin(); row != this->end(); ++i, ++row) {
 
        if (i != k) {
 
          const double val = (*row)[k];
 
          for (j = 0, col = row->begin(); col != row->end(); ++j, ++col) {
 
            if (j != k) {
              (*col) -= val * (*root)[j] * extra * din;
            }
          }
 
          (*row)[k] *= din;
        }
      }
 
      for (j = 0, col = root->begin(); col != root->end(); ++j, ++col) {
 
        if (j != k) {
          (*col) *= din;
        }
      }
 
      (*root)[k] = del * din;
    }
  };
}
 
#endif