Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-08-05 08:18:26 

0001 /*
0002  * VMatrix.cpp
0003  *
0004  *  Created on: Feb 15, 2012
0005  *      Author: kleinwrt
0006  */
0007 
0008 /** \file
0009  *  VMatrix 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 "VMatrix.h"
0031 
0032 //! Namespace for the general broken lines package
0033 namespace gbl {
0034 
0035 /*********** simple Matrix based on std::vector<double> **********/
0036 
0037 VMatrix::VMatrix(const unsigned int nRows, const unsigned int nCols) :
0038         numRows(nRows), numCols(nCols), theVec(nRows * nCols) {
0039 }
0040 
0041 VMatrix::VMatrix(const VMatrix &aMatrix) :
0042         numRows(aMatrix.numRows), numCols(aMatrix.numCols), theVec(
0043                 aMatrix.theVec) {
0044 
0045 }
0046 
0047 VMatrix::~VMatrix() {
0048 }
0049 
0050 /// Resize Matrix.
0051 /**
0052  * \param [in] nRows Number of rows.
0053  * \param [in] nCols Number of columns.
0054  */
0055 void VMatrix::resize(const unsigned int nRows, const unsigned int nCols) {
0056     numRows = nRows;
0057     numCols = nCols;
0058     theVec.resize(nRows * nCols);
0059 }
0060 
0061 /// Get transposed matrix.
0062 /**
0063  * \return Transposed matrix.
0064  */
0065 VMatrix VMatrix::transpose() const {
0066     VMatrix aResult(numCols, numRows);
0067     for (unsigned int i = 0; i < numRows; ++i) {
0068         for (unsigned int j = 0; j < numCols; ++j) {
0069             aResult(j, i) = theVec[numCols * i + j];
0070         }
0071     }
0072     return aResult;
0073 }
0074 
0075 /// Get number of rows.
0076 /**
0077  * \return Number of rows.
0078  */
0079 unsigned int VMatrix::getNumRows() const {
0080     return numRows;
0081 }
0082 
0083 /// Get number of columns.
0084 /**
0085  * \return Number of columns.
0086  */
0087 unsigned int VMatrix::getNumCols() const {
0088     return numCols;
0089 }
0090 
0091 /// Print matrix.
0092 void VMatrix::print() const {
0093     std::cout << " VMatrix: " << numRows << "*" << numCols << std::endl;
0094     for (unsigned int i = 0; i < numRows; ++i) {
0095         for (unsigned int j = 0; j < numCols; ++j) {
0096             if (j % 5 == 0) {
0097                 std::cout << std::endl << std::setw(4) << i << ","
0098                         << std::setw(4) << j << "-" << std::setw(4)
0099                         << std::min(j + 4, numCols) << ":";
0100             }
0101             std::cout << std::setw(13) << theVec[numCols * i + j];
0102         }
0103     }
0104     std::cout << std::endl << std::endl;
0105 }
0106 
0107 /// Multiplication Matrix*Vector.
0108 VVector VMatrix::operator*(const VVector &aVector) const {
0109     VVector aResult(numRows);
0110     for (unsigned int i = 0; i < this->numRows; ++i) {
0111         double sum = 0.0;
0112         for (unsigned int j = 0; j < this->numCols; ++j) {
0113             sum += theVec[numCols * i + j] * aVector(j);
0114         }
0115         aResult(i) = sum;
0116     }
0117     return aResult;
0118 }
0119 
0120 /// Multiplication Matrix*Matrix.
0121 VMatrix VMatrix::operator*(const VMatrix &aMatrix) const {
0122 
0123     VMatrix aResult(numRows, aMatrix.numCols);
0124     for (unsigned int i = 0; i < numRows; ++i) {
0125         for (unsigned int j = 0; j < aMatrix.numCols; ++j) {
0126             double sum = 0.0;
0127             for (unsigned int k = 0; k < numCols; ++k) {
0128                 sum += theVec[numCols * i + k] * aMatrix(k, j);
0129             }
0130             aResult(i, j) = sum;
0131         }
0132     }
0133     return aResult;
0134 }
0135 
0136 /// Addition Matrix+Matrix.
0137 VMatrix VMatrix::operator+(const VMatrix &aMatrix) const {
0138     VMatrix aResult(numRows, numCols);
0139     for (unsigned int i = 0; i < numRows; ++i) {
0140         for (unsigned int j = 0; j < numCols; ++j) {
0141             aResult(i, j) = theVec[numCols * i + j] + aMatrix(i, j);
0142         }
0143     }
0144     return aResult;
0145 }
0146 
0147 /// Assignment Matrix=Matrix.
0148 VMatrix &VMatrix::operator=(const VMatrix &aMatrix) {
0149     if (this != &aMatrix) {   // Gracefully handle self assignment
0150         numRows = aMatrix.getNumRows();
0151         numCols = aMatrix.getNumCols();
0152         theVec.resize(numRows * numCols);
0153         for (unsigned int i = 0; i < numRows; ++i) {
0154             for (unsigned int j = 0; j < numCols; ++j) {
0155                 theVec[numCols * i + j] = aMatrix(i, j);
0156             }
0157         }
0158     }
0159     return *this;
0160 }
0161 
0162 /*********** simple symmetric Matrix based on std::vector<double> **********/
0163 
0164 VSymMatrix::VSymMatrix(const unsigned int nRows) :
0165         numRows(nRows), theVec((nRows * nRows + nRows) / 2) {
0166 }
0167 
0168 VSymMatrix::~VSymMatrix() {
0169 }
0170 
0171 /// Resize symmetric matrix.
0172 /**
0173  * \param [in] nRows Number of rows.
0174  */
0175 void VSymMatrix::resize(const unsigned int nRows) {
0176     numRows = nRows;
0177     theVec.resize((nRows * nRows + nRows) / 2);
0178 }
0179 
0180 /// Get number of rows (= number of colums).
0181 /**
0182  * \return Number of rows.
0183  */
0184 unsigned int VSymMatrix::getNumRows() const {
0185     return numRows;
0186 }
0187 
0188 /// Print matrix.
0189 void VSymMatrix::print() const {
0190     std::cout << " VSymMatrix: " << numRows << "*" << numRows << std::endl;
0191     for (unsigned int i = 0; i < numRows; ++i) {
0192         for (unsigned int j = 0; j <= i; ++j) {
0193             if (j % 5 == 0) {
0194                 std::cout << std::endl << std::setw(4) << i << ","
0195                         << std::setw(4) << j << "-" << std::setw(4)
0196                         << std::min(j + 4, i) << ":";
0197             }
0198             std::cout << std::setw(13) << theVec[(i * i + i) / 2 + j];
0199         }
0200     }
0201     std::cout << std::endl << std::endl;
0202 }
0203 
0204 /// Subtraction SymMatrix-(sym)Matrix.
0205 VSymMatrix VSymMatrix::operator-(const VMatrix &aMatrix) const {
0206     VSymMatrix aResult(numRows);
0207     for (unsigned int i = 0; i < numRows; ++i) {
0208         for (unsigned int j = 0; j <= i; ++j) {
0209             aResult(i, j) = theVec[(i * i + i) / 2 + j] - aMatrix(i, j);
0210         }
0211     }
0212     return aResult;
0213 }
0214 
0215 /// Multiplication SymMatrix*Vector.
0216 VVector VSymMatrix::operator*(const VVector &aVector) const {
0217     VVector aResult(numRows);
0218     for (unsigned int i = 0; i < numRows; ++i) {
0219         aResult(i) = theVec[(i * i + i) / 2 + i] * aVector(i);
0220         for (unsigned int j = 0; j < i; ++j) {
0221             aResult(j) += theVec[(i * i + i) / 2 + j] * aVector(i);
0222             aResult(i) += theVec[(i * i + i) / 2 + j] * aVector(j);
0223         }
0224     }
0225     return aResult;
0226 }
0227 
0228 /// Multiplication SymMatrix*Matrix.
0229 VMatrix VSymMatrix::operator*(const VMatrix &aMatrix) const {
0230     unsigned int nCol = aMatrix.getNumCols();
0231     VMatrix aResult(numRows, nCol);
0232     for (unsigned int l = 0; l < nCol; ++l) {
0233         for (unsigned int i = 0; i < numRows; ++i) {
0234             aResult(i, l) = theVec[(i * i + i) / 2 + i] * aMatrix(i, l);
0235             for (unsigned int j = 0; j < i; ++j) {
0236                 aResult(j, l) += theVec[(i * i + i) / 2 + j] * aMatrix(i, l);
0237                 aResult(i, l) += theVec[(i * i + i) / 2 + j] * aMatrix(j, l);
0238             }
0239         }
0240     }
0241     return aResult;
0242 }
0243 
0244 /*********** simple Vector based on std::vector<double> **********/
0245 
0246 VVector::VVector(const unsigned int nRows) :
0247         numRows(nRows), theVec(nRows) {
0248 }
0249 
0250 VVector::VVector(const VVector &aVector) :
0251         numRows(aVector.numRows), theVec(aVector.theVec) {
0252 
0253 }
0254 
0255 VVector::~VVector() {
0256 }
0257 
0258 /// Resize vector.
0259 /**
0260  * \param [in] nRows Number of rows.
0261  */
0262 void VVector::resize(const unsigned int nRows) {
0263     numRows = nRows;
0264     theVec.resize(nRows);
0265 }
0266 
0267 /// Get part of vector.
0268 /**
0269  * \param [in] len Length of part.
0270  * \param [in] start Offset of part.
0271  * \return Part of vector.
0272  */
0273 VVector VVector::getVec(unsigned int len, unsigned int start) const {
0274     VVector aResult(len);
0275     std::memcpy(&aResult.theVec[0], &theVec[start], sizeof(double) * len);
0276     return aResult;
0277 }
0278 
0279 /// Put part of vector.
0280 /**
0281  * \param [in] aVector Vector with part.
0282  * \param [in] start Offset of part.
0283  */
0284 void VVector::putVec(const VVector &aVector, unsigned int start) {
0285     std::memcpy(&theVec[start], &aVector.theVec[0],
0286             sizeof(double) * aVector.numRows);
0287 }
0288 
0289 /// Get number of rows.
0290 /**
0291  * \return Number of rows.
0292  */
0293 unsigned int VVector::getNumRows() const {
0294     return numRows;
0295 }
0296 
0297 /// Print vector.
0298 void VVector::print() const {
0299     std::cout << " VVector: " << numRows << std::endl;
0300     for (unsigned int i = 0; i < numRows; ++i) {
0301 
0302         if (i % 5 == 0) {
0303             std::cout << std::endl << std::setw(4) << i << "-" << std::setw(4)
0304                     << std::min(i + 4, numRows) << ":";
0305         }
0306         std::cout << std::setw(13) << theVec[i];
0307     }
0308     std::cout << std::endl << std::endl;
0309 }
0310 
0311 /// Subtraction Vector-Vector.
0312 VVector VVector::operator-(const VVector &aVector) const {
0313     VVector aResult(numRows);
0314     for (unsigned int i = 0; i < numRows; ++i) {
0315         aResult(i) = theVec[i] - aVector(i);
0316     }
0317     return aResult;
0318 }
0319 
0320 /// Assignment Vector=Vector.
0321 VVector &VVector::operator=(const VVector &aVector) {
0322     if (this != &aVector) {   // Gracefully handle self assignment
0323         numRows = aVector.getNumRows();
0324         theVec.resize(numRows);
0325         for (unsigned int i = 0; i < numRows; ++i) {
0326             theVec[i] = aVector(i);
0327         }
0328     }
0329     return *this;
0330 }
0331 
0332 /*============================================================================
0333  from mpnum.F (MillePede-II by V. Blobel, Univ. Hamburg)
0334  ============================================================================*/
0335 /// Matrix inversion.
0336 /**
0337  *     Invert symmetric N-by-N matrix V in symmetric storage mode
0338  *               V(1) = V11, V(2) = V12, V(3) = V22, V(4) = V13, . . .
0339  *               replaced by inverse matrix
0340  *
0341  *     Method of solution is by elimination selecting the  pivot  on  the
0342  *     diagonal each stage. The rank of the matrix is returned in  NRANK.
0343  *     For NRANK ne N, all remaining  rows  and  cols  of  the  resulting
0344  *     matrix V are  set  to  zero.
0345  *  \exception 1 : matrix is singular.
0346  *  \return Rank of matrix.
0347  */
0348 unsigned int VSymMatrix::invert() {
0349 
0350     const double eps = 1.0E-10;
0351     unsigned int aSize = numRows;
0352     std::vector<int> next(aSize);
0353     std::vector<double> diag(aSize);
0354     int nSize = aSize;
0355 
0356     int first = 1;
0357     for (int i = 1; i <= nSize; ++i) {
0358         next[i - 1] = i + 1; // set "next" pointer
0359         diag[i - 1] = fabs(theVec[(i * i + i) / 2 - 1]); // save abs of diagonal elements
0360     }
0361     next[aSize - 1] = -1; // end flag
0362 
0363     unsigned int nrank = 0;
0364     for (int i = 1; i <= nSize; ++i) { // start of loop
0365         int k = 0;
0366         double vkk = 0.0;
0367 
0368         int j = first;
0369         int previous = 0;
0370         int last = previous;
0371         // look for pivot
0372         while (j > 0) {
0373             int jj = (j * j + j) / 2 - 1;
0374             if (fabs(theVec[jj]) > std::max(fabs(vkk), eps * diag[j - 1])) {
0375                 vkk = theVec[jj];
0376                 k = j;
0377                 last = previous;
0378             }
0379             previous = j;
0380             j = next[j - 1];
0381         }
0382         // pivot found
0383         if (k > 0) {
0384             int kk = (k * k + k) / 2 - 1;
0385             if (last <= 0) {
0386                 first = next[k - 1];
0387             } else {
0388                 next[last - 1] = next[k - 1];
0389             }
0390             next[k - 1] = 0; // index is used, reset
0391             nrank++; // increase rank and ...
0392 
0393             vkk = 1.0 / vkk;
0394             theVec[kk] = -vkk;
0395             int jk = kk - k;
0396             int jl = -1;
0397             for (int m = 1; m <= nSize; ++m) { // elimination
0398                 if (m == k) {
0399                     jk = kk;
0400                     jl += m;
0401                 } else {
0402                     if (m < k) {
0403                         ++jk;
0404                     } else {
0405                         jk += m - 1;
0406                     }
0407 
0408                     double vjk = theVec[jk];
0409                     theVec[jk] = vkk * vjk;
0410                     int lk = kk - k;
0411                     if (m >= k) {
0412                         for (int l = 1; l <= k - 1; ++l) {
0413                             ++jl;
0414                             ++lk;
0415                             theVec[jl] -= theVec[lk] * vjk;
0416                         }
0417                         ++jl;
0418                         lk = kk;
0419                         for (int l = k + 1; l <= m; ++l) {
0420                             ++jl;
0421                             lk += l - 1;
0422                             theVec[jl] -= theVec[lk] * vjk;
0423                         }
0424                     } else {
0425                         for (int l = 1; l <= m; ++l) {
0426                             ++jl;
0427                             ++lk;
0428                             theVec[jl] -= theVec[lk] * vjk;
0429                         }
0430                     }
0431                 }
0432             }
0433         } else {
0434             for (int m = 1; m <= nSize; ++m) {
0435                 if (next[m - 1] >= 0) {
0436                     int kk = (m * m - m) / 2 - 1;
0437                     for (int n = 1; n <= nSize; ++n) {
0438                         if (next[n - 1] >= 0) {
0439                             theVec[kk + n] = 0.0; // clear matrix row/col
0440                         }
0441                     }
0442                 }
0443             }
0444             throw 1; // singular
0445         }
0446     }
0447     for (int ij = 0; ij < (nSize * nSize + nSize) / 2; ++ij) {
0448         theVec[ij] = -theVec[ij]; // finally reverse sign of all matrix elements
0449     }
0450     return nrank;
0451 }
0452 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!