00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_SKYLINEINPLACELU_H
00026 #define EIGEN_SKYLINEINPLACELU_H
00027
00028 namespace Eigen {
00029
00039 template<typename MatrixType>
00040 class SkylineInplaceLU {
00041 protected:
00042 typedef typename MatrixType::Scalar Scalar;
00043 typedef typename MatrixType::Index Index;
00044
00045 typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
00046
00047 public:
00048
00051 SkylineInplaceLU(MatrixType& matrix, int flags = 0)
00052 : m_flags(flags), m_status(0), m_lu(matrix) {
00053 m_precision = RealScalar(0.1) * Eigen::dummy_precision<RealScalar > ();
00054 m_lu.IsRowMajor ? computeRowMajor() : compute();
00055 }
00056
00067 void setPrecision(RealScalar v) {
00068 m_precision = v;
00069 }
00070
00074 RealScalar precision() const {
00075 return m_precision;
00076 }
00077
00086 void setFlags(int f) {
00087 m_flags = f;
00088 }
00089
00091 int flags() const {
00092 return m_flags;
00093 }
00094
00095 void setOrderingMethod(int m) {
00096 m_flags = m;
00097 }
00098
00099 int orderingMethod() const {
00100 return m_flags;
00101 }
00102
00104 void compute();
00105 void computeRowMajor();
00106
00108
00109
00111
00112
00113 template<typename BDerived, typename XDerived>
00114 bool solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x,
00115 const int transposed = 0) const;
00116
00118 inline bool succeeded(void) const {
00119 return m_succeeded;
00120 }
00121
00122 protected:
00123 RealScalar m_precision;
00124 int m_flags;
00125 mutable int m_status;
00126 bool m_succeeded;
00127 MatrixType& m_lu;
00128 };
00129
00133 template<typename MatrixType>
00134
00135 void SkylineInplaceLU<MatrixType>::compute() {
00136 const size_t rows = m_lu.rows();
00137 const size_t cols = m_lu.cols();
00138
00139 eigen_assert(rows == cols && "We do not (yet) support rectangular LU.");
00140 eigen_assert(!m_lu.IsRowMajor && "LU decomposition does not work with rowMajor Storage");
00141
00142 for (Index row = 0; row < rows; row++) {
00143 const double pivot = m_lu.coeffDiag(row);
00144
00145
00146 const Index& col = row;
00147 for (typename MatrixType::InnerLowerIterator lIt(m_lu, col); lIt; ++lIt) {
00148 lIt.valueRef() /= pivot;
00149 }
00150
00151
00152 typename MatrixType::InnerLowerIterator lIt(m_lu, col);
00153 for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) {
00154 typename MatrixType::InnerUpperIterator uItPivot(m_lu, row);
00155 typename MatrixType::InnerUpperIterator uIt(m_lu, rrow);
00156 const double coef = lIt.value();
00157
00158 uItPivot += (rrow - row - 1);
00159
00160
00161 for (++uItPivot; uIt && uItPivot;) {
00162 uIt.valueRef() -= uItPivot.value() * coef;
00163
00164 ++uIt;
00165 ++uItPivot;
00166 }
00167 ++lIt;
00168 }
00169
00170
00171 typename MatrixType::InnerLowerIterator lIt3(m_lu, col);
00172 for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) {
00173 typename MatrixType::InnerUpperIterator uItPivot(m_lu, row);
00174 const double coef = lIt3.value();
00175
00176
00177 for (Index i = 0; i < rrow - row - 1; i++) {
00178 m_lu.coeffRefLower(rrow, row + i + 1) -= uItPivot.value() * coef;
00179 ++uItPivot;
00180 }
00181 ++lIt3;
00182 }
00183
00184 typename MatrixType::InnerLowerIterator lIt2(m_lu, col);
00185 for (Index rrow = row + 1; rrow < m_lu.rows(); rrow++) {
00186
00187 typename MatrixType::InnerUpperIterator uItPivot(m_lu, row);
00188 typename MatrixType::InnerUpperIterator uIt(m_lu, rrow);
00189 const double coef = lIt2.value();
00190
00191 uItPivot += (rrow - row - 1);
00192 m_lu.coeffRefDiag(rrow) -= uItPivot.value() * coef;
00193 ++lIt2;
00194 }
00195 }
00196 }
00197
00198 template<typename MatrixType>
00199 void SkylineInplaceLU<MatrixType>::computeRowMajor() {
00200 const size_t rows = m_lu.rows();
00201 const size_t cols = m_lu.cols();
00202
00203 eigen_assert(rows == cols && "We do not (yet) support rectangular LU.");
00204 eigen_assert(m_lu.IsRowMajor && "You're trying to apply rowMajor decomposition on a ColMajor matrix !");
00205
00206 for (Index row = 0; row < rows; row++) {
00207 typename MatrixType::InnerLowerIterator llIt(m_lu, row);
00208
00209
00210 for (Index col = llIt.col(); col < row; col++) {
00211 if (m_lu.coeffExistLower(row, col)) {
00212 const double diag = m_lu.coeffDiag(col);
00213
00214 typename MatrixType::InnerLowerIterator lIt(m_lu, row);
00215 typename MatrixType::InnerUpperIterator uIt(m_lu, col);
00216
00217
00218 const Index offset = lIt.col() - uIt.row();
00219
00220
00221 Index stop = offset > 0 ? col - lIt.col() : col - uIt.row();
00222
00223
00224 #ifdef VECTORIZE
00225 Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop);
00226 Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop);
00227
00228
00229 Scalar newCoeff = m_lu.coeffLower(row, col) - rowVal.dot(colVal);
00230 #else
00231 if (offset > 0)
00232 uIt += offset;
00233 else
00234 lIt += -offset;
00235 Scalar newCoeff = m_lu.coeffLower(row, col);
00236
00237 for (Index k = 0; k < stop; ++k) {
00238 const Scalar tmp = newCoeff;
00239 newCoeff = tmp - lIt.value() * uIt.value();
00240 ++lIt;
00241 ++uIt;
00242 }
00243 #endif
00244
00245 m_lu.coeffRefLower(row, col) = newCoeff / diag;
00246 }
00247 }
00248
00249
00250 const Index col = row;
00251 typename MatrixType::InnerUpperIterator uuIt(m_lu, col);
00252 for (Index rrow = uuIt.row(); rrow < col; rrow++) {
00253
00254 typename MatrixType::InnerLowerIterator lIt(m_lu, rrow);
00255 typename MatrixType::InnerUpperIterator uIt(m_lu, col);
00256 const Index offset = lIt.col() - uIt.row();
00257
00258 Index stop = offset > 0 ? rrow - lIt.col() : rrow - uIt.row();
00259
00260 #ifdef VECTORIZE
00261 Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop);
00262 Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop);
00263
00264 Scalar newCoeff = m_lu.coeffUpper(rrow, col) - rowVal.dot(colVal);
00265 #else
00266 if (offset > 0)
00267 uIt += offset;
00268 else
00269 lIt += -offset;
00270 Scalar newCoeff = m_lu.coeffUpper(rrow, col);
00271 for (Index k = 0; k < stop; ++k) {
00272 const Scalar tmp = newCoeff;
00273 newCoeff = tmp - lIt.value() * uIt.value();
00274
00275 ++lIt;
00276 ++uIt;
00277 }
00278 #endif
00279 m_lu.coeffRefUpper(rrow, col) = newCoeff;
00280 }
00281
00282
00283
00284 typename MatrixType::InnerLowerIterator lIt(m_lu, row);
00285 typename MatrixType::InnerUpperIterator uIt(m_lu, row);
00286
00287 const Index offset = lIt.col() - uIt.row();
00288
00289
00290 Index stop = offset > 0 ? lIt.size() : uIt.size();
00291 #ifdef VECTORIZE
00292 Map<VectorXd > rowVal(lIt.valuePtr() + (offset > 0 ? 0 : -offset), stop);
00293 Map<VectorXd > colVal(uIt.valuePtr() + (offset > 0 ? offset : 0), stop);
00294 Scalar newCoeff = m_lu.coeffDiag(row) - rowVal.dot(colVal);
00295 #else
00296 if (offset > 0)
00297 uIt += offset;
00298 else
00299 lIt += -offset;
00300 Scalar newCoeff = m_lu.coeffDiag(row);
00301 for (Index k = 0; k < stop; ++k) {
00302 const Scalar tmp = newCoeff;
00303 newCoeff = tmp - lIt.value() * uIt.value();
00304 ++lIt;
00305 ++uIt;
00306 }
00307 #endif
00308 m_lu.coeffRefDiag(row) = newCoeff;
00309 }
00310 }
00311
00320 template<typename MatrixType>
00321 template<typename BDerived, typename XDerived>
00322 bool SkylineInplaceLU<MatrixType>::solve(const MatrixBase<BDerived> &b, MatrixBase<XDerived>* x, const int transposed) const {
00323 const size_t rows = m_lu.rows();
00324 const size_t cols = m_lu.cols();
00325
00326
00327 for (Index row = 0; row < rows; row++) {
00328 x->coeffRef(row) = b.coeff(row);
00329 Scalar newVal = x->coeff(row);
00330 typename MatrixType::InnerLowerIterator lIt(m_lu, row);
00331
00332 Index col = lIt.col();
00333 while (lIt.col() < row) {
00334
00335 newVal -= x->coeff(col++) * lIt.value();
00336 ++lIt;
00337 }
00338
00339 x->coeffRef(row) = newVal;
00340 }
00341
00342
00343 for (Index col = rows - 1; col > 0; col--) {
00344 x->coeffRef(col) = x->coeff(col) / m_lu.coeffDiag(col);
00345
00346 const Scalar x_col = x->coeff(col);
00347
00348 typename MatrixType::InnerUpperIterator uIt(m_lu, col);
00349 uIt += uIt.size()-1;
00350
00351
00352 while (uIt) {
00353 x->coeffRef(uIt.row()) -= x_col * uIt.value();
00354
00355 uIt += -1;
00356 }
00357
00358
00359 }
00360 x->coeffRef(0) = x->coeff(0) / m_lu.coeffDiag(0);
00361
00362 return true;
00363 }
00364
00365 }
00366
00367 #endif // EIGEN_SKYLINELU_H