GBL/src/BorderedBandMatrix.cc

0001 /*
0002  * BorderedBandMatrix.cpp
0003  *
0004  *  Created on: Aug 14, 2011
0005  *      Author: kleinwrt
0006  */
0007
0008 /** \file
0009  *  BorderedBandMatrix methods.
0010  *
0011  *  \author Claus Kleinwort, DESY, 2011 (Claus.Kleinwort@desy.de)
0012  *
0013  *  \copyright
0014  *  Copyright (c) 2011 - 2016 Deutsches Elektronen-Synchroton,
0015  *  Member of the Helmholtz Association, (DESY), HAMBURG, GERMANY \n\n
0016  *  This library is free software; you can redistribute it and/or modify
0017  *  it under the terms of the GNU Library General Public License as
0018  *  published by the Free Software Foundation; either version 2 of the
0019  *  License, or (at your option) any later version. \n\n
0020  *  This library is distributed in the hope that it will be useful,
0021  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
0022  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0023  *  GNU Library General Public License for more details. \n\n
0024  *  You should have received a copy of the GNU Library General Public
0025  *  License along with this program (see the file COPYING.LIB for more
0026  *  details); if not, write to the Free Software Foundation, Inc.,
0027  *  675 Mass Ave, Cambridge, MA 02139, USA.
0028  */
0029
0030 #include "BorderedBandMatrix.h"
0031
0032 //! Namespace for the general broken lines package
0033 namespace gbl {
0034
0035 /// Create bordered band matrix.
0036 BorderedBandMatrix::BorderedBandMatrix() : numSize(0), numBorder(0), numBand(0), numCol(0) {
0037 }
0038
0039 BorderedBandMatrix::~BorderedBandMatrix() {
0040 }
0041
0042 /// Resize bordered band matrix.
0043 /**
0044  * \param nSize [in] Size of matrix
0045  * \param nBorder [in] Size of border (=1 for q/p + additional local parameters)
0046  * \param nBand [in] Band width (usually = 5, for simplified jacobians = 4)
0047  */
0048 void BorderedBandMatrix::resize(unsigned int nSize, unsigned int nBorder,
0049         unsigned int nBand) {
0050     numSize = nSize;
0051     numBorder = nBorder;
0052     numCol = nSize - nBorder;
0053     numBand = 0;
0054     theBorder.resize(numBorder);
0055     theMixed.resize(numBorder, numCol);
0056     theBand.resize((nBand + 1), numCol);
0057 }
0058
0059 /// Add symmetric block matrix.
0060 /**
0061  * Add (extended) block matrix defined by 'aVector * aWeight * aVector.T'
0062  * to bordered band matrix:
0063  * BBmatrix(anIndex(i),anIndex(j)) += aVector(i) * aWeight * aVector(j).
0064  * \param aWeight [in] Weight
0065  * \param anIndex [in] List of rows/colums to be used
0066  * \param aVector [in] Vector
0067  */
0068 void BorderedBandMatrix::addBlockMatrix(double aWeight,
0069         const std::vector<unsigned int>* anIndex,
0070         const std::vector<double>* aVector) {
0071     int nBorder = numBorder;
0072     for (unsigned int i = 0; i < anIndex->size(); ++i) {
0073         int iIndex = (*anIndex)[i] - 1; // anIndex has to be sorted
0074         for (unsigned int j = 0; j <= i; ++j) {
0075             int jIndex = (*anIndex)[j] - 1;
0076             if (iIndex < nBorder) {
0077                 theBorder(iIndex, jIndex) += (*aVector)[i] * aWeight
0078                         * (*aVector)[j];
0079             } else if (jIndex < nBorder) {
0080                 theMixed(jIndex, iIndex - nBorder) += (*aVector)[i] * aWeight
0081                         * (*aVector)[j];
0082             } else {
0083                 unsigned int nBand = iIndex - jIndex;
0084                 theBand(nBand, jIndex - nBorder) += (*aVector)[i] * aWeight
0085                         * (*aVector)[j];
0086                 numBand = std::max(numBand, nBand); // update band width
0087             }
0088         }
0089     }
0090 }
0091
0092 /// Retrieve symmetric block matrix.
0093 /**
0094  * Get (compressed) block from bordered band matrix: aMatrix(i,j) = BBmatrix(anIndex(i),anIndex(j)).
0095  * \param anIndex [in] List of rows/colums to be used
0096  */
0097 TMatrixDSym BorderedBandMatrix::getBlockMatrix(
0098         const std::vector<unsigned int> &anIndex) const {
0099
0100     TMatrixDSym aMatrix(anIndex.size());
0101     int nBorder = numBorder;
0102     for (unsigned int i = 0; i < anIndex.size(); ++i) {
0103         int iIndex = anIndex[i] - 1; // anIndex has to be sorted
0104         for (unsigned int j = 0; j <= i; ++j) {
0105             int jIndex = anIndex[j] - 1;
0106             if (iIndex < nBorder) {
0107                 aMatrix(i, j) = theBorder(iIndex, jIndex); // border part of inverse
0108             } else if (jIndex < nBorder) {
0109                 aMatrix(i, j) = -theMixed(jIndex, iIndex - nBorder); // mixed part of inverse
0110             } else {
0111                 unsigned int nBand = iIndex - jIndex;
0112                 aMatrix(i, j) = theBand(nBand, jIndex - nBorder); // band part of inverse
0113             }
0114             aMatrix(j, i) = aMatrix(i, j);
0115         }
0116     }
0117     return aMatrix;
0118 }
0119
0120 /// Solve linear equation system, partially calculate inverse.
0121 /**
0122  * Solve linear equation A*x=b system with bordered band matrix A,
0123  * calculate bordered band part of inverse of A. Use decomposition
0124  * in border and band part for block matrix algebra:
0125  *
0126  *     | A  Ct |   | x1 |   | b1 |        , A  is the border part
0127  *     |       | * |    | = |    |        , Ct is the mixed part
0128  *     | C  D  |   | x2 |   | b2 |        , D  is the band part
0129  *
0130  * Explicit inversion of D is avoided by using solution X of D*X=C (X=D^-1*C,
0131  * obtained from Cholesky decomposition and forward/backward substitution)
0132  *
0133  *     | x1 |   | E*b1 - E*Xt*b2 |        , E^-1 = A-Ct*D^-1*C = A-Ct*X
0134  *     |    | = |                |
0135  *     | x2 |   |  x   - X*x1    |        , x is solution of D*x=b2 (x=D^-1*b2)
0136  *
0137  * Inverse matrix is:
0138  *
0139  *     |  E   -E*Xt          |
0140  *     |                     |            , only band part of (D^-1 + X*E*Xt)
0141  *     | -X*E  D^-1 + X*E*Xt |              is calculated
0142  *
0143  *
0144  * \param [in] aRightHandSide Right hand side (vector) 'b' of A*x=b
0145  * \param [out] aSolution Solution (vector) x of A*x=b
0146  */
0147 void BorderedBandMatrix::solveAndInvertBorderedBand(
0148         const VVector &aRightHandSide, VVector &aSolution) {
0149
0150     // decompose band
0151     decomposeBand();
0152     // invert band
0153     VMatrix inverseBand = invertBand();
0154     if (numBorder > 0) { // need to use block matrix decomposition to solve
0155         // solve for mixed part
0156         const VMatrix auxMat = solveBand(theMixed); // = Xt
0157         const VMatrix auxMatT = auxMat.transpose(); // = X
0158         // solve for border part
0159         const VVector auxVec = aRightHandSide.getVec(numBorder)
0160                 - auxMat * aRightHandSide.getVec(numCol, numBorder); // = b1 - Xt*b2
0161         VSymMatrix inverseBorder = theBorder - theMixed * auxMatT;
0162         inverseBorder.invert(); // = E
0163         const VVector borderSolution = inverseBorder * auxVec; // = x1
0164         // solve for band part
0165         const VVector bandSolution = solveBand(
0166                 aRightHandSide.getVec(numCol, numBorder)); // = x
0167         aSolution.putVec(borderSolution);
0168         aSolution.putVec(bandSolution - auxMatT * borderSolution, numBorder); // = x2
0169         // parts of inverse
0170         theBorder = inverseBorder; // E
0171         theMixed = inverseBorder * auxMat; // E*Xt (-mixed part of inverse) !!!
0172         theBand = inverseBand + bandOfAVAT(auxMatT, inverseBorder); // band(D^-1 + X*E*Xt)
0173     } else {
0174         aSolution.putVec(solveBand(aRightHandSide));
0175         theBand = inverseBand;
0176     }
0177 }
0178
0179 /// Print bordered band matrix.
0180 void BorderedBandMatrix::printMatrix() const {
0181     std::cout << "Border part " << std::endl;
0182     theBorder.print();
0183     std::cout << "Mixed  part " << std::endl;
0184     theMixed.print();
0185     std::cout << "Band   part " << std::endl;
0186     theBand.print();
0187 }
0188
0189 /*============================================================================
0190  from Dbandmatrix.F (MillePede-II by V. Blobel, Univ. Hamburg)
0191  ============================================================================*/
0192 /// (root free) Cholesky decomposition of band part: C=LDL^T
0193 /**
0194  * Decompose band matrix into diagonal matrix D and lower triangular band matrix
0195  * L (diagonal=1). Overwrite band matrix with D and off-diagonal part of L.
0196  *  \exception 2 : matrix is singular.
0197  *  \exception 3 : matrix is not positive definite.
0198  */
0199 void BorderedBandMatrix::decomposeBand() {
0200
0201     int nRow = numBand + 1;
0202     int nCol = numCol;
0203     VVector auxVec(nCol);
0204     for (int i = 0; i < nCol; ++i) {
0205         auxVec(i) = theBand(0, i) * 16.0; // save diagonal elements
0206     }
0207     for (int i = 0; i < nCol; ++i) {
0208         if ((theBand(0, i) + auxVec(i)) != theBand(0, i)) {
0209             theBand(0, i) = 1.0 / theBand(0, i);
0210             if (theBand(0, i) < 0.) {
0211                 throw 3; // not positive definite
0212             }
0213         } else {
0214             theBand(0, i) = 0.0;
0215             throw 2; // singular
0216         }
0217         for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
0218             double rxw = theBand(j, i) * theBand(0, i);
0219             for (int k = 0; k < std::min(nRow, nCol - i) - j; ++k) {
0220                 theBand(k, i + j) -= theBand(k + j, i) * rxw;
0221             }
0222             theBand(j, i) = rxw;
0223         }
0224     }
0225 }
0226
0227 /// Solve for band part.
0228 /**
0229  * Solve C*x=b for band part using decomposition C=LDL^T
0230  * and forward (L*z=b) and backward substitution (L^T*x=D^-1*z).
0231  * \param [in] aRightHandSide Right hand side (vector) 'b' of C*x=b
0232  * \return Solution (vector) 'x' of C*x=b
0233  */
0234 VVector BorderedBandMatrix::solveBand(const VVector &aRightHandSide) const {
0235
0236     int nRow = theBand.getNumRows();
0237     int nCol = theBand.getNumCols();
0238     VVector aSolution(aRightHandSide);
0239     for (int i = 0; i < nCol; ++i) // forward substitution
0240             {
0241         for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
0242             aSolution(j + i) -= theBand(j, i) * aSolution(i);
0243         }
0244     }
0245     for (int i = nCol - 1; i >= 0; i--) // backward substitution
0246             {
0247         double rxw = theBand(0, i) * aSolution(i);
0248         for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
0249             rxw -= theBand(j, i) * aSolution(j + i);
0250         }
0251         aSolution(i) = rxw;
0252     }
0253     return aSolution;
0254 }
0255
0256 /// solve band part for mixed part (border rows).
0257 /**
0258  * Solve C*X=B for mixed part using decomposition C=LDL^T
0259  * and forward and backward substitution.
0260  * \param [in] aRightHandSide Right hand side (matrix) 'B' of C*X=B
0261  * \return Solution (matrix) 'X' of C*X=B
0262  */
0263 VMatrix BorderedBandMatrix::solveBand(const VMatrix &aRightHandSide) const {
0264
0265     int nRow = theBand.getNumRows();
0266     int nCol = theBand.getNumCols();
0267     VMatrix aSolution(aRightHandSide);
0268     for (unsigned int iBorder = 0; iBorder < numBorder; iBorder++) {
0269         for (int i = 0; i < nCol; ++i) // forward substitution
0270                 {
0271             for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
0272                 aSolution(iBorder, j + i) -= theBand(j, i)
0273                         * aSolution(iBorder, i);
0274             }
0275         }
0276         for (int i = nCol - 1; i >= 0; i--) // backward substitution
0277                 {
0278             double rxw = theBand(0, i) * aSolution(iBorder, i);
0279             for (int j = 1; j < std::min(nRow, nCol - i); ++j) {
0280                 rxw -= theBand(j, i) * aSolution(iBorder, j + i);
0281             }
0282             aSolution(iBorder, i) = rxw;
0283         }
0284     }
0285     return aSolution;
0286 }
0287
0288 /// Invert band part.
0289 /**
0290  * \return Inverted band
0291  */
0292 VMatrix BorderedBandMatrix::invertBand() {
0293
0294     int nRow = numBand + 1;
0295     int nCol = numCol;
0296     VMatrix inverseBand(nRow, nCol);
0297
0298     for (int i = nCol - 1; i >= 0; i--) {
0299         double rxw = theBand(0, i);
0300         for (int j = i; j >= std::max(0, i - nRow + 1); j--) {
0301             for (int k = j + 1; k < std::min(nCol, j + nRow); ++k) {
0302                 rxw -= inverseBand(abs(i - k), std::min(i, k))
0303                         * theBand(k - j, j);
0304             }
0305             inverseBand(i - j, j) = rxw;
0306             rxw = 0.;
0307         }
0308     }
0309     return inverseBand;
0310 }
0311
0312 /// Calculate band part of: 'anArray * aSymArray * anArray.T'.
0313 /**
0314  * \return Band part of product
0315  */
0316 VMatrix BorderedBandMatrix::bandOfAVAT(const VMatrix &anArray,
0317         const VSymMatrix &aSymArray) const {
0318     int nBand = numBand;
0319     int nCol = numCol;
0320     int nBorder = numBorder;
0321     double sum;
0322     VMatrix aBand((nBand + 1), nCol);
0323     for (int i = 0; i < nCol; ++i) {
0324         for (int j = std::max(0, i - nBand); j <= i; ++j) {
0325             sum = 0.;
0326             for (int l = 0; l < nBorder; ++l) { // diagonal
0327                 sum += anArray(i, l) * aSymArray(l, l) * anArray(j, l);
0328                 for (int k = 0; k < l; ++k) { // off diagonal
0329                     sum += anArray(i, l) * aSymArray(l, k) * anArray(j, k)
0330                             + anArray(i, k) * aSymArray(l, k) * anArray(j, l);
0331                 }
0332             }
0333             aBand(i - j, j) = sum;
0334         }
0335     }
0336     return aBand;
0337 }
0338
0339 }