Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / extern / Eigen2 / Eigen / src / SVD / SVD.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_SVD_H
26 #define EIGEN_SVD_H
27
28 /** \ingroup SVD_Module
29   * \nonstableyet
30   *
31   * \class SVD
32   *
33   * \brief Standard SVD decomposition of a matrix and associated features
34   *
35   * \param MatrixType the type of the matrix of which we are computing the SVD decomposition
36   *
37   * This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N
38   * with \c M \>= \c N.
39   *
40   *
41   * \sa MatrixBase::SVD()
42   */
43 template<typename MatrixType> class SVD
44 {
45   private:
46     typedef typename MatrixType::Scalar Scalar;
47     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
48
49     enum {
50       PacketSize = ei_packet_traits<Scalar>::size,
51       AlignmentMask = int(PacketSize)-1,
52       MinSize = EIGEN_ENUM_MIN(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)
53     };
54
55     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector;
56     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector;
57
58     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType;
59     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType;
60     typedef Matrix<Scalar, MinSize, 1> SingularValuesType;
61
62   public:
63
64     SVD(const MatrixType& matrix)
65       : m_matU(matrix.rows(), std::min(matrix.rows(), matrix.cols())),
66         m_matV(matrix.cols(),matrix.cols()),
67         m_sigma(std::min(matrix.rows(),matrix.cols()))
68     {
69       compute(matrix);
70     }
71
72     template<typename OtherDerived, typename ResultType>
73     bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const;
74
75     const MatrixUType& matrixU() const { return m_matU; }
76     const SingularValuesType& singularValues() const { return m_sigma; }
77     const MatrixVType& matrixV() const { return m_matV; }
78
79     void compute(const MatrixType& matrix);
80     SVD& sort();
81
82     template<typename UnitaryType, typename PositiveType>
83     void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const;
84     template<typename PositiveType, typename UnitaryType>
85     void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const;
86     template<typename RotationType, typename ScalingType>
87     void computeRotationScaling(RotationType *unitary, ScalingType *positive) const;
88     template<typename ScalingType, typename RotationType>
89     void computeScalingRotation(ScalingType *positive, RotationType *unitary) const;
90
91   protected:
92     /** \internal */
93     MatrixUType m_matU;
94     /** \internal */
95     MatrixVType m_matV;
96     /** \internal */
97     SingularValuesType m_sigma;
98 };
99
100 /** Computes / recomputes the SVD decomposition A = U S V^* of \a matrix
101   *
102   * \note this code has been adapted from JAMA (public domain)
103   */
104 template<typename MatrixType>
105 void SVD<MatrixType>::compute(const MatrixType& matrix)
106 {
107   const int m = matrix.rows();
108   const int n = matrix.cols();
109   const int nu = std::min(m,n);
110
111   m_matU.resize(m, nu);
112   m_matU.setZero();
113   m_sigma.resize(std::min(m,n));
114   m_matV.resize(n,n);
115
116   RowVector e(n);
117   ColVector work(m);
118   MatrixType matA(matrix);
119   const bool wantu = true;
120   const bool wantv = true;
121   int i=0, j=0, k=0;
122
123   // Reduce A to bidiagonal form, storing the diagonal elements
124   // in s and the super-diagonal elements in e.
125   int nct = std::min(m-1,n);
126   int nrt = std::max(0,std::min(n-2,m));
127   for (k = 0; k < std::max(nct,nrt); ++k)
128   {
129     if (k < nct)
130     {
131       // Compute the transformation for the k-th column and
132       // place the k-th diagonal in m_sigma[k].
133       m_sigma[k] = matA.col(k).end(m-k).norm();
134       if (m_sigma[k] != 0.0) // FIXME
135       {
136         if (matA(k,k) < 0.0)
137           m_sigma[k] = -m_sigma[k];
138         matA.col(k).end(m-k) /= m_sigma[k];
139         matA(k,k) += 1.0;
140       }
141       m_sigma[k] = -m_sigma[k];
142     }
143
144     for (j = k+1; j < n; ++j)
145     {
146       if ((k < nct) && (m_sigma[k] != 0.0))
147       {
148         // Apply the transformation.
149         Scalar t = matA.col(k).end(m-k).dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ??
150         t = -t/matA(k,k);
151         matA.col(j).end(m-k) += t * matA.col(k).end(m-k);
152       }
153
154       // Place the k-th row of A into e for the
155       // subsequent calculation of the row transformation.
156       e[j] = matA(k,j);
157     }
158
159     // Place the transformation in U for subsequent back multiplication.
160     if (wantu & (k < nct))
161       m_matU.col(k).end(m-k) = matA.col(k).end(m-k);
162
163     if (k < nrt)
164     {
165       // Compute the k-th row transformation and place the
166       // k-th super-diagonal in e[k].
167       e[k] = e.end(n-k-1).norm();
168       if (e[k] != 0.0)
169       {
170           if (e[k+1] < 0.0)
171             e[k] = -e[k];
172           e.end(n-k-1) /= e[k];
173           e[k+1] += 1.0;
174       }
175       e[k] = -e[k];
176       if ((k+1 < m) & (e[k] != 0.0))
177       {
178         // Apply the transformation.
179         work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1);
180         for (j = k+1; j < n; ++j)
181           matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1);
182       }
183
184       // Place the transformation in V for subsequent back multiplication.
185       if (wantv)
186         m_matV.col(k).end(n-k-1) = e.end(n-k-1);
187     }
188   }
189
190
191   // Set up the final bidiagonal matrix or order p.
192   int p = std::min(n,m+1);
193   if (nct < n)
194     m_sigma[nct] = matA(nct,nct);
195   if (m < p)
196     m_sigma[p-1] = 0.0;
197   if (nrt+1 < p)
198     e[nrt] = matA(nrt,p-1);
199   e[p-1] = 0.0;
200
201   // If required, generate U.
202   if (wantu)
203   {
204     for (j = nct; j < nu; ++j)
205     {
206       m_matU.col(j).setZero();
207       m_matU(j,j) = 1.0;
208     }
209     for (k = nct-1; k >= 0; k--)
210     {
211       if (m_sigma[k] != 0.0)
212       {
213         for (j = k+1; j < nu; ++j)
214         {
215           Scalar t = m_matU.col(k).end(m-k).dot(m_matU.col(j).end(m-k)); // FIXME is it really a dot product we want ?
216           t = -t/m_matU(k,k);
217           m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k);
218         }
219         m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k);
220         m_matU(k,k) = Scalar(1) + m_matU(k,k);
221         if (k-1>0)
222           m_matU.col(k).start(k-1).setZero();
223       }
224       else
225       {
226         m_matU.col(k).setZero();
227         m_matU(k,k) = 1.0;
228       }
229     }
230   }
231
232   // If required, generate V.
233   if (wantv)
234   {
235     for (k = n-1; k >= 0; k--)
236     {
237       if ((k < nrt) & (e[k] != 0.0))
238       {
239         for (j = k+1; j < nu; ++j)
240         {
241           Scalar t = m_matV.col(k).end(n-k-1).dot(m_matV.col(j).end(n-k-1)); // FIXME is it really a dot product we want ?
242           t = -t/m_matV(k+1,k);
243           m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1);
244         }
245       }
246       m_matV.col(k).setZero();
247       m_matV(k,k) = 1.0;
248     }
249   }
250
251   // Main iteration loop for the singular values.
252   int pp = p-1;
253   int iter = 0;
254   Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
255   while (p > 0)
256   {
257     int k=0;
258     int kase=0;
259
260     // Here is where a test for too many iterations would go.
261
262     // This section of the program inspects for
263     // negligible elements in the s and e arrays.  On
264     // completion the variables kase and k are set as follows.
265
266     // kase = 1     if s(p) and e[k-1] are negligible and k<p
267     // kase = 2     if s(k) is negligible and k<p
268     // kase = 3     if e[k-1] is negligible, k<p, and
269     //              s(k), ..., s(p) are not negligible (qr step).
270     // kase = 4     if e(p-1) is negligible (convergence).
271
272     for (k = p-2; k >= -1; --k)
273     {
274       if (k == -1)
275           break;
276       if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1])))
277       {
278           e[k] = 0.0;
279           break;
280       }
281     }
282     if (k == p-2)
283     {
284       kase = 4;
285     }
286     else
287     {
288       int ks;
289       for (ks = p-1; ks >= k; --ks)
290       {
291         if (ks == k)
292           break;
293         Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0));
294         if (ei_abs(m_sigma[ks]) <= eps*t)
295         {
296           m_sigma[ks] = 0.0;
297           break;
298         }
299       }
300       if (ks == k)
301       {
302         kase = 3;
303       }
304       else if (ks == p-1)
305       {
306         kase = 1;
307       }
308       else
309       {
310         kase = 2;
311         k = ks;
312       }
313     }
314     ++k;
315
316     // Perform the task indicated by kase.
317     switch (kase)
318     {
319
320       // Deflate negligible s(p).
321       case 1:
322       {
323         Scalar f(e[p-2]);
324         e[p-2] = 0.0;
325         for (j = p-2; j >= k; --j)
326         {
327           Scalar t(ei_hypot(m_sigma[j],f));
328           Scalar cs(m_sigma[j]/t);
329           Scalar sn(f/t);
330           m_sigma[j] = t;
331           if (j != k)
332           {
333             f = -sn*e[j-1];
334             e[j-1] = cs*e[j-1];
335           }
336           if (wantv)
337           {
338             for (i = 0; i < n; ++i)
339             {
340               t = cs*m_matV(i,j) + sn*m_matV(i,p-1);
341               m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1);
342               m_matV(i,j) = t;
343             }
344           }
345         }
346       }
347       break;
348
349       // Split at negligible s(k).
350       case 2:
351       {
352         Scalar f(e[k-1]);
353         e[k-1] = 0.0;
354         for (j = k; j < p; ++j)
355         {
356           Scalar t(ei_hypot(m_sigma[j],f));
357           Scalar cs( m_sigma[j]/t);
358           Scalar sn(f/t);
359           m_sigma[j] = t;
360           f = -sn*e[j];
361           e[j] = cs*e[j];
362           if (wantu)
363           {
364             for (i = 0; i < m; ++i)
365             {
366               t = cs*m_matU(i,j) + sn*m_matU(i,k-1);
367               m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1);
368               m_matU(i,j) = t;
369             }
370           }
371         }
372       }
373       break;
374
375       // Perform one qr step.
376       case 3:
377       {
378         // Calculate the shift.
379         Scalar scale = std::max(std::max(std::max(std::max(
380                         ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])),
381                         ei_abs(m_sigma[k])),ei_abs(e[k]));
382         Scalar sp = m_sigma[p-1]/scale;
383         Scalar spm1 = m_sigma[p-2]/scale;
384         Scalar epm1 = e[p-2]/scale;
385         Scalar sk = m_sigma[k]/scale;
386         Scalar ek = e[k]/scale;
387         Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2);
388         Scalar c = (sp*epm1)*(sp*epm1);
389         Scalar shift = 0.0;
390         if ((b != 0.0) || (c != 0.0))
391         {
392           shift = ei_sqrt(b*b + c);
393           if (b < 0.0)
394             shift = -shift;
395           shift = c/(b + shift);
396         }
397         Scalar f = (sk + sp)*(sk - sp) + shift;
398         Scalar g = sk*ek;
399
400         // Chase zeros.
401
402         for (j = k; j < p-1; ++j)
403         {
404           Scalar t = ei_hypot(f,g);
405           Scalar cs = f/t;
406           Scalar sn = g/t;
407           if (j != k)
408             e[j-1] = t;
409           f = cs*m_sigma[j] + sn*e[j];
410           e[j] = cs*e[j] - sn*m_sigma[j];
411           g = sn*m_sigma[j+1];
412           m_sigma[j+1] = cs*m_sigma[j+1];
413           if (wantv)
414           {
415             for (i = 0; i < n; ++i)
416             {
417               t = cs*m_matV(i,j) + sn*m_matV(i,j+1);
418               m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1);
419               m_matV(i,j) = t;
420             }
421           }
422           t = ei_hypot(f,g);
423           cs = f/t;
424           sn = g/t;
425           m_sigma[j] = t;
426           f = cs*e[j] + sn*m_sigma[j+1];
427           m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1];
428           g = sn*e[j+1];
429           e[j+1] = cs*e[j+1];
430           if (wantu && (j < m-1))
431           {
432             for (i = 0; i < m; ++i)
433             {
434               t = cs*m_matU(i,j) + sn*m_matU(i,j+1);
435               m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1);
436               m_matU(i,j) = t;
437             }
438           }
439         }
440         e[p-2] = f;
441         iter = iter + 1;
442       }
443       break;
444
445       // Convergence.
446       case 4:
447       {
448         // Make the singular values positive.
449         if (m_sigma[k] <= 0.0)
450         {
451           m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0);
452           if (wantv)
453             m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1);
454         }
455
456         // Order the singular values.
457         while (k < pp)
458         {
459           if (m_sigma[k] >= m_sigma[k+1])
460             break;
461           Scalar t = m_sigma[k];
462           m_sigma[k] = m_sigma[k+1];
463           m_sigma[k+1] = t;
464           if (wantv && (k < n-1))
465             m_matV.col(k).swap(m_matV.col(k+1));
466           if (wantu && (k < m-1))
467             m_matU.col(k).swap(m_matU.col(k+1));
468           ++k;
469         }
470         iter = 0;
471         p--;
472       }
473       break;
474     } // end big switch
475   } // end iterations
476 }
477
478 template<typename MatrixType>
479 SVD<MatrixType>& SVD<MatrixType>::sort()
480 {
481   int mu = m_matU.rows();
482   int mv = m_matV.rows();
483   int n  = m_matU.cols();
484
485   for (int i=0; i<n; ++i)
486   {
487     int  k = i;
488     Scalar p = m_sigma.coeff(i);
489
490     for (int j=i+1; j<n; ++j)
491     {
492       if (m_sigma.coeff(j) > p)
493       {
494         k = j;
495         p = m_sigma.coeff(j);
496       }
497     }
498     if (k != i)
499     {
500       m_sigma.coeffRef(k) = m_sigma.coeff(i);  // i.e.
501       m_sigma.coeffRef(i) = p;                 // swaps the i-th and the k-th elements
502
503       int j = mu;
504       for(int s=0; j!=0; ++s, --j)
505         std::swap(m_matU.coeffRef(s,i), m_matU.coeffRef(s,k));
506
507       j = mv;
508       for (int s=0; j!=0; ++s, --j)
509         std::swap(m_matV.coeffRef(s,i), m_matV.coeffRef(s,k));
510     }
511   }
512   return *this;
513 }
514
515 /** \returns the solution of \f$ A x = b \f$ using the current SVD decomposition of A.
516   * The parts of the solution corresponding to zero singular values are ignored.
517   *
518   * \sa MatrixBase::svd(), LU::solve(), LLT::solve()
519   */
520 template<typename MatrixType>
521 template<typename OtherDerived, typename ResultType>
522 bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
523 {
524   const int rows = m_matU.rows();
525   ei_assert(b.rows() == rows);
526
527   Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
528   for (int j=0; j<b.cols(); ++j)
529   {
530     Matrix<Scalar,MatrixUType::RowsAtCompileTime,1> aux = m_matU.transpose() * b.col(j);
531
532     for (int i = 0; i <m_matU.cols(); ++i)
533     {
534       Scalar si = m_sigma.coeff(i);
535       if (ei_isMuchSmallerThan(ei_abs(si),maxVal))
536         aux.coeffRef(i) = 0;
537       else
538         aux.coeffRef(i) /= si;
539     }
540
541     result->col(j) = m_matV * aux;
542   }
543   return true;
544 }
545
546 /** Computes the polar decomposition of the matrix, as a product unitary x positive.
547   *
548   * If either pointer is zero, the corresponding computation is skipped.
549   *
550   * Only for square matrices.
551   *
552   * \sa computePositiveUnitary(), computeRotationScaling()
553   */
554 template<typename MatrixType>
555 template<typename UnitaryType, typename PositiveType>
556 void SVD<MatrixType>::computeUnitaryPositive(UnitaryType *unitary,
557                                              PositiveType *positive) const
558 {
559   ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices");
560   if(unitary) *unitary = m_matU * m_matV.adjoint();
561   if(positive) *positive = m_matV * m_sigma.asDiagonal() * m_matV.adjoint();
562 }
563
564 /** Computes the polar decomposition of the matrix, as a product positive x unitary.
565   *
566   * If either pointer is zero, the corresponding computation is skipped.
567   *
568   * Only for square matrices.
569   *
570   * \sa computeUnitaryPositive(), computeRotationScaling()
571   */
572 template<typename MatrixType>
573 template<typename UnitaryType, typename PositiveType>
574 void SVD<MatrixType>::computePositiveUnitary(UnitaryType *positive,
575                                              PositiveType *unitary) const
576 {
577   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
578   if(unitary) *unitary = m_matU * m_matV.adjoint();
579   if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint();
580 }
581
582 /** decomposes the matrix as a product rotation x scaling, the scaling being
583   * not necessarily positive.
584   *
585   * If either pointer is zero, the corresponding computation is skipped.
586   *
587   * This method requires the Geometry module.
588   *
589   * \sa computeScalingRotation(), computeUnitaryPositive()
590   */
591 template<typename MatrixType>
592 template<typename RotationType, typename ScalingType>
593 void SVD<MatrixType>::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const
594 {
595   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
596   Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
597   Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
598   sv.coeffRef(0) *= x;
599   if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint());
600   if(rotation)
601   {
602     MatrixType m(m_matU);
603     m.col(0) /= x;
604     rotation->lazyAssign(m * m_matV.adjoint());
605   }
606 }
607
608 /** decomposes the matrix as a product scaling x rotation, the scaling being
609   * not necessarily positive.
610   *
611   * If either pointer is zero, the corresponding computation is skipped.
612   *
613   * This method requires the Geometry module.
614   *
615   * \sa computeRotationScaling(), computeUnitaryPositive()
616   */
617 template<typename MatrixType>
618 template<typename ScalingType, typename RotationType>
619 void SVD<MatrixType>::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const
620 {
621   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
622   Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
623   Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
624   sv.coeffRef(0) *= x;
625   if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint());
626   if(rotation)
627   {
628     MatrixType m(m_matU);
629     m.col(0) /= x;
630     rotation->lazyAssign(m * m_matV.adjoint());
631   }
632 }
633
634
635 /** \svd_module
636   * \returns the SVD decomposition of \c *this
637   */
638 template<typename Derived>
639 inline SVD<typename MatrixBase<Derived>::PlainMatrixType>
640 MatrixBase<Derived>::svd() const
641 {
642   return SVD<PlainMatrixType>(derived());
643 }
644
645 #endif // EIGEN_SVD_H