* add Jacobi transformations

* add Jacobi (Hestenes) SVD decomposition for square matrices
* add function for trivial Householder

6 files changed, 303 insertions, 0 deletions

diff --git a/Eigen/Jacobi b/Eigen/Jacobi
new file mode 100644
index 000000000..33a6b757e
--- /dev/null
+++ b/Eigen/Jacobi
@@ -0,0 +1,24 @@
+#ifndef EIGEN_JACOBI_MODULE_H
+#define EIGEN_JACOBI_MODULE_H
+
+#include "Core"
+
+#include "src/Core/util/DisableMSVCWarnings.h"
+
+namespace Eigen {
+
+/** \defgroup Jacobi_Module Jacobi module
+  * This module provides Jacobi rotations.
+  *
+  * \code
+  * #include <Eigen/Jacobi>
+  * \endcode
+  */
+
+#include "src/Jacobi/Jacobi.h"
+
+} // namespace Eigen
+
+#include "src/Core/util/EnableMSVCWarnings.h"
+
+#endif // EIGEN_JACOBI_MODULE_H
diff --git a/Eigen/SVD b/Eigen/SVD
index eef05564b..3aab35118 100644
--- a/Eigen/SVD
+++ b/Eigen/SVD
@@ -2,6 +2,8 @@
 #define EIGEN_SVD_MODULE_H
 
 #include "Core"
+#include "Householder"
+#include "Jacobi"
 
 #include "src/Core/util/DisableMSVCWarnings.h"
 
@@ -20,7 +22,9 @@ namespace Eigen {
   * \endcode
   */
 
+#include "src/SVD/Bidiagonalization.h"
 #include "src/SVD/SVD.h"
+#include "src/SVD/JacobiSquareSVD.h"
 
 } // namespace Eigen
 
diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h
index 6ec7ddbb7..688b7c4d0 100644
--- a/Eigen/src/Core/MatrixBase.h
+++ b/Eigen/src/Core/MatrixBase.h
@@ -778,6 +778,12 @@ template<typename Derived> class MatrixBase
     void applyHouseholderOnTheRight(const EssentialPart& essential,
                                     const RealScalar& beta);
 
+///////// Jacobi module /////////
+
+    void applyJacobiOnTheLeft(int p, int q, Scalar c, Scalar s);
+    void applyJacobiOnTheRight(int p, int q, Scalar c, Scalar s);
+    bool makeJacobi(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s);
+    bool makeJacobiForAtA(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s);
 
     #ifdef EIGEN_MATRIXBASE_PLUGIN
     #include EIGEN_MATRIXBASE_PLUGIN
diff --git a/Eigen/src/Householder/Householder.h b/Eigen/src/Householder/Householder.h
index 8972806de..2a4b1395c 100644
--- a/Eigen/src/Householder/Householder.h
+++ b/Eigen/src/Householder/Householder.h
@@ -32,6 +32,15 @@ template<int n> struct ei_decrement_size
   };
 };
 
