BorderedBandMatrix.cc

Go to the documentation of this file.

/*
 * BorderedBandMatrix.cpp
 *
 *  Created on: Aug 14, 2011
 *      Author: kleinwrt
 */

#include "Alignment/ReferenceTrajectories/interface/BorderedBandMatrix.h"
using namespace Eigen;

namespace gbl {

  BorderedBandMatrix::BorderedBandMatrix() :
    numSize(0), numBorder(0), numBand(0), numCol(0) {
  }

  BorderedBandMatrix::~BorderedBandMatrix() {
  }


  void BorderedBandMatrix::resize(unsigned int nSize, unsigned int nBorder,
                                  unsigned int nBand) {
    numSize = nSize;
    numBorder = nBorder;
    numCol = nSize - nBorder;
    numBand = 0;
    theBorder.resize(numBorder);
    theMixed.resize(numBorder, numCol);
    theBand.resize((nBand + 1), numCol);
  }


  void BorderedBandMatrix::addBlockMatrix(double aWeight,
                                          const std::vector<unsigned int>* anIndex,
                                          const std::vector<double>* aVector) {
    int nBorder = numBorder;
    for (unsigned int i = 0; i < anIndex->size(); ++i) {
      int iIndex = (*anIndex)[i] - 1; // anIndex has to be sorted
      for (unsigned int j = 0; j <= i; ++j) {
        int jIndex = (*anIndex)[j] - 1;
        if (iIndex < nBorder) {
          theBorder(iIndex, jIndex) += (*aVector)[i] * aWeight
            * (*aVector)[j];
        } else if (jIndex < nBorder) {
          theMixed(jIndex, iIndex - nBorder) += (*aVector)[i] * aWeight
            * (*aVector)[j];
        } else {
          unsigned int nBand = iIndex - jIndex;
          theBand(nBand, jIndex - nBorder) += (*aVector)[i] * aWeight
            * (*aVector)[j];
          numBand = std::max(numBand, nBand); // update band width
        }
      }
    }
  }


  void BorderedBandMatrix::addBlockMatrix(double aWeight, unsigned int aSize,
                                          unsigned int* anIndex, double* aVector) {
    int nBorder = numBorder;
    for (unsigned int i = 0; i < aSize; ++i) {
      int iIndex = anIndex[i] - 1; // anIndex has to be sorted
      for (unsigned int j = 0; j <= i; ++j) {
        int jIndex = anIndex[j] - 1;
        if (iIndex < nBorder) {
          theBorder(iIndex, jIndex) += aVector[i] * aWeight * aVector[j];
        } else if (jIndex < nBorder) {
          theMixed(jIndex, iIndex - nBorder) += aVector[i] * aWeight
            * aVector[j];
        } else {
          unsigned int nBand = iIndex - jIndex;
          theBand(nBand, jIndex - nBorder) += aVector[i] * aWeight
            * aVector[j];
          numBand = std::max(numBand, nBand); // update band width
        }
      }
    }
  }


  MatrixXd BorderedBandMatrix::getBlockMatrix(
                                              const std::vector<unsigned int> anIndex) const {

    MatrixXd aMatrix(anIndex.size(), anIndex.size());
    int nBorder = numBorder;
    for (unsigned int i = 0; i < anIndex.size(); ++i) {
      int iIndex = anIndex[i] - 1; // anIndex has to be sorted
      for (unsigned int j = 0; j <= i; ++j) {
        int jIndex = anIndex[j] - 1;
        if (iIndex < nBorder) {
          aMatrix(i, j) = theBorder(iIndex, jIndex); // border part of inverse
        } else if (jIndex < nBorder) {
          aMatrix(i, j) = -theMixed(jIndex, iIndex - nBorder); // mixed part of inverse
        } else {
          unsigned int nBand = iIndex - jIndex;
          aMatrix(i, j) = theBand(nBand, jIndex - nBorder); // band part of inverse
        }
        aMatrix(j, i) = aMatrix(i, j);
      }
    }
    return aMatrix;
  }


  MatrixXd BorderedBandMatrix::getBlockMatrix(unsigned int aSize,
                                              unsigned int* anIndex) const {

    MatrixXd aMatrix(aSize, aSize);
    int nBorder = numBorder;
    for (unsigned int i = 0; i < aSize; ++i) {
      int iIndex = anIndex[i] - 1; // anIndex has to be sorted
      for (unsigned int j = 0; j <= i; ++j) {
        int jIndex = anIndex[j] - 1;
        if (iIndex < nBorder) {
          aMatrix(i, j) = theBorder(iIndex, jIndex); // border part of inverse
        } else if (jIndex < nBorder) {
          aMatrix(i, j) = -theMixed(jIndex, iIndex - nBorder); // mixed part of inverse
        } else {
          unsigned int nBand = iIndex - jIndex;
          aMatrix(i, j) = theBand(nBand, jIndex - nBorder); // band part of inverse
        }
        aMatrix(j, i) = aMatrix(i, j);
      }
    }
    return aMatrix;
  }


