From f2536416da990f12e98d01806331ad8d78545863 Mon Sep 17 00:00:00 2001
From: Benoit Jacob <jacob.benoit.1@gmail.com>
Date: Thu, 13 Aug 2009 14:56:39 -0400
Subject: [PATCH] * remove EIGEN_DONT_INLINE that harm performance for small
 sizes * normalize left Jacobi rotations to avoid having to swap rows * set
 precision to 2*machine_epsilon instead of machine_epsilon, we lose 1 bit of
 precision   but gain between 10% and 100% speed, plus reduce the risk that
 some day we hit a bad matrix   where it's impossible to approach machine
 precision

---
 Eigen/src/Core/products/RotationInThePlane.h |  4 ++--
 Eigen/src/Jacobi/Jacobi.h                    | 16 ++++++++++++++--
 Eigen/src/SVD/JacobiSquareSVD.h              | 15 ++++-----------
 3 files changed, 20 insertions(+), 15 deletions(-)

diff --git a/Eigen/src/Core/products/RotationInThePlane.h b/Eigen/src/Core/products/RotationInThePlane.h
index aa0b4d640..7fc62c675 100644
--- a/Eigen/src/Core/products/RotationInThePlane.h
+++ b/Eigen/src/Core/products/RotationInThePlane.h
@@ -50,7 +50,7 @@ void ei_apply_rotation_in_the_plane(VectorX& x, VectorY& y, typename VectorX::Sc
 template<typename Scalar>
 struct ei_apply_rotation_in_the_plane_selector<Scalar,Dynamic>
 {
-  static EIGEN_DONT_INLINE void run(Scalar* x, Scalar* y, int size, Scalar c, Scalar s, int incrx, int incry)
+  static void run(Scalar* x, Scalar* y, int size, Scalar c, Scalar s, int incrx, int incry)
   {
     for(int i=0; i<size; ++i)
     {
@@ -68,7 +68,7 @@ struct ei_apply_rotation_in_the_plane_selector<Scalar,Dynamic>
 template<typename Scalar>
 struct ei_apply_rotation_in_the_plane_selector<Scalar,1>
 {
-  static EIGEN_DONT_INLINE void run(Scalar* x, Scalar* y, int size, Scalar c, Scalar s, int, int)
+  static void run(Scalar* x, Scalar* y, int size, Scalar c, Scalar s, int, int)
   {
     typedef typename ei_packet_traits<Scalar>::type Packet;
     enum { PacketSize = ei_packet_traits<Scalar>::size, Peeling = 2 };
diff --git a/Eigen/src/Jacobi/Jacobi.h b/Eigen/src/Jacobi/Jacobi.h
index 10ae9eb8f..40181cd08 100644
--- a/Eigen/src/Jacobi/Jacobi.h
+++ b/Eigen/src/Jacobi/Jacobi.h
@@ -26,7 +26,7 @@
 #define EIGEN_JACOBI_H
 
 template<typename Derived>
-void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, Scalar c, Scalar s)
+inline void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, Scalar c, Scalar s)
 {
   RowXpr x(row(p));
   RowXpr y(row(q));
@@ -34,7 +34,7 @@ void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, Scalar c, Scalar s)
 }
 
 template<typename Derived>
-void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, Scalar c, Scalar s)
+inline void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, Scalar c, Scalar s)
 {
   ColXpr x(col(p));
   ColXpr y(col(q));
@@ -89,5 +89,17 @@ inline bool MatrixBase<Derived>::makeJacobiForAAt(int p, int q, Scalar *c, Scala
                        c,s);
 }
 
+template<typename Scalar>
+inline void ei_normalizeJacobi(Scalar *c, Scalar *s, const Scalar& x, const Scalar& y)
+{
+  Scalar a = x * *c - y * *s;
+  Scalar b = x * *s + y * *c;
+  if(ei_abs(b)>ei_abs(a)) {
+    Scalar x = *c;
+    *c = -*s;
+    *s = x;
+  }
+}
+
 
 #endif // EIGEN_JACOBI_H
diff --git a/Eigen/src/SVD/JacobiSquareSVD.h b/Eigen/src/SVD/JacobiSquareSVD.h
index 18c3db980..82133f7be 100644
--- a/Eigen/src/SVD/JacobiSquareSVD.h
+++ b/Eigen/src/SVD/JacobiSquareSVD.h
@@ -102,6 +102,7 @@ void JacobiSquareSVD<MatrixType, ComputeU, ComputeV>::compute(const MatrixType&
   if(ComputeU) m_matrixU = MatrixUType::Identity(size,size);
   if(ComputeV) m_matrixV = MatrixUType::Identity(size,size);
   m_singularValues.resize(size);
+  const RealScalar precision = 2 * machine_epsilon<Scalar>();
 
 sweep_again:
   for(int p = 1; p < size; ++p)
@@ -110,7 +111,7 @@ sweep_again:
     {
       Scalar c, s;
       while(std::max(ei_abs(work_matrix.coeff(p,q)),ei_abs(work_matrix.coeff(q,p)))
-          > std::max(ei_abs(work_matrix.coeff(p,p)),ei_abs(work_matrix.coeff(q,q)))*machine_epsilon<Scalar>())
+          > std::max(ei_abs(work_matrix.coeff(p,p)),ei_abs(work_matrix.coeff(q,q)))*precision)
       {  
         if(work_matrix.makeJacobiForAtA(p,q,&c,&s))
         {
@@ -119,24 +120,16 @@ sweep_again:
         }
         if(work_matrix.makeJacobiForAAt(p,q,&c,&s))
         {
-          Scalar x = ei_abs2(work_matrix.coeff(p,p)) + ei_abs2(work_matrix.coeff(p,q));
-          Scalar y = ei_conj(work_matrix.coeff(q,p))*work_matrix.coeff(p,p) + ei_conj(work_matrix.coeff(q,q))*work_matrix.coeff(p,q);
-          Scalar z = ei_abs2(work_matrix.coeff(q,p)) + ei_abs2(work_matrix.coeff(q,q));
+          ei_normalizeJacobi(&c, &s, work_matrix.coeff(p,p), work_matrix.coeff(q,p)),
           work_matrix.applyJacobiOnTheLeft(p,q,c,s);
           if(ComputeU) m_matrixU.applyJacobiOnTheRight(p,q,c,s);
-          if(std::max(ei_abs(work_matrix.coeff(p,q)),ei_abs(work_matrix.coeff(q,p)))
-           > std::max(ei_abs(work_matrix.coeff(p,p)),ei_abs(work_matrix.coeff(q,q))) )
-          {
-            work_matrix.row(p).swap(work_matrix.row(q));
-            if(ComputeU) m_matrixU.col(p).swap(m_matrixU.col(q));
-          }
         }
       }
     }
   }
   
   RealScalar biggestOnDiag = work_matrix.diagonal().cwise().abs().maxCoeff();
-  RealScalar maxAllowedOffDiag = biggestOnDiag * machine_epsilon<Scalar>();
+  RealScalar maxAllowedOffDiag = biggestOnDiag * precision;
   for(int p = 0; p < size; ++p)
   {
     for(int q = 0; q < p; ++q)