Cycles: svn merge -r41225:41232 ^/trunk/blender
[blender.git] / extern / Eigen2 / Eigen / src / QR / Tridiagonalization.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra. Eigen itself is part of the KDE project.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24
25 #ifndef EIGEN_TRIDIAGONALIZATION_H
26 #define EIGEN_TRIDIAGONALIZATION_H
27
28 /** \ingroup QR_Module
29   * \nonstableyet
30   *
31   * \class Tridiagonalization
32   *
33   * \brief Trigiagonal decomposition of a selfadjoint matrix
34   *
35   * \param MatrixType the type of the matrix of which we are performing the tridiagonalization
36   *
37   * This class performs a tridiagonal decomposition of a selfadjoint matrix \f$ A \f$ such that:
38   * \f$ A = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real symmetric tridiagonal matrix.
39   *
40   * \sa MatrixBase::tridiagonalize()
41   */
42 template<typename _MatrixType> class Tridiagonalization
43 {
44   public:
45
46     typedef _MatrixType MatrixType;
47     typedef typename MatrixType::Scalar Scalar;
48     typedef typename NumTraits<Scalar>::Real RealScalar;
49     typedef typename ei_packet_traits<Scalar>::type Packet;
50
51     enum {
52       Size = MatrixType::RowsAtCompileTime,
53       SizeMinusOne = MatrixType::RowsAtCompileTime==Dynamic
54                    ? Dynamic
55                    : MatrixType::RowsAtCompileTime-1,
56       PacketSize = ei_packet_traits<Scalar>::size
57     };
58
59     typedef Matrix<Scalar, SizeMinusOne, 1> CoeffVectorType;
60     typedef Matrix<RealScalar, Size, 1> DiagonalType;
61     typedef Matrix<RealScalar, SizeMinusOne, 1> SubDiagonalType;
62
63     typedef typename NestByValue<DiagonalCoeffs<MatrixType> >::RealReturnType DiagonalReturnType;
64
65     typedef typename NestByValue<DiagonalCoeffs<
66         NestByValue<Block<MatrixType,SizeMinusOne,SizeMinusOne> > > >::RealReturnType SubDiagonalReturnType;
67
68     /** This constructor initializes a Tridiagonalization object for
69       * further use with Tridiagonalization::compute()
70       */
71     Tridiagonalization(int size = Size==Dynamic ? 2 : Size)
72       : m_matrix(size,size), m_hCoeffs(size-1)
73     {}
74
75     Tridiagonalization(const MatrixType& matrix)
76       : m_matrix(matrix),
77         m_hCoeffs(matrix.cols()-1)
78     {
79       _compute(m_matrix, m_hCoeffs);
80     }
81
82     /** Computes or re-compute the tridiagonalization for the matrix \a matrix.
83       *
84       * This method allows to re-use the allocated data.
85       */
86     void compute(const MatrixType& matrix)
87     {
88       m_matrix = matrix;
89       m_hCoeffs.resize(matrix.rows()-1, 1);
90       _compute(m_matrix, m_hCoeffs);
91     }
92
93     /** \returns the householder coefficients allowing to
94       * reconstruct the matrix Q from the packed data.
95       *
96       * \sa packedMatrix()
97       */
98     inline CoeffVectorType householderCoefficients(void) const { return m_hCoeffs; }
99
100     /** \returns the internal result of the decomposition.
101       *
102       * The returned matrix contains the following information:
103       *  - the strict upper part is equal to the input matrix A
104       *  - the diagonal and lower sub-diagonal represent the tridiagonal symmetric matrix (real).
105       *  - the rest of the lower part contains the Householder vectors that, combined with
106       *    Householder coefficients returned by householderCoefficients(),
107       *    allows to reconstruct the matrix Q as follow:
108       *       Q = H_{N-1} ... H_1 H_0
109       *    where the matrices H are the Householder transformations:
110       *       H_i = (I - h_i * v_i * v_i')
111       *    where h_i == householderCoefficients()[i] and v_i is a Householder vector:
112       *       v_i = [ 0, ..., 0, 1, M(i+2,i), ..., M(N-1,i) ]
113       *
114       * See LAPACK for further details on this packed storage.
115       */
116     inline const MatrixType& packedMatrix(void) const { return m_matrix; }
117
118     MatrixType matrixQ(void) const;
119     MatrixType matrixT(void) const;
120     const DiagonalReturnType diagonal(void) const;
121     const SubDiagonalReturnType subDiagonal(void) const;
122
123     static void decomposeInPlace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ = true);
124
125   private:
126
127     static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs);
128
129     static void _decomposeInPlace3x3(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ = true);
130
131   protected:
132     MatrixType m_matrix;
133     CoeffVectorType m_hCoeffs;
134 };
135
136 /** \returns an expression of the diagonal vector */
137 template<typename MatrixType>
138 const typename Tridiagonalization<MatrixType>::DiagonalReturnType
139 Tridiagonalization<MatrixType>::diagonal(void) const
140 {
141   return m_matrix.diagonal().nestByValue().real();
142 }
143
144 /** \returns an expression of the sub-diagonal vector */
145 template<typename MatrixType>
146 const typename Tridiagonalization<MatrixType>::SubDiagonalReturnType
147 Tridiagonalization<MatrixType>::subDiagonal(void) const
148 {
149   int n = m_matrix.rows();
150   return Block<MatrixType,SizeMinusOne,SizeMinusOne>(m_matrix, 1, 0, n-1,n-1)
151     .nestByValue().diagonal().nestByValue().real();
152 }
153
154 /** constructs and returns the tridiagonal matrix T.
155   * Note that the matrix T is equivalent to the diagonal and sub-diagonal of the packed matrix.
156   * Therefore, it might be often sufficient to directly use the packed matrix, or the vector
157   * expressions returned by diagonal() and subDiagonal() instead of creating a new matrix.
158   */
159 template<typename MatrixType>
160 typename Tridiagonalization<MatrixType>::MatrixType
161 Tridiagonalization<MatrixType>::matrixT(void) const
162 {
163   // FIXME should this function (and other similar ones) rather take a matrix as argument
164   // and fill it ? (to avoid temporaries)
165   int n = m_matrix.rows();
166   MatrixType matT = m_matrix;
167   matT.corner(TopRight,n-1, n-1).diagonal() = subDiagonal().template cast<Scalar>().conjugate();
168   if (n>2)
169   {
170     matT.corner(TopRight,n-2, n-2).template part<UpperTriangular>().setZero();
171     matT.corner(BottomLeft,n-2, n-2).template part<LowerTriangular>().setZero();
172   }
173   return matT;
174 }
175
176 #ifndef EIGEN_HIDE_HEAVY_CODE
177
178 /** \internal
179   * Performs a tridiagonal decomposition of \a matA in place.
180   *
181   * \param matA the input selfadjoint matrix
182   * \param hCoeffs returned Householder coefficients
183   *
184   * The result is written in the lower triangular part of \a matA.
185   *
186   * Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
187   *
188   * \sa packedMatrix()
189   */
190 template<typename MatrixType>
191 void Tridiagonalization<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs)
192 {
193   assert(matA.rows()==matA.cols());
194   int n = matA.rows();
195 //   std::cerr << matA << "\n\n";
196   for (int i = 0; i<n-2; ++i)
197   {
198     // let's consider the vector v = i-th column starting at position i+1
199
200     // start of the householder transformation
201     // squared norm of the vector v skipping the first element
202     RealScalar v1norm2 = matA.col(i).end(n-(i+2)).squaredNorm();
203
204     // FIXME comparing against 1
205     if (ei_isMuchSmallerThan(v1norm2,static_cast<Scalar>(1)))
206     {
207       hCoeffs.coeffRef(i) = 0.;
208     }
209     else
210     {
211       Scalar v0 = matA.col(i).coeff(i+1);
212       RealScalar beta = ei_sqrt(ei_abs2(v0)+v1norm2);
213       if (ei_real(v0)>=0.)
214         beta = -beta;
215       matA.col(i).end(n-(i+2)) *= (Scalar(1)/(v0-beta));
216       matA.col(i).coeffRef(i+1) = beta;
217       Scalar h = (beta - v0) / beta;
218       // end of the householder transformation
219
220       // Apply similarity transformation to remaining columns,
221       // i.e., A = H' A H where H = I - h v v' and v = matA.col(i).end(n-i-1)
222
223       matA.col(i).coeffRef(i+1) = 1;
224
225       /* This is the initial algorithm which minimize operation counts and maximize
226        * the use of Eigen's expression. Unfortunately, the first matrix-vector product
227        * using Part<LowerTriangular|Selfadjoint>  is very very slow */
228       #ifdef EIGEN_NEVER_DEFINED
229       // matrix - vector product
230       hCoeffs.end(n-i-1) = (matA.corner(BottomRight,n-i-1,n-i-1).template part<LowerTriangular|SelfAdjoint>()
231                                 * (h * matA.col(i).end(n-i-1))).lazy();
232       // simple axpy
233       hCoeffs.end(n-i-1) += (h * Scalar(-0.5) * matA.col(i).end(n-i-1).dot(hCoeffs.end(n-i-1)))
234                             * matA.col(i).end(n-i-1);
235       // rank-2 update
236       //Block<MatrixType,Dynamic,1> B(matA,i+1,i,n-i-1,1);
237       matA.corner(BottomRight,n-i-1,n-i-1).template part<LowerTriangular>() -=
238             (matA.col(i).end(n-i-1) * hCoeffs.end(n-i-1).adjoint()).lazy()
239           + (hCoeffs.end(n-i-1) * matA.col(i).end(n-i-1).adjoint()).lazy();
240       #endif
241       /* end initial algorithm */
242
243       /* If we still want to minimize operation count (i.e., perform operation on the lower part only)
244        * then we could provide the following algorithm for selfadjoint - vector product. However, a full
245        * matrix-vector product is still faster (at least for dynamic size, and not too small, did not check
246        * small matrices). The algo performs block matrix-vector and transposed matrix vector products. */
247       #ifdef EIGEN_NEVER_DEFINED
248       int n4 = (std::max(0,n-4)/4)*4;
249       hCoeffs.end(n-i-1).setZero();
250       for (int b=i+1; b<n4; b+=4)
251       {
252         // the ?x4 part:
253         hCoeffs.end(b-4) +=
254             Block<MatrixType,Dynamic,4>(matA,b+4,b,n-b-4,4) * matA.template block<4,1>(b,i);
255         // the respective transposed part:
256         Block<CoeffVectorType,4,1>(hCoeffs, b, 0, 4,1) +=
257             Block<MatrixType,Dynamic,4>(matA,b+4,b,n-b-4,4).adjoint() * Block<MatrixType,Dynamic,1>(matA,b+4,i,n-b-4,1);
258         // the 4x4 block diagonal:
259         Block<CoeffVectorType,4,1>(hCoeffs, b, 0, 4,1) +=
260             (Block<MatrixType,4,4>(matA,b,b,4,4).template part<LowerTriangular|SelfAdjoint>()
261              * (h * Block<MatrixType,4,1>(matA,b,i,4,1))).lazy();
262       }
263       #endif
264       // todo: handle the remaining part
265       /* end optimized selfadjoint - vector product */
266
267       /* Another interesting note: the above rank-2 update is much slower than the following hand written loop.
268        * After an analyze of the ASM, it seems GCC (4.2) generate poor code because of the Block. Moreover,
269        * if we remove the specialization of Block for Matrix then it is even worse, much worse ! */
270       #ifdef EIGEN_NEVER_DEFINED
271       for (int j1=i+1; j1<n; ++j1)
272       for (int i1=j1;  i1<n; ++i1)
273         matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
274                               + hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
275       #endif
276       /* end hand writen partial rank-2 update */
277
278       /* The current fastest implementation: the full matrix is used, no "optimization" to use/compute
279        * only half of the matrix. Custom vectorization of the inner col -= alpha X + beta Y such that access
280        * to col are always aligned. Once we support that in Assign, then the algorithm could be rewriten as
281        * a single compact expression. This code is therefore a good benchmark when will do that. */
282
283       // let's use the end of hCoeffs to store temporary values:
284       hCoeffs.end(n-i-1) = (matA.corner(BottomRight,n-i-1,n-i-1) * (h * matA.col(i).end(n-i-1))).lazy();
285       // FIXME in the above expr a temporary is created because of the scalar multiple by h
286
287       hCoeffs.end(n-i-1) += (h * Scalar(-0.5) * matA.col(i).end(n-i-1).dot(hCoeffs.end(n-i-1)))
288                             * matA.col(i).end(n-i-1);
289
290       const Scalar* EIGEN_RESTRICT pb = &matA.coeffRef(0,i);
291       const Scalar* EIGEN_RESTRICT pa = (&hCoeffs.coeffRef(0)) - 1;
292       for (int j1=i+1; j1<n; ++j1)
293       {
294         int starti = i+1;
295         int alignedEnd = starti;
296         if (PacketSize>1)
297         {
298           int alignedStart = (starti) + ei_alignmentOffset(&matA.coeffRef(starti,j1), n-starti);
299           alignedEnd = alignedStart + ((n-alignedStart)/PacketSize)*PacketSize;
300
301           for (int i1=starti; i1<alignedStart; ++i1)
302             matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
303                                   + hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
304
305           Packet tmp0 = ei_pset1(hCoeffs.coeff(j1-1));
306           Packet tmp1 = ei_pset1(matA.coeff(j1,i));
307           Scalar* pc = &matA.coeffRef(0,j1);
308           for (int i1=alignedStart ; i1<alignedEnd; i1+=PacketSize)
309             ei_pstore(pc+i1,ei_psub(ei_pload(pc+i1),
310               ei_padd(ei_pmul(tmp0, ei_ploadu(pb+i1)),
311                       ei_pmul(tmp1, ei_ploadu(pa+i1)))));
312         }
313         for (int i1=alignedEnd; i1<n; ++i1)
314           matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
315                                 + hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
316       }
317       /* end optimized implementation */
318
319       // note: at that point matA(i+1,i+1) is the (i+1)-th element of the final diagonal
320       // note: the sequence of the beta values leads to the subdiagonal entries
321       matA.col(i).coeffRef(i+1) = beta;
322
323       hCoeffs.coeffRef(i) = h;
324     }
325   }
326   if (NumTraits<Scalar>::IsComplex)
327   {
328     // Householder transformation on the remaining single scalar
329     int i = n-2;
330     Scalar v0 = matA.col(i).coeff(i+1);
331     RealScalar beta = ei_abs(v0);
332     if (ei_real(v0)>=0.)
333       beta = -beta;
334     matA.col(i).coeffRef(i+1) = beta;
335     if(ei_isMuchSmallerThan(beta, Scalar(1))) hCoeffs.coeffRef(i) = Scalar(0);
336     else hCoeffs.coeffRef(i) = (beta - v0) / beta;
337   }
338   else
339   {
340     hCoeffs.coeffRef(n-2) = 0;
341   }
342 }
343
344 /** reconstructs and returns the matrix Q */
345 template<typename MatrixType>
346 typename Tridiagonalization<MatrixType>::MatrixType
347 Tridiagonalization<MatrixType>::matrixQ(void) const
348 {
349   int n = m_matrix.rows();
350   MatrixType matQ = MatrixType::Identity(n,n);
351   for (int i = n-2; i>=0; i--)
352   {
353     Scalar tmp = m_matrix.coeff(i+1,i);
354     m_matrix.const_cast_derived().coeffRef(i+1,i) = 1;
355
356     matQ.corner(BottomRight,n-i-1,n-i-1) -=
357       ((m_hCoeffs.coeff(i) * m_matrix.col(i).end(n-i-1)) *
358       (m_matrix.col(i).end(n-i-1).adjoint() * matQ.corner(BottomRight,n-i-1,n-i-1)).lazy()).lazy();
359
360     m_matrix.const_cast_derived().coeffRef(i+1,i) = tmp;
361   }
362   return matQ;
363 }
364
365 /** Performs a full decomposition in place */
366 template<typename MatrixType>
367 void Tridiagonalization<MatrixType>::decomposeInPlace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
368 {
369   int n = mat.rows();
370   ei_assert(mat.cols()==n && diag.size()==n && subdiag.size()==n-1);
371   if (n==3 && (!NumTraits<Scalar>::IsComplex) )
372   {
373     _decomposeInPlace3x3(mat, diag, subdiag, extractQ);
374   }
375   else
376   {
377     Tridiagonalization tridiag(mat);
378     diag = tridiag.diagonal();
379     subdiag = tridiag.subDiagonal();
380     if (extractQ)
381       mat = tridiag.matrixQ();
382   }
383 }
384
385 /** \internal
386   * Optimized path for 3x3 matrices.
387   * Especially useful for plane fitting.
388   */
389 template<typename MatrixType>
390 void Tridiagonalization<MatrixType>::_decomposeInPlace3x3(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
391 {
392   diag[0] = ei_real(mat(0,0));
393   RealScalar v1norm2 = ei_abs2(mat(0,2));
394   if (ei_isMuchSmallerThan(v1norm2, RealScalar(1)))
395   {
396     diag[1] = ei_real(mat(1,1));
397     diag[2] = ei_real(mat(2,2));
398     subdiag[0] = ei_real(mat(0,1));
399     subdiag[1] = ei_real(mat(1,2));
400     if (extractQ)
401       mat.setIdentity();
402   }
403   else
404   {
405     RealScalar beta = ei_sqrt(ei_abs2(mat(0,1))+v1norm2);
406     RealScalar invBeta = RealScalar(1)/beta;
407     Scalar m01 = mat(0,1) * invBeta;
408     Scalar m02 = mat(0,2) * invBeta;
409     Scalar q = RealScalar(2)*m01*mat(1,2) + m02*(mat(2,2) - mat(1,1));
410     diag[1] = ei_real(mat(1,1) + m02*q);
411     diag[2] = ei_real(mat(2,2) - m02*q);
412     subdiag[0] = beta;
413     subdiag[1] = ei_real(mat(1,2) - m01 * q);
414     if (extractQ)
415     {
416       mat(0,0) = 1;
417       mat(0,1) = 0;
418       mat(0,2) = 0;
419       mat(1,0) = 0;
420       mat(1,1) = m01;
421       mat(1,2) = m02;
422       mat(2,0) = 0;
423       mat(2,1) = m02;
424       mat(2,2) = -m01;
425     }
426   }
427 }
428
429 #endif // EIGEN_HIDE_HEAVY_CODE
430
431 #endif // EIGEN_TRIDIAGONALIZATION_H