SkylineInplaceLU.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Guillaume Saupin <guillaume.saupin@cea.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_SKYLINEINPLACELU_H
00011 #define EIGEN_SKYLINEINPLACELU_H
00012 
00013 namespace Eigen { 
00014 
00024 template<typename MatrixType>
00025 class SkylineInplaceLU {
00026 protected:
00027     typedef typename MatrixType::Scalar Scalar;
00028     typedef typename MatrixType::Index Index;
00029     
00030     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
00031 
00032 public:
00033 
00036     SkylineInplaceLU(MatrixType& matrix, int flags = 0)
00037     : /*m_matrix(matrix.rows(), matrix.cols()),*/ m_flags(flags), m_status(0), m_lu(matrix) {
00038         m_precision = RealScalar(0.1) * Eigen::dummy_precision<RealScalar > ();
00039         m_lu.IsRowMajor ? computeRowMajor() : compute();
00040     }
00041 
00052     void setPrecision(RealScalar v) {
00053         m_precision = v;
00054     }
00055 
00059     RealScalar precision() const {
00060         return m_precision;
00061     }
00062 
00071     void setFlags(int f) {
00072         m_flags = f;
00073     }
00074 
00076     int flags() const {
00077         return m_flags;
00078     }
00079 
00080     void setOrderingMethod(int m) {
00081         m_flags = m;
00082     }
00083 
00084     int orderingMethod() const {
00085         return m_flags;
00086     }
00087 
00089     void compute();
00090     void computeRowMajor();
00091 
00093     //inline const MatrixType& matrixL() const { return m_matrixL; }
00094 
00096     //inline const MatrixType& matrixU() const { return m_matrixU; }
00097 
00098     template<typename BDerived, typename XDerived>
00099     bool solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x,
00100             const int transposed = 0) const;
00101 
00103     inline bool succeeded(void) const {
00104         return m_succeeded;
00105     }
00106 
00107 protected:
00108     RealScalar m_precision;
00109     int m_flags;
00110     mutable int m_status;
00111     bool m_succeeded;
00112     MatrixType& m_lu;
00113 };
00114 
00118 template<typename MatrixType>
00119 //template<typename _Scalar>
00120 void SkylineInplaceLU<MatrixType>::compute() {
00121     const size_t rows = m_lu.rows();
00122     const size_t cols = m_lu.cols();
00123 
00124     eigen_assert(rows == cols && "We do not (yet) support rectangular LU.");
00125     eigen_assert(!m_lu.IsRowMajor && "LU decomposition does not work with rowMajor Storage");
00126 
00127     for (Index row = 0; row < rows; row++) {
00128         const double pivot = m_lu.coeffDiag(row);
00129 
00130         //Lower matrix Columns update
00131         const Index& col = row;
00132         for (typename MatrixType::InnerLowerIterator lIt(m_lu, col); lIt; ++lIt) {
00133             lIt.valueRef() /= pivot;
00134         }
00135 
00136         //Upper matrix update -> contiguous memory access
00137         typename MatrixType::InnerLowerIterator lIt(m_lu, col);
00138         for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) {
00139             typename MatrixType::InnerUpperIterator uItPivot(m_lu, row);
00140             typename MatrixType::InnerUpperIterator uIt(m_lu, rrow);
00141             const double coef = lIt.value();
00142 
00143             uItPivot += (rrow - row - 1);
00144 
00145             //update upper part  -> contiguous memory access
00146             for (++uItPivot; uIt && uItPivot;) {
00147                 uIt.valueRef() -= uItPivot.value() * coef;
00148 
00149                 ++uIt;
00150                 ++uItPivot;
00151             }
00152             ++lIt;
00153         }
00154 
00155         //Upper matrix update -> non contiguous memory access
00156         typename MatrixType::InnerLowerIterator lIt3(m_lu, col);
00157         for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) {
00158             typename MatrixType::InnerUpperIterator uItPivot(m_lu, row);
00159             const double coef = lIt3.value();
00160 
00161             //update lower part ->  non contiguous memory access
00162             for (Index i = 0; i < rrow - row - 1; i++) {
00163                 m_lu.coeffRefLower(rrow, row + i + 1) -= uItPivot.value() * coef;
00164                 ++uItPivot;
00165             }
00166             ++lIt3;
00167         }
00168         //update diag -> contiguous
00169         typename MatrixType::InnerLowerIterator lIt2(m_lu, col);
00170         for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) {
00171 
00172             typename MatrixType::InnerUpperIterator uItPivot(m_lu, row);
00173             typename MatrixType::InnerUpperIterator uIt(m_lu, rrow);
00174             const double coef = lIt2.value();
00175 
00176             uItPivot += (rrow - row - 1);
00177             m_lu.coeffRefDiag(rrow) -= uItPivot.value() * coef;
00178             ++lIt2;
00179         }
00180     }
00181 }
00182 
00183 template<typename MatrixType>
00184 void SkylineInplaceLU<MatrixType>::computeRowMajor() {
00185     const size_t rows = m_lu.rows();
00186     const size_t cols = m_lu.cols();
00187 
00188     eigen_assert(rows == cols && "We do not (yet) support rectangular LU.");
00189     eigen_assert(m_lu.IsRowMajor && "You're trying to apply rowMajor decomposition on a ColMajor matrix !");
00190 
00191     for (Index row = 0; row < rows; row++) {
00192         typename MatrixType::InnerLowerIterator llIt(m_lu, row);
00193 
00194 
00195         for (Index col = llIt.col(); col < row; col++) {
00196             if (m_lu.coeffExistLower(row, col)) {
00197                 const double diag = m_lu.coeffDiag(col);
00198 
00199                 typename MatrixType::InnerLowerIterator lIt(m_lu, row);
00200                 typename MatrixType::InnerUpperIterator uIt(m_lu, col);
00201 
00202 
00203                 const Index offset = lIt.col() - uIt.row();
00204 
00205 
00206                 Index stop = offset > 0 ? col - lIt.col() : col - uIt.row();
00207 
00208                 //#define VECTORIZE
00209 #ifdef VECTORIZE
00210                 Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop);
00211                 Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop);
00212 
00213 
00214                 Scalar newCoeff = m_lu.coeffLower(row, col) - rowVal.dot(colVal);
00215 #else
00216                 if (offset > 0) //Skip zero value of lIt
00217                     uIt += offset;
00218                 else //Skip zero values of uIt
00219                     lIt += -offset;
00220                 Scalar newCoeff = m_lu.coeffLower(row, col);
00221 
00222                 for (Index k = 0; k < stop; ++k) {
00223                     const Scalar tmp = newCoeff;
00224                     newCoeff = tmp - lIt.value() * uIt.value();
00225                     ++lIt;
00226                     ++uIt;
00227                 }
00228 #endif
00229 
00230                 m_lu.coeffRefLower(row, col) = newCoeff / diag;
00231             }
00232         }
00233 
00234         //Upper matrix update
00235         const Index col = row;
00236         typename MatrixType::InnerUpperIterator uuIt(m_lu, col);
00237         for (Index rrow = uuIt.row(); rrow < col; rrow++) {
00238 
00239             typename MatrixType::InnerLowerIterator lIt(m_lu, rrow);
00240             typename MatrixType::InnerUpperIterator uIt(m_lu, col);
00241             const Index offset = lIt.col() - uIt.row();
00242 
00243             Index stop = offset > 0 ? rrow - lIt.col() : rrow - uIt.row();
00244 
00245 #ifdef VECTORIZE
00246             Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop);
00247             Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop);
00248 
00249             Scalar newCoeff = m_lu.coeffUpper(rrow, col) - rowVal.dot(colVal);
00250 #else
00251             if (offset > 0) //Skip zero value of lIt
00252                 uIt += offset;
00253             else //Skip zero values of uIt
00254                 lIt += -offset;
00255             Scalar newCoeff = m_lu.coeffUpper(rrow, col);
00256             for (Index k = 0; k < stop; ++k) {
00257                 const Scalar tmp = newCoeff;
00258                 newCoeff = tmp - lIt.value() * uIt.value();
00259 
00260                 ++lIt;
00261                 ++uIt;
00262             }
00263 #endif
00264             m_lu.coeffRefUpper(rrow, col) = newCoeff;
00265         }
00266 
00267 
00268         //Diag matrix update
00269         typename MatrixType::InnerLowerIterator lIt(m_lu, row);
00270         typename MatrixType::InnerUpperIterator uIt(m_lu, row);
00271 
00272         const Index offset = lIt.col() - uIt.row();
00273 
00274 
00275         Index stop = offset > 0 ? lIt.size() : uIt.size();
00276 #ifdef VECTORIZE
00277         Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop);
00278         Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop);
00279         Scalar newCoeff = m_lu.coeffDiag(row) - rowVal.dot(colVal);
00280 #else
00281         if (offset > 0) //Skip zero value of lIt
00282             uIt += offset;
00283         else //Skip zero values of uIt
00284             lIt += -offset;
00285         Scalar newCoeff = m_lu.coeffDiag(row);
00286         for (Index k = 0; k < stop; ++k) {
00287             const Scalar tmp = newCoeff;
00288             newCoeff = tmp - lIt.value() * uIt.value();
00289             ++lIt;
00290             ++uIt;
00291         }
00292 #endif
00293         m_lu.coeffRefDiag(row) = newCoeff;
00294     }
00295 }
00296 
00305 template<typename MatrixType>
00306 template<typename BDerived, typename XDerived>
00307 bool SkylineInplaceLU<MatrixType>::solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x, const int transposed) const {
00308     const size_t rows = m_lu.rows();
00309     const size_t cols = m_lu.cols();
00310 
00311 
00312     for (Index row = 0; row < rows; row++) {
00313         x->coeffRef(row) = b.coeff(row);
00314         Scalar newVal = x->coeff(row);
00315         typename MatrixType::InnerLowerIterator lIt(m_lu, row);
00316 
00317         Index col = lIt.col();
00318         while (lIt.col() < row) {
00319 
00320             newVal -= x->coeff(col++) * lIt.value();
00321             ++lIt;
00322         }
00323 
00324         x->coeffRef(row) = newVal;
00325     }
00326 
00327 
00328     for (Index col = rows - 1; col > 0; col--) {
00329         x->coeffRef(col) = x->coeff(col) / m_lu.coeffDiag(col);
00330 
00331         const Scalar x_col = x->coeff(col);
00332 
00333         typename MatrixType::InnerUpperIterator uIt(m_lu, col);
00334         uIt += uIt.size()-1;
00335 
00336 
00337         while (uIt) {
00338             x->coeffRef(uIt.row()) -= x_col * uIt.value();
00339             //TODO : introduce --operator
00340             uIt += -1;
00341         }
00342 
00343 
00344     }
00345     x->coeffRef(0) = x->coeff(0) / m_lu.coeffDiag(0);
00346 
00347     return true;
00348 }
00349 
00350 } // end namespace Eigen
00351 
00352 #endif // EIGEN_SKYLINELU_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:39:07