Another set of UI messages fixes and tweaks! No functional changes.
[blender.git] / extern / Eigen3 / Eigen / src / Cholesky / LDLT.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Keir Mierle <mierle@gmail.com>
6 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
7 //
8 // Eigen is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 3 of the License, or (at your option) any later version.
12 //
13 // Alternatively, you can redistribute it and/or
14 // modify it under the terms of the GNU General Public License as
15 // published by the Free Software Foundation; either version 2 of
16 // the License, or (at your option) any later version.
17 //
18 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
19 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
20 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
21 // GNU General Public License for more details.
22 //
23 // You should have received a copy of the GNU Lesser General Public
24 // License and a copy of the GNU General Public License along with
25 // Eigen. If not, see <http://www.gnu.org/licenses/>.
26
27 #ifndef EIGEN_LDLT_H
28 #define EIGEN_LDLT_H
29
30 namespace internal {
31 template<typename MatrixType, int UpLo> struct LDLT_Traits;
32 }
33
34 /** \ingroup cholesky_Module
35   *
36   * \class LDLT
37   *
38   * \brief Robust Cholesky decomposition of a matrix with pivoting
39   *
40   * \param MatrixType the type of the matrix of which to compute the LDL^T Cholesky decomposition
41   *
42   * Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite
43   * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
44   * is lower triangular with a unit diagonal and D is a diagonal matrix.
45   *
46   * The decomposition uses pivoting to ensure stability, so that L will have
47   * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
48   * on D also stabilizes the computation.
49   *
50   * Remember that Cholesky decompositions are not rank-revealing. Also, do not use a Cholesky
51         * decomposition to determine whether a system of equations has a solution.
52   *
53   * \sa MatrixBase::ldlt(), class LLT
54   */
55  /* THIS PART OF THE DOX IS CURRENTLY DISABLED BECAUSE INACCURATE BECAUSE OF BUG IN THE DECOMPOSITION CODE
56   * Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
57   * the strict lower part does not have to store correct values.
58   */
59 template<typename _MatrixType, int _UpLo> class LDLT
60 {
61   public:
62     typedef _MatrixType MatrixType;
63     enum {
64       RowsAtCompileTime = MatrixType::RowsAtCompileTime,
65       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
66       Options = MatrixType::Options & ~RowMajorBit, // these are the options for the TmpMatrixType, we need a ColMajor matrix here!
67       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
68       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
69       UpLo = _UpLo
70     };
71     typedef typename MatrixType::Scalar Scalar;
72     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
73     typedef typename MatrixType::Index Index;
74     typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> TmpMatrixType;
75
76     typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
77     typedef PermutationMatrix<RowsAtCompileTime, MaxRowsAtCompileTime> PermutationType;
78
79     typedef internal::LDLT_Traits<MatrixType,UpLo> Traits;
80
81     /** \brief Default Constructor.
82       *
83       * The default constructor is useful in cases in which the user intends to
84       * perform decompositions via LDLT::compute(const MatrixType&).
85       */
86     LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {}
87
88     /** \brief Default Constructor with memory preallocation
89       *
90       * Like the default constructor but with preallocation of the internal data
91       * according to the specified problem \a size.
92       * \sa LDLT()
93       */
94     LDLT(Index size)
95       : m_matrix(size, size),
96         m_transpositions(size),
97         m_temporary(size),
98         m_isInitialized(false)
99     {}
100
101     LDLT(const MatrixType& matrix)
102       : m_matrix(matrix.rows(), matrix.cols()),
103         m_transpositions(matrix.rows()),
104         m_temporary(matrix.rows()),
105         m_isInitialized(false)
106     {
107       compute(matrix);
108     }
109
110     /** \returns a view of the upper triangular matrix U */
111     inline typename Traits::MatrixU matrixU() const
112     {
113       eigen_assert(m_isInitialized && "LDLT is not initialized.");
114       return Traits::getU(m_matrix);
115     }
116
117     /** \returns a view of the lower triangular matrix L */
118     inline typename Traits::MatrixL matrixL() const
119     {
120       eigen_assert(m_isInitialized && "LDLT is not initialized.");
121       return Traits::getL(m_matrix);
122     }
123
124     /** \returns the permutation matrix P as a transposition sequence.
125       */
126     inline const TranspositionType& transpositionsP() const
127     {
128       eigen_assert(m_isInitialized && "LDLT is not initialized.");
129       return m_transpositions;
130     }
131
132     /** \returns the coefficients of the diagonal matrix D */
133     inline Diagonal<const MatrixType> vectorD(void) const
134     {
135       eigen_assert(m_isInitialized && "LDLT is not initialized.");
136       return m_matrix.diagonal();
137     }
138
139     /** \returns true if the matrix is positive (semidefinite) */
140     inline bool isPositive(void) const
141     {
142       eigen_assert(m_isInitialized && "LDLT is not initialized.");
143       return m_sign == 1;
144     }
145     
146     #ifdef EIGEN2_SUPPORT
147     inline bool isPositiveDefinite() const
148     {
149       return isPositive();
150     }
151     #endif
152
153     /** \returns true if the matrix is negative (semidefinite) */
154     inline bool isNegative(void) const
155     {
156       eigen_assert(m_isInitialized && "LDLT is not initialized.");
157       return m_sign == -1;
158     }
159
160     /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
161       *
162       * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
163       *
164       * \note_about_checking_solutions
165       *
166       * More precisely, this method solves \f$ A x = b \f$ using the decomposition \f$ A = P^T L D L^* P \f$
167       * by solving the systems \f$ P^T y_1 = b \f$, \f$ L y_2 = y_1 \f$, \f$ D y_3 = y_2 \f$, 
168       * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
169       * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
170       * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
171       * computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular.
172       *
173       * \sa MatrixBase::ldlt()
174       */
175     template<typename Rhs>
176     inline const internal::solve_retval<LDLT, Rhs>
177     solve(const MatrixBase<Rhs>& b) const
178     {
179       eigen_assert(m_isInitialized && "LDLT is not initialized.");
180       eigen_assert(m_matrix.rows()==b.rows()
181                 && "LDLT::solve(): invalid number of rows of the right hand side matrix b");
182       return internal::solve_retval<LDLT, Rhs>(*this, b.derived());
183     }
184
185     #ifdef EIGEN2_SUPPORT
186     template<typename OtherDerived, typename ResultType>
187     bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const
188     {
189       *result = this->solve(b);
190       return true;
191     }
192     #endif
193
194     template<typename Derived>
195     bool solveInPlace(MatrixBase<Derived> &bAndX) const;
196
197     LDLT& compute(const MatrixType& matrix);
198
199     /** \returns the internal LDLT decomposition matrix
200       *
201       * TODO: document the storage layout
202       */
203     inline const MatrixType& matrixLDLT() const
204     {
205       eigen_assert(m_isInitialized && "LDLT is not initialized.");
206       return m_matrix;
207     }
208
209     MatrixType reconstructedMatrix() const;
210
211     inline Index rows() const { return m_matrix.rows(); }
212     inline Index cols() const { return m_matrix.cols(); }
213
214   protected:
215
216     /** \internal
217       * Used to compute and store the Cholesky decomposition A = L D L^* = U^* D U.
218       * The strict upper part is used during the decomposition, the strict lower
219       * part correspond to the coefficients of L (its diagonal is equal to 1 and
220       * is not stored), and the diagonal entries correspond to D.
221       */
222     MatrixType m_matrix;
223     TranspositionType m_transpositions;
224     TmpMatrixType m_temporary;
225     int m_sign;
226     bool m_isInitialized;
227 };
228
229 namespace internal {
230
231 template<int UpLo> struct ldlt_inplace;
232
233 template<> struct ldlt_inplace<Lower>
234 {
235   template<typename MatrixType, typename TranspositionType, typename Workspace>
236   static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
237   {
238     typedef typename MatrixType::Scalar Scalar;
239     typedef typename MatrixType::RealScalar RealScalar;
240     typedef typename MatrixType::Index Index;
241     eigen_assert(mat.rows()==mat.cols());
242     const Index size = mat.rows();
243
244     if (size <= 1)
245     {
246       transpositions.setIdentity();
247       if(sign)
248         *sign = real(mat.coeff(0,0))>0 ? 1:-1;
249       return true;
250     }
251
252     RealScalar cutoff = 0, biggest_in_corner;
253
254     for (Index k = 0; k < size; ++k)
255     {
256       // Find largest diagonal element
257       Index index_of_biggest_in_corner;
258       biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
259       index_of_biggest_in_corner += k;
260
261       if(k == 0)
262       {
263         // The biggest overall is the point of reference to which further diagonals
264         // are compared; if any diagonal is negligible compared
265         // to the largest overall, the algorithm bails.
266         cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
267
268         if(sign)
269           *sign = real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0 ? 1 : -1;
270       }
271
272       // Finish early if the matrix is not full rank.
273       if(biggest_in_corner < cutoff)
274       {
275         for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
276         break;
277       }
278
279       transpositions.coeffRef(k) = index_of_biggest_in_corner;
280       if(k != index_of_biggest_in_corner)
281       {
282         // apply the transposition while taking care to consider only
283         // the lower triangular part
284         Index s = size-index_of_biggest_in_corner-1; // trailing size after the biggest element
285         mat.row(k).head(k).swap(mat.row(index_of_biggest_in_corner).head(k));
286         mat.col(k).tail(s).swap(mat.col(index_of_biggest_in_corner).tail(s));
287         std::swap(mat.coeffRef(k,k),mat.coeffRef(index_of_biggest_in_corner,index_of_biggest_in_corner));
288         for(int i=k+1;i<index_of_biggest_in_corner;++i)
289         {
290           Scalar tmp = mat.coeffRef(i,k);
291           mat.coeffRef(i,k) = conj(mat.coeffRef(index_of_biggest_in_corner,i));
292           mat.coeffRef(index_of_biggest_in_corner,i) = conj(tmp);
293         }
294         if(NumTraits<Scalar>::IsComplex)
295           mat.coeffRef(index_of_biggest_in_corner,k) = conj(mat.coeff(index_of_biggest_in_corner,k));
296       }
297
298       // partition the matrix:
299       //       A00 |  -  |  -
300       // lu  = A10 | A11 |  -
301       //       A20 | A21 | A22
302       Index rs = size - k - 1;
303       Block<MatrixType,Dynamic,1> A21(mat,k+1,k,rs,1);
304       Block<MatrixType,1,Dynamic> A10(mat,k,0,1,k);
305       Block<MatrixType,Dynamic,Dynamic> A20(mat,k+1,0,rs,k);
306
307       if(k>0)
308       {
309         temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
310         mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
311         if(rs>0)
312           A21.noalias() -= A20 * temp.head(k);
313       }
314       if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
315         A21 /= mat.coeffRef(k,k);
316     }
317
318     return true;
319   }
320 };
321
322 template<> struct ldlt_inplace<Upper>
323 {
324   template<typename MatrixType, typename TranspositionType, typename Workspace>
325   static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
326   {
327     Transpose<MatrixType> matt(mat);
328     return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
329   }
330 };
331
332 template<typename MatrixType> struct LDLT_Traits<MatrixType,Lower>
333 {
334   typedef TriangularView<MatrixType, UnitLower> MatrixL;
335   typedef TriangularView<typename MatrixType::AdjointReturnType, UnitUpper> MatrixU;
336   inline static MatrixL getL(const MatrixType& m) { return m; }
337   inline static MatrixU getU(const MatrixType& m) { return m.adjoint(); }
338 };
339
340 template<typename MatrixType> struct LDLT_Traits<MatrixType,Upper>
341 {
342   typedef TriangularView<typename MatrixType::AdjointReturnType, UnitLower> MatrixL;
343   typedef TriangularView<MatrixType, UnitUpper> MatrixU;
344   inline static MatrixL getL(const MatrixType& m) { return m.adjoint(); }
345   inline static MatrixU getU(const MatrixType& m) { return m; }
346 };
347
348 } // end namespace internal
349
350 /** Compute / recompute the LDLT decomposition A = L D L^* = U^* D U of \a matrix
351   */
352 template<typename MatrixType, int _UpLo>
353 LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
354 {
355   eigen_assert(a.rows()==a.cols());
356   const Index size = a.rows();
357
358   m_matrix = a;
359
360   m_transpositions.resize(size);
361   m_isInitialized = false;
362   m_temporary.resize(size);
363
364   internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
365
366   m_isInitialized = true;
367   return *this;
368 }
369
370 namespace internal {
371 template<typename _MatrixType, int _UpLo, typename Rhs>
372 struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
373   : solve_retval_base<LDLT<_MatrixType,_UpLo>, Rhs>
374 {
375   typedef LDLT<_MatrixType,_UpLo> LDLTType;
376   EIGEN_MAKE_SOLVE_HELPERS(LDLTType,Rhs)
377
378   template<typename Dest> void evalTo(Dest& dst) const
379   {
380     eigen_assert(rhs().rows() == dec().matrixLDLT().rows());
381     // dst = P b
382     dst = dec().transpositionsP() * rhs();
383
384     // dst = L^-1 (P b)
385     dec().matrixL().solveInPlace(dst);
386
387     // dst = D^-1 (L^-1 P b)
388     // more precisely, use pseudo-inverse of D (see bug 241)
389     using std::abs;
390     using std::max;
391     typedef typename LDLTType::MatrixType MatrixType;
392     typedef typename LDLTType::Scalar Scalar;
393     typedef typename LDLTType::RealScalar RealScalar;
394     const Diagonal<const MatrixType> vectorD = dec().vectorD();
395     RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
396                                  RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
397     for (Index i = 0; i < vectorD.size(); ++i) {
398       if(abs(vectorD(i)) > tolerance)
399         dst.row(i) /= vectorD(i);
400       else
401         dst.row(i).setZero();
402     }
403
404     // dst = L^-T (D^-1 L^-1 P b)
405     dec().matrixU().solveInPlace(dst);
406
407     // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
408     dst = dec().transpositionsP().transpose() * dst;
409   }
410 };
411 }
412
413 /** \internal use x = ldlt_object.solve(x);
414   *
415   * This is the \em in-place version of solve().
416   *
417   * \param bAndX represents both the right-hand side matrix b and result x.
418   *
419   * \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
420   *
421   * This version avoids a copy when the right hand side matrix b is not
422   * needed anymore.
423   *
424   * \sa LDLT::solve(), MatrixBase::ldlt()
425   */
426 template<typename MatrixType,int _UpLo>
427 template<typename Derived>
428 bool LDLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
429 {
430   eigen_assert(m_isInitialized && "LDLT is not initialized.");
431   const Index size = m_matrix.rows();
432   eigen_assert(size == bAndX.rows());
433
434   bAndX = this->solve(bAndX);
435
436   return true;
437 }
438
439 /** \returns the matrix represented by the decomposition,
440  * i.e., it returns the product: P^T L D L^* P.
441  * This function is provided for debug purpose. */
442 template<typename MatrixType, int _UpLo>
443 MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
444 {
445   eigen_assert(m_isInitialized && "LDLT is not initialized.");
446   const Index size = m_matrix.rows();
447   MatrixType res(size,size);
448
449   // P
450   res.setIdentity();
451   res = transpositionsP() * res;
452   // L^* P
453   res = matrixU() * res;
454   // D(L^*P)
455   res = vectorD().asDiagonal() * res;
456   // L(DL^*P)
457   res = matrixL() * res;
458   // P^T (LDL^*P)
459   res = transpositionsP().transpose() * res;
460
461   return res;
462 }
463
464 /** \cholesky_module
465   * \returns the Cholesky decomposition with full pivoting without square root of \c *this
466   */
467 template<typename MatrixType, unsigned int UpLo>
468 inline const LDLT<typename SelfAdjointView<MatrixType, UpLo>::PlainObject, UpLo>
469 SelfAdjointView<MatrixType, UpLo>::ldlt() const
470 {
471   return LDLT<PlainObject,UpLo>(m_matrix);
472 }
473
474 /** \cholesky_module
475   * \returns the Cholesky decomposition with full pivoting without square root of \c *this
476   */
477 template<typename Derived>
478 inline const LDLT<typename MatrixBase<Derived>::PlainObject>
479 MatrixBase<Derived>::ldlt() const
480 {
481   return LDLT<PlainObject>(derived());
482 }
483
484 #endif // EIGEN_LDLT_H