  void BorderedBandMatrix::solveAndInvertBorderedBand(
                                                      const VVector &aRightHandSide, VVector &aSolution) {

    // decompose band
    decomposeBand();
    // invert band
    VMatrix inverseBand = invertBand();
    if (numBorder > 0) { // need to use block matrix decomposition to solve
      // solve for mixed part
      const VMatrix auxMat = solveBand(theMixed);// = Xt
      const VMatrix auxMatT = auxMat.transpose();// = X
      // solve for border part
      const VVector auxVec = aRightHandSide.getVec(numBorder)
        - auxMat * aRightHandSide.getVec(numCol, numBorder);// = b1 - Xt*b2
      VSymMatrix inverseBorder = theBorder - theMixed * auxMatT;
      inverseBorder.invert();// = E
      const VVector borderSolution = inverseBorder * auxVec;// = x1
      // solve for band part
      const VVector bandSolution = solveBand(
                                             aRightHandSide.getVec(numCol, numBorder));// = x
      aSolution.putVec(borderSolution);
      aSolution.putVec(bandSolution - auxMatT * borderSolution, numBorder);// = x2
      // parts of inverse
      theBorder = inverseBorder;// E
      theMixed = inverseBorder * auxMat;// E*Xt (-mixed part of inverse) !!!
      theBand = inverseBand + bandOfAVAT(auxMatT, inverseBorder);// band(D^-1 + X*E*Xt)
    } else {
      aSolution.putVec(solveBand(aRightHandSide));
      theBand = inverseBand;
    }
  }

  void BorderedBandMatrix::printMatrix() const {
    std::cout << "Border part " << std::endl;
    theBorder.print();
    std::cout << "Mixed  part " << std::endl;
    theMixed.print();
    std::cout << "Band   part " << std::endl;
    theBand.print();
  }

  /*============================================================================
    from Dbandmatrix.F (MillePede-II by V. Blobel, Univ. Hamburg)
    ============================================================================*/

  void BorderedBandMatrix::decomposeBand() {

    int nRow = numBand + 1;
    int nCol = numCol;
    VVector auxVec(nCol);
    for (int i = 0; i < nCol; ++i) {
      auxVec(i) = theBand(0, i) * 16.0; // save diagonal elements
    }
    for (int i = 0; i < nCol; ++i) {
      if ((theBand(0, i) + auxVec(i)) != theBand(0, i)) {
        theBand(0, i) = 1.0 / theBand(0, i);
        if (theBand(0, i) < 0.) {
          throw 3; // not positive definite
        }
      } else {
        theBand(0, i) = 0.0;
        throw 2; // singular
      }
      for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
        double rxw = theBand(j, i) * theBand(0, i);
        for (int k = 0; k < std::min(nRow, nCol - i) - j; ++k) {
          theBand(k, i + j) -= theBand(k + j, i) * rxw;
        }
        theBand(j, i) = rxw;
      }
    }
  }


  VVector BorderedBandMatrix::solveBand(const VVector &aRightHandSide) const {

    int nRow = theBand.getNumRows();
    int nCol = theBand.getNumCols();
    VVector aSolution(aRightHandSide);
    for (int i = 0; i < nCol; ++i) // forward substitution
      {
        for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
          aSolution(j + i) -= theBand(j, i) * aSolution(i);
        }
      }
    for (int i = nCol - 1; i >= 0; i--) // backward substitution
      {
        double rxw = theBand(0, i) * aSolution(i);
        for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
          rxw -= theBand(j, i) * aSolution(j + i);
        }
        aSolution(i) = rxw;
      }
    return aSolution;
  }


  VMatrix BorderedBandMatrix::solveBand(const VMatrix &aRightHandSide) const {

    int nRow = theBand.getNumRows();
    int nCol = theBand.getNumCols();
    VMatrix aSolution(aRightHandSide);
    for (unsigned int iBorder = 0; iBorder < numBorder; iBorder++) {
      for (int i = 0; i < nCol; ++i) // forward substitution
        {
          for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
            aSolution(iBorder, j + i) -= theBand(j, i)
              * aSolution(iBorder, i);
          }
        }
      for (int i = nCol - 1; i >= 0; i--) // backward substitution
        {
          double rxw = theBand(0, i) * aSolution(iBorder, i);
          for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
            rxw -= theBand(j, i) * aSolution(iBorder, j + i);
          }
          aSolution(iBorder, i) = rxw;
        }
    }
    return aSolution;
  }


  VMatrix BorderedBandMatrix::invertBand() {

    int nRow = numBand + 1;
    int nCol = numCol;
    VMatrix inverseBand(nRow, nCol);

    for (int i = nCol - 1; i >= 0; i--) {
      double rxw = theBand(0, i);
      for (int j = i; j >= std::max(0, i - nRow + 1); j--) {
        for (int k = j + 1; k < std::min(nCol, j + nRow); ++k) {
          rxw -= inverseBand(abs(i - k), std::min(i, k))
            * theBand(k - j, j);
        }
        inverseBand(i - j, j) = rxw;
        rxw = 0.;
      }
    }
    return inverseBand;
  }


  VMatrix BorderedBandMatrix::bandOfAVAT(const VMatrix &anArray,
                                         const VSymMatrix &aSymArray) const {
    int nBand = numBand;
    int nCol = numCol;
    int nBorder = numBorder;
    double sum;
    VMatrix aBand((nBand + 1), nCol);
    for (int i = 0; i < nCol; ++i) {
      for (int j = std::max(0, i - nBand); j <= i; ++j) {
        sum = 0.;
        for (int l = 0; l < nBorder; ++l) { // diagonal
          sum += anArray(i, l) * aSymArray(l, l) * anArray(j, l);
          for (int k = 0; k < l; ++k) { // off diagonal
            sum += anArray(i, l) * aSymArray(l, k) * anArray(j, k)
              + anArray(i, k) * aSymArray(l, k) * anArray(j, l);
          }
        }
        aBand(i - j, j) = sum;
      }
    }
    return aBand;
  }

}