+template<typename EssentialPart>
+void makeTrivialHouseholder(
+  EssentialPart *essential,
+  typename EssentialPart::RealScalar *beta)
+{
+  *beta = typename EssentialPart::RealScalar(0);
+  essential->setZero();
+}
+
 template<typename Derived>
 template<typename EssentialPart>
 void MatrixBase<Derived>::makeHouseholder(
diff --git a/Eigen/src/Jacobi/Jacobi.h b/Eigen/src/Jacobi/Jacobi.h
new file mode 100644
index 000000000..993a723ab
--- /dev/null
+++ b/Eigen/src/Jacobi/Jacobi.h
@@ -0,0 +1,91 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_JACOBI_H
+#define EIGEN_JACOBI_H
+
+template<typename Derived>
+void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, Scalar c, Scalar s)
+{
+  for(int i = 0; i < cols(); ++i)
+  {
+    Scalar tmp = coeff(p,i);
+    coeffRef(p,i) = c * tmp - s * coeff(q,i);
+    coeffRef(q,i) = s * tmp + c * coeff(q,i);
+  }
+}
+
+template<typename Derived>
+void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, Scalar c, Scalar s)
+{
+  for(int i = 0; i < rows(); ++i)
+  {
+    Scalar tmp = coeff(i,p);
+    coeffRef(i,p) = c * tmp - s * coeff(i,q);
+    coeffRef(i,q) = s * tmp + c * coeff(i,q);
+  }
+}
+
+template<typename Scalar>
+bool ei_makeJacobi(Scalar x, Scalar y, Scalar z, Scalar max_coeff, Scalar *c, Scalar *s)
+{
+  if(ei_abs(y) < max_coeff * 0.5 * machine_epsilon<Scalar>())
+  {
+    *c = Scalar(1);
+    *s = Scalar(0);
+    return true;
+  }
+  else
+  {
+    Scalar tau = (z - x) / (2 * y);
+    Scalar w = ei_sqrt(1 + ei_abs2(tau));
+    Scalar t;
+    if(tau>0)
+      t = Scalar(1) / (tau + w);
+    else
+      t = Scalar(1) / (tau - w);
+    *c = Scalar(1) / ei_sqrt(1 + ei_abs2(t));
+    *s = *c * t;
+    return false;
+  }
+}
+
+template<typename Derived>
+inline bool MatrixBase<Derived>::makeJacobi(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s)
+{
+  return ei_makeJacobi(coeff(p,p), coeff(p,q), coeff(q,q), max_coeff, c, s);
+}
+
+template<typename Derived>
+inline bool MatrixBase<Derived>::makeJacobiForAtA(int p, int q, Scalar max_coeff, Scalar *c, Scalar *s)
+{
+  return ei_makeJacobi(col(p).squaredNorm(),
+                       col(p).dot(col(q)),
+                       col(q).squaredNorm(),
+                       max_coeff,
+                       c,s);
+}
+
+
+#endif // EIGEN_JACOBI_H
diff --git a/Eigen/src/SVD/JacobiSquareSVD.h b/Eigen/src/SVD/JacobiSquareSVD.h
new file mode 100644
index 000000000..e9ecc89ac
--- /dev/null
+++ b/Eigen/src/SVD/JacobiSquareSVD.h
@@ -0,0 +1,169 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef EIGEN_JACOBISQUARESVD_H
+#define EIGEN_JACOBISQUARESVD_H
+
+/** \ingroup SVD_Module
+  * \nonstableyet
+  *
+  * \class JacobiSquareSVD
+  *
+  * \brief Jacobi SVD decomposition of a square matrix
+  *
+  * \param MatrixType the type of the matrix of which we are computing the SVD decomposition
+  * \param ComputeU whether the U matrix should be computed
+  * \param ComputeV whether the V matrix should be computed
+  *
+  * \sa MatrixBase::jacobiSvd()
+  */
+template<typename MatrixType, bool ComputeU, bool ComputeV> class JacobiSquareSVD
+{
+  private:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+    enum {
+      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
+      Options = MatrixType::Options
+    };
+    
+    typedef Matrix<Scalar, Dynamic, Dynamic, Options> DummyMatrixType;
+    typedef typename ei_meta_if<ComputeU,
+                                Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime,
+                                       Options, MaxRowsAtCompileTime, MaxRowsAtCompileTime>,
+                                DummyMatrixType>::ret MatrixUType;
+    typedef typename Diagonal<MatrixType,0>::PlainMatrixType SingularValuesType;
+    typedef Matrix<Scalar, 1, RowsAtCompileTime, Options, 1, MaxRowsAtCompileTime> RowType;
+    typedef Matrix<Scalar, RowsAtCompileTime, 1, Options, MaxRowsAtCompileTime, 1> ColType;
+
+  public:
+
+    JacobiSquareSVD() : m_isInitialized(false) {}
+
+    JacobiSquareSVD(const MatrixType& matrix)
+    {
+      compute(matrix);
+    }
+    
+    void compute(const MatrixType& matrix);
+    
+    const MatrixUType& matrixU() const
+    {
+      ei_assert(m_isInitialized && "SVD is not initialized.");
+      return m_matrixU;
+    }
+
+    const SingularValuesType& singularValues() const
+    {
+      ei_assert(m_isInitialized && "SVD is not initialized.");
+      return m_singularValues;
+    }
+
+    const MatrixUType& matrixV() const
+    {
+      ei_assert(m_isInitialized && "SVD is not initialized.");
+      return m_matrixV;
+    }
+
+  protected:
+    MatrixUType m_matrixU;
+    MatrixUType m_matrixV;
+    SingularValuesType m_singularValues;
+    bool m_isInitialized;
+};
+
+template<typename MatrixType, bool ComputeU, bool ComputeV>
+void JacobiSquareSVD<MatrixType, ComputeU, ComputeV>::compute(const MatrixType& matrix)
+{
+  MatrixType work_matrix(matrix);
+  int size = matrix.rows();
+  if(ComputeU) m_matrixU = MatrixUType::Identity(size,size);
+  if(ComputeV) m_matrixV = MatrixUType::Identity(size,size);
+  m_singularValues.resize(size);
+  RealScalar max_coeff = work_matrix.cwise().abs().maxCoeff();
+  for(int k = 1; k < 40; ++k) {
+    bool finished = true;
+    for(int p = 1; p < size; ++p)
+    {
+      for(int q = 0; q < p; ++q)
+      {
+        Scalar c, s;
+        finished &= work_matrix.makeJacobiForAtA(p,q,max_coeff,&c,&s);
+        work_matrix.applyJacobiOnTheRight(p,q,c,s);
+        if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,c,s);
+      }
+    }
+    if(finished) break;
+  }
+  
+  for(int i = 0; i < size; ++i)
+  {
+    m_singularValues.coeffRef(i) = work_matrix.col(i).norm();
+  }
+
+  int first_zero = size;
+  RealScalar biggest = m_singularValues.maxCoeff();
+  for(int i = 0; i < size; i++)
+  {
+    int pos;
+    RealScalar biggest_remaining = m_singularValues.end(size-i).maxCoeff(&pos);
+    if(first_zero == size && ei_isMuchSmallerThan(biggest_remaining, biggest)) first_zero = pos + i;
+    if(pos)
+    {
+      pos += i;
+      std::swap(m_singularValues.coeffRef(i), m_singularValues.coeffRef(pos));
+      if(ComputeU) work_matrix.col(pos).swap(work_matrix.col(i));
+      if(ComputeV) m_matrixV.col(pos).swap(m_matrixV.col(i));
+    }
+  }
+  
+  if(ComputeU)
+  {
+    for(int i = 0; i < first_zero; ++i)
+    {
+      m_matrixU.col(i) = work_matrix.col(i) / m_singularValues.coeff(i);
+    }
+    if(first_zero < size)
+    {
+      for(int i = first_zero; i < size; ++i)
+      {
+        for(int j = 0; j < size; ++j)
+        {
+          m_matrixU.col(i).setZero();
+          m_matrixU.coeffRef(j,i) = Scalar(1);
+          for(int k = 0; k < first_zero; ++k)
+            m_matrixU.col(i) -= m_matrixU.col(i).dot(m_matrixU.col(k)) * m_matrixU.col(k);
+          RealScalar n = m_matrixU.col(i).norm();
+          if(!ei_isMuchSmallerThan(n, biggest))
+          {
+            m_matrixU.col(i) /= n;
+            break;
+          }
+        }
+      }
+    }     
+  }
+}
+#endif // EIGEN_JACOBISQUARESVD_H