quick reimplementation of SVD from the numeral recipes book:

this is still not Eigen style code but at least it works for
n>m and it is more accurate than the JAMA based version. (I needed
it now, this is why I did that)

2 files changed, 232 insertions, 328 deletions

diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index a76ccbdaf..8d1bbf9d2 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -371,13 +371,14 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBas
   if (ei_isApprox(c,Scalar(-1)))
   {
     c = std::max<Scalar>(c,-1);
-
-    SVD<Matrix<Scalar,3,3> > svd(v0 * v0.transpose() + v1 * v1.transpose());
+    Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
+    SVD<Matrix<Scalar,2,3> > svd(m);
     Vector3 axis = svd.matrixV().col(2);
     
     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
     this->w() = ei_sqrt(w2);
     this->vec() = axis * ei_sqrt(Scalar(1) - w2);
+    
     return *this;
   }
 
diff --git a/Eigen/src/SVD/SVD.h b/Eigen/src/SVD/SVD.h
index 45a7fbfa5..71f6763a8 100644
--- a/Eigen/src/SVD/SVD.h
+++ b/Eigen/src/SVD/SVD.h
@@ -34,9 +34,7 @@
   *
   * \param MatrixType the type of the matrix of which we are computing the SVD decomposition
   *
-  * This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N
-  * with \c M \>= \c N.
-  *
+  * This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N.
   *
   * \sa MatrixBase::SVD()
   */
@@ -55,13 +53,13 @@ template<typename MatrixType> class SVD
     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector;
     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector;
 
-    typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType;
+    typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> MatrixUType;
     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType;
-    typedef Matrix<Scalar, MinSize, 1> SingularValuesType;
+    typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> SingularValuesType;
 
   public:
 
-    /** 
+    /**
     * \brief Default Constructor.
     *
     * The default constructor is useful in cases in which the user intends to
@@ -70,9 +68,9 @@ template<typename MatrixType> class SVD
     SVD() : m_matU(), m_matV(), m_sigma(), m_isInitialized(false) {}
 
     SVD(const MatrixType& matrix)
-      : m_matU(matrix.rows(), std::min(matrix.rows(), matrix.cols())),
+      : m_matU(matrix.rows(), matrix.rows()),
         m_matV(matrix.cols(),matrix.cols()),
-        m_sigma(std::min(matrix.rows(),matrix.cols())),
+        m_sigma(matrix.cols()),
         m_isInitialized(false)
     {
       compute(matrix);
@@ -81,22 +79,22 @@ template<typename MatrixType> class SVD
     template<typename OtherDerived, typename ResultType>
     bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const;
 
-    const MatrixUType& matrixU() const 
-    { 
+    const MatrixUType& matrixU() const
+    {
       ei_assert(m_isInitialized && "SVD is not initialized.");
-      return m_matU; 
+      return m_matU;
     }
 
-    const SingularValuesType& singularValues() const 
+    const SingularValuesType& singularValues() const
     {
       ei_assert(m_isInitialized && "SVD is not initialized.");
-      return m_sigma; 
+      return m_sigma;
     }
 
-    const MatrixVType& matrixV() const 
+    const MatrixVType& matrixV() const
     {
       ei_assert(m_isInitialized && "SVD is not initialized.");
-      return m_matV; 
+      return m_matV;
     }
 
     void compute(const MatrixType& matrix);
@@ -112,6 +110,23 @@ template<typename MatrixType> class SVD
     void computeScalingRotation(ScalingType *positive, RotationType *unitary) const;
 
   protected:
+    // Computes (a^2 + b^2)^(1/2) without destructive underflow or overflow.
+    inline static Scalar pythagora(Scalar a, Scalar b)
+    {
+      Scalar abs_a = ei_abs(a);
+      Scalar abs_b = ei_abs(b);
+      if (abs_a > abs_b)
+        return abs_a*ei_sqrt(Scalar(1.0)+ei_abs2(abs_b/abs_a));
+      else
+        return (abs_b == Scalar(0.0) ? Scalar(0.0) : abs_b*ei_sqrt(Scalar(1.0)+ei_abs2(abs_a/abs_b)));
+    }
+
+    inline static Scalar sign(Scalar a, Scalar b)
+    {
+      return (b >= Scalar(0.0) ? ei_abs(a) : -ei_abs(a));
+    }
+    
+  protected:
     /** \internal */
     MatrixUType m_matU;
     /** \internal */
@@ -123,7 +138,7 @@ template<typename MatrixType> class SVD
 
 /** Computes / recomputes the SVD decomposition A = U S V^* of \a matrix
   *
-  * \note this code has been adapted from JAMA (public domain)
+  * \note this code has been adapted from Numerical Recipes, second edition.
   */
 template<typename MatrixType>
 void SVD<MatrixType>::compute(const MatrixType& matrix)
@@ -132,371 +147,259 @@ void SVD<MatrixType>::compute(const MatrixType& matrix)
   const int n = matrix.cols();
   const int nu = std::min(m,n);
 
-  m_matU.resize(m, nu);
+  m_matU.resize(m, m);
   m_matU.setZero();
-  m_sigma.resize(std::min(m,n));
+  m_sigma.resize(n);
   m_matV.resize(n,n);
 
-  RowVector e(n);
-  ColVector work(m);
-  MatrixType matA(matrix);
-  const bool wantu = true;
-  const bool wantv = true;
-  int i=0, j=0, k=0;
-
-  // Reduce A to bidiagonal form, storing the diagonal elements
-  // in s and the super-diagonal elements in e.
-  int nct = std::min(m-1,n);
-  int nrt = std::max(0,std::min(n-2,m));
-  for (k = 0; k < std::max(nct,nrt); ++k)
-  {
-    if (k < nct)
-    {
-      // Compute the transformation for the k-th column and
-      // place the k-th diagonal in m_sigma[k].
-      m_sigma[k] = matA.col(k).end(m-k).norm();
-      if (m_sigma[k] != 0.0) // FIXME
-      {
-        if (matA(k,k) < 0.0)
-          m_sigma[k] = -m_sigma[k];
-        matA.col(k).end(m-k) /= m_sigma[k];
-        matA(k,k) += 1.0;
-      }
-      m_sigma[k] = -m_sigma[k];
-    }
-
-    for (j = k+1; j < n; ++j)
-    {
-      if ((k < nct) && (m_sigma[k] != 0.0))
-      {
-        // Apply the transformation.
-        Scalar t = matA.col(k).end(m-k).dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ??
-        t = -t/matA(k,k);
-        matA.col(j).end(m-k) += t * matA.col(k).end(m-k);
-      }
+  int max_iters = 30;
 
-      // Place the k-th row of A into e for the
-      // subsequent calculation of the row transformation.
-      e[j] = matA(k,j);
-    }
+  MatrixVType& V = m_matV;
+  MatrixType A = matrix;
+  SingularValuesType& W = m_sigma;
 
-    // Place the transformation in U for subsequent back multiplication.
-    if (wantu & (k < nct))
-      m_matU.col(k).end(m-k) = matA.col(k).end(m-k);
+  int flag,i,its,j,jj,k,l,nm;
+  Scalar anorm, c, f, g, h, s, scale, x, y, z;
+  bool convergence = true;
 
-    if (k < nrt)
+  Matrix<Scalar,Dynamic,1> rv1(n);
+  g = scale = anorm = 0;
+  // Householder reduction to bidiagonal form.
+  for (i=0; i<n; i++)
+  {
+    l = i+1;
+    rv1[i] = scale*g;
+    g = s = scale = 0.0;
+    if (i < m)
     {
-      // Compute the k-th row transformation and place the
-      // k-th super-diagonal in e[k].
-      e[k] = e.end(n-k-1).norm();
-      if (e[k] != 0.0)
-      {
-          if (e[k+1] < 0.0)
-            e[k] = -e[k];
-          e.end(n-k-1) /= e[k];
-          e[k+1] += 1.0;
-      }
-      e[k] = -e[k];
-      if ((k+1 < m) & (e[k] != 0.0))
+      scale = A.col(i).end(m-i).cwise().abs().sum();
+      if (scale)
       {
-        // Apply the transformation.
-        work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1);
-        for (j = k+1; j < n; ++j)
-          matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1);
+        for (k=i; k<m; k++)
+        {
+          A(k, i) /= scale;
+          s += A(k, i)*A(k, i);
+        }
+        f = A(i, i);
+        g = -sign( ei_sqrt(s), f );
+        h = f*g - s;
+        A(i, i)=f-g;
+        for (j=l; j<n; j++)
+        {
+          s = A.col(i).end(m-i).dot(A.col(j).end(m-i));
+          f = s/h;
+          A.col(j).end(m-i) += f*A.col(i).end(m-i);
+        }
+        A.col(i).end(m-i) *= scale;
       }
-
-      // Place the transformation in V for subsequent back multiplication.
-      if (wantv)
-        m_matV.col(k).end(n-k-1) = e.end(n-k-1);
-    }
-  }
-
-
-  // Set up the final bidiagonal matrix or order p.
-  int p = std::min(n,m+1);
-  if (nct < n)
-    m_sigma[nct] = matA(nct,nct);
-  if (m < p)
-    m_sigma[p-1] = 0.0;
-  if (nrt+1 < p)
-    e[nrt] = matA(nrt,p-1);
-  e[p-1] = 0.0;
-
-  // If required, generate U.
-  if (wantu)
-  {
-    for (j = nct; j < nu; ++j)
-    {
-      m_matU.col(j).setZero();
-      m_matU(j,j) = 1.0;
     }
-    for (k = nct-1; k >= 0; k--)
+    W[i] = scale *g;
+    g = s = scale = 0.0;
+    if (i < m && i != (n-1))
     {
-      if (m_sigma[k] != 0.0)
+      scale = A.row(i).end(n-l).cwise().abs().sum();
+      if (scale)
       {
-        for (j = k+1; j < nu; ++j)
+        for (k=l; k<n; k++)
         {
-          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 ?
-          t = -t/m_matU(k,k);
-          m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k);
+          A(i, k) /= scale;
+          s += A(i, k)*A(i, k);
         }
-        m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k);
-        m_matU(k,k) = Scalar(1) + m_matU(k,k);
-        if (k-1>0)
-          m_matU.col(k).start(k-1).setZero();
-      }
-      else
-      {
-        m_matU.col(k).setZero();
-        m_matU(k,k) = 1.0;
+        f = A(i, l);
+        g = -sign(ei_sqrt(s),f);
+        h = f*g - s;
+        A(i, l) = f-g;
+        for (k=l; k<n; k++)
+          rv1[k] = A(i, k)/h;
+        for (j=l; j<m; j++)
+        {
+          s = A.row(j).end(n-l).dot(A.row(i).end(n-l));
+          A.row(j).end(n-l) += s*rv1.end(n-l).transpose();
+        }
+        A.row(i).end(n-l) *= scale;
       }
     }
+    anorm = std::max( anorm, (ei_abs(W[i])+ei_abs(rv1[i])) );
   }
-
-  // If required, generate V.
-  if (wantv)
+  // Accumulation of right-hand transformations.
+  for (i=(n-1); i>=0; i--)
   {
-    for (k = n-1; k >= 0; k--)
+    //Accumulation of right-hand transformations.
+    if (i < (n-1))
     {
-      if ((k < nrt) & (e[k] != 0.0))
+      if (g)
       {
-        for (j = k+1; j < nu; ++j)
+        for (j=l; j<n;j++) //Double division to avoid possible underflow.
+          V(j, i) = (A(i, j)/A(i, l))/g;
+        for (j=l; j<n; j++)
         {
-          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 ?
-          t = -t/m_matV(k+1,k);
-          m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1);
+          s = A.row(i).end(n-l).dot(V.col(j).end(n-l));
+          V.col(j).end(n-l) += s * V.col(i).end(n-l);
         }
       }
-      m_matV.col(k).setZero();
-      m_matV(k,k) = 1.0;
+      V.row(i).end(n-l).setZero();
+      V.col(i).end(n-l).setZero();
     }
+    V(i, i) = 1.0;
+    g = rv1[i];
+    l = i;
   }
-
-  // Main iteration loop for the singular values.
-  int pp = p-1;
-  int iter = 0;
-  Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
-  while (p > 0)
+  // Accumulation of left-hand transformations.
+  for (i=std::min(m,n)-1; i>=0; i--)
   {
-    int k=0;
-    int kase=0;
-
-    // Here is where a test for too many iterations would go.
-
-    // This section of the program inspects for
-    // negligible elements in the s and e arrays.  On
-    // completion the variables kase and k are set as follows.
-
-    // kase = 1     if s(p) and e[k-1] are negligible and k<p
-    // kase = 2     if s(k) is negligible and k<p
-    // kase = 3     if e[k-1] is negligible, k<p, and
-    //              s(k), ..., s(p) are not negligible (qr step).
-    // kase = 4     if e(p-1) is negligible (convergence).
-
-    for (k = p-2; k >= -1; --k)
+    l = i+1;
+    g = W[i];
+    for (j=l; j<n; j++)
+      A(i, j)=0.0;
+    if (g)
     {
-      if (k == -1)
-          break;
-      if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1])))
+      g = (Scalar)1.0/g;
+      for (j=l; j<n; j++)
       {
-          e[k] = 0.0;
-          break;
+        s = A.col(i).end(m-i).dot(A.col(j).end(m-i));
+        f = (s/A(i, i))*g;
+        A.col(j).end(m-i) += f * A.col(i).end(m-i);
       }
-    }
-    if (k == p-2)
-    {
-      kase = 4;
+      A.col(i).end(m-i) *= g;
     }
     else
+      A.col(i).end(m-i).setZero();
+    ++A(i, i);
+  }
+  // Diagonalization of the bidiagonal form: Loop over
+  // singular values, and over allowed iterations.
+  for (k=(n-1); k>=0; k--)
+  {
+    for (its=1; its<=max_iters; its++)
     {
-      int ks;
-      for (ks = p-1; ks >= k; --ks)
+      flag=1;
+      for (l=k; l>=0; l--)
       {
-        if (ks == k)
-          break;
-        Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0));
-        if (ei_abs(m_sigma[ks]) <= eps*t)
+        // Test for splitting.
+        nm=l-1;
+        // Note that rv1[1] is always zero.
+        if ((double)(ei_abs(rv1[l])+anorm) == anorm)
         {
-          m_sigma[ks] = 0.0;
+          flag=0;
           break;
         }
+        if ((double)(ei_abs(W[nm])+anorm) == anorm)
+          break;
       }
-      if (ks == k)
-      {
-        kase = 3;
-      }
-      else if (ks == p-1)
-      {
-        kase = 1;
-      }
-      else
-      {
-        kase = 2;
-        k = ks;
-      }
-    }
-    ++k;
-
-    // Perform the task indicated by kase.
-    switch (kase)
-    {
-
-      // Deflate negligible s(p).
-      case 1:
+      if (flag)
       {
-        Scalar f(e[p-2]);
-        e[p-2] = 0.0;
-        for (j = p-2; j >= k; --j)
+        c=0.0;  //Cancellation of rv1[l], if l > 1.
+        s=1.0;
+        for (i=l ;i<=k; i++)
         {
-          Scalar t(ei_hypot(m_sigma[j],f));
-          Scalar cs(m_sigma[j]/t);
-          Scalar sn(f/t);
-          m_sigma[j] = t;
-          if (j != k)
-          {
-            f = -sn*e[j-1];
-            e[j-1] = cs*e[j-1];
-          }
-          if (wantv)
+          f = s*rv1[i];
+          rv1[i] = c*rv1[i];
+          if ((double)(ei_abs(f)+anorm) == anorm)
+            break;
+          g = W[i];
+          h = pythagora(f,g);
+          W[i] = h;
+          h = (Scalar)1.0/h;
+          c = g*h;
+          s = -f*h;
+          for (j=0; j<m; j++)
           {
-            for (i = 0; i < n; ++i)
-            {
-              t = cs*m_matV(i,j) + sn*m_matV(i,p-1);
-              m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1);
-              m_matV(i,j) = t;
-            }
+            y = A(j, nm);
+            z = A(j, i);
+            A(j, nm)  = y*c + z*s;
+            A(j, i)   = z*c - y*s;
           }
         }
       }
-      break;
-
-      // Split at negligible s(k).
-      case 2:
+      z = W[k];
+      if (l == k)  //Convergence.
       {
-        Scalar f(e[k-1]);
-        e[k-1] = 0.0;
-        for (j = k; j < p; ++j)
-        {
-          Scalar t(ei_hypot(m_sigma[j],f));
-          Scalar cs( m_sigma[j]/t);
-          Scalar sn(f/t);
-          m_sigma[j] = t;
-          f = -sn*e[j];
-          e[j] = cs*e[j];
-          if (wantu)
-          {
-            for (i = 0; i < m; ++i)
-            {
-              t = cs*m_matU(i,j) + sn*m_matU(i,k-1);
-              m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1);
-              m_matU(i,j) = t;
-            }
-          }
+        if (z < 0.0) { // Singular value is made nonnegative.
+          W[k] = -z;
+          V.col(k) = -V.col(k);
         }
+        break;
       }
-      break;
-
-      // Perform one qr step.
-      case 3:
+      if (its == max_iters)
       {
-        // Calculate the shift.
-        Scalar scale = std::max(std::max(std::max(std::max(
-                        ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])),
-                        ei_abs(m_sigma[k])),ei_abs(e[k]));
-        Scalar sp = m_sigma[p-1]/scale;
-        Scalar spm1 = m_sigma[p-2]/scale;
-        Scalar epm1 = e[p-2]/scale;
-        Scalar sk = m_sigma[k]/scale;
-        Scalar ek = e[k]/scale;
-        Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2);
-        Scalar c = (sp*epm1)*(sp*epm1);
-        Scalar shift = 0.0;
-        if ((b != 0.0) || (c != 0.0))
+        convergence = false;
+      }
+      x = W[l]; // Shift from bottom 2-by-2 minor.
+      nm = k-1;
+      y = W[nm];
+      g = rv1[nm];
+      h = rv1[k];
+      f = ((y-z)*(y+z) + (g-h)*(g+h))/((Scalar)2.0*h*y);
+      g = pythagora(f,1.0);
+      f = ((x-z)*(x+z) + h*((y/(f+sign(g,f)))-h))/x;
+      c = s = 1.0;
+      //Next QR transformation:
+      for (j=l; j<= nm;j++)
+      {
+        i = j+1;
+        g = rv1[i];
+        y = W[i];
+        h = s*g;
+        g = c*g;
+        z = pythagora(f,h);
+        rv1[j] = z;
+        c = f/z;
+        s = h/z;
+        f = x*c + g*s;
+        g = g*c - x*s;
+        h = y*s;
+        y *= c;
+        for (jj=0; jj<n; jj++)
         {
-          shift = ei_sqrt(b*b + c);
-          if (b < 0.0)
-            shift = -shift;
-          shift = c/(b + shift);
+          x = V(jj, j);
+          z = V(jj, i);
+          V(jj, j) = x*c + z*s;
+          V(jj, i) = z*c - x*s;
         }
-        Scalar f = (sk + sp)*(sk - sp) + shift;
-        Scalar g = sk*ek;
-
-        // Chase zeros.
-
-        for (j = k; j < p-1; ++j)
+        z = pythagora(f,h);
+        W[j] = z;
+        // Rotation can be arbitrary if z = 0.
+        if (z)
         {
-          Scalar t = ei_hypot(f,g);
-          Scalar cs = f/t;
-          Scalar sn = g/t;
-          if (j != k)
-            e[j-1] = t;
-          f = cs*m_sigma[j] + sn*e[j];
-          e[j] = cs*e[j] - sn*m_sigma[j];
-          g = sn*m_sigma[j+1];
-          m_sigma[j+1] = cs*m_sigma[j+1];
-          if (wantv)
-          {
-            for (i = 0; i < n; ++i)
-            {
-              t = cs*m_matV(i,j) + sn*m_matV(i,j+1);
-              m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1);
-              m_matV(i,j) = t;
-            }
-          }
-          t = ei_hypot(f,g);
-          cs = f/t;
-          sn = g/t;
-          m_sigma[j] = t;
-          f = cs*e[j] + sn*m_sigma[j+1];
-          m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1];
-          g = sn*e[j+1];
-          e[j+1] = cs*e[j+1];
-          if (wantu && (j < m-1))
-          {
-            for (i = 0; i < m; ++i)
-            {
-              t = cs*m_matU(i,j) + sn*m_matU(i,j+1);
-              m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1);
-              m_matU(i,j) = t;
-            }
-          }
+          z = Scalar(1.0)/z;
+          c = f*z;
+          s = h*z;
         }
-        e[p-2] = f;
-        iter = iter + 1;
-      }
-      break;
-
-      // Convergence.
-      case 4:
-      {
-        // Make the singular values positive.
-        if (m_sigma[k] <= 0.0)
+        f = c*g + s*y;
+        x = c*y - s*g;
+        for (jj=0; jj<m; jj++)
         {
-          m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0);
-          if (wantv)
-            m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1);
+          y = A(jj, j);
+          z = A(jj, i);
+          A(jj, j) = y*c + z*s;
+          A(jj, i) = z*c - y*s;
         }
+      }
+      rv1[l] = 0.0;
+      rv1[k] = f;
+      W[k]   = x;
+    }
+  }
 
-        // Order the singular values.
-        while (k < pp)
-        {
-          if (m_sigma[k] >= m_sigma[k+1])
-            break;
-          Scalar t = m_sigma[k];
-          m_sigma[k] = m_sigma[k+1];
-          m_sigma[k+1] = t;
-          if (wantv && (k < n-1))
-            m_matV.col(k).swap(m_matV.col(k+1));
-          if (wantu && (k < m-1))
-            m_matU.col(k).swap(m_matU.col(k+1));
-          ++k;
-        }
-        iter = 0;
-        p--;
+  // sort the singular values:
+  {
+    for (int i=0; i<n; i++)
+    {
+      int k;
+      W.end(n-i).minCoeff(&k);
+      if (k != i)
+      {
+        std::swap(W[k],W[i]);
+        A.col(i).swap(A.col(k));
+        V.col(i).swap(V.col(k));
       }
-      break;
-    } // end big switch
-  } // end iterations
+    }
+  }
+  m_matU.setZero();
+  if (m>=n)
+    m_matU.block(0,0,m,n) = A;
+  else
+    m_matU = A.block(0,0,m,m);
 
   m_isInitialized = true;
 }