added default bool option to svd to sort the singular values and V. the default is true so pass false to avoid sorting

cpp/Matrix.cpp

@@ -866,7 +866,7 @@ Matrix cholesky_inverse(const Matrix &A)
 /* ************************************************************************* */
 
 // version with in place modification of A
-void svd(Matrix& A, Vector& s, Matrix& V) {
+void svd(Matrix& A, Vector& s, Matrix& V, bool sort) {
 
   const size_t m=A.size1(), n=A.size2();
 
@@ -878,7 +878,7 @@ void svd(Matrix& A, Vector& s, Matrix& V) {
 
   // perform SVD
   // need to pass pointer - 1 in NRC routines so u[1][1] is first element !
-  svdcmp(u-1,m,n,q-1,v-1);
+  svdcmp(u-1,m,n,q-1,v-1, sort);
 	
   // copy singular values back
   s.resize(n);
@@ -891,9 +891,9 @@ void svd(Matrix& A, Vector& s, Matrix& V) {
 }
 
 /* ************************************************************************* */
-void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V) {
+void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V, bool sort) {
   U = A;      // copy
-  svd(U,s,V); // call in-place version
+  svd(U,s,V,sort); // call in-place version
 }
 
 #if 0
@@ -941,7 +941,7 @@ Matrix square_root_positive(const Matrix& A) {
   // Perform SVD, TODO: symmetric SVD?
   Matrix U,V;
   Vector S;
-  svd(A,U,S,V);
+  svd(A,U,S,V,false);
 
   // invert and sqrt diagonal of S
   // We also arbitrarily choose sign to make result have positive signs

cpp/Matrix.h

@@ -308,11 +308,12 @@ inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(
  * @param U output argument: m*n matrix
  * @param S output argument: n-dim vector of singular values, *not* sorted !!!
  * @param V output argument: n*n matrix
+ * @param sort boolean flag to sort singular values and V
  */ 
-void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
+void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V, bool sort=true);
 
 // in-place version
-void svd(Matrix& A, Vector& S, Matrix& V);
+void svd(Matrix& A, Vector& S, Matrix& V, bool sort=true);
 
 /** Use SVD to calculate inverse square root of a matrix */
 Matrix inverse_square_root(const Matrix& A);

cpp/svdcmp.cpp

@@ -8,42 +8,41 @@
 #include <math.h>    /* for 'fabs' */
 #include <iostream>
 #include <vector>
+#include "Matrix.h"
 
 using namespace std;
 
-
 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
 static double sqrarg;
 #define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
-static double maxarg1,maxarg2;
+static double maxarg1, maxarg2;
 #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
         (maxarg1) : (maxarg2))
-static int iminarg1,iminarg2;
+static int iminarg1, iminarg2;
 #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
         (iminarg1) : (iminarg2))
 
-
 /* ************************************************************************* */
 /*
-double pythag(double a, double b)
+ double pythag(double a, double b)
-{
+ {
  double absa = 0.0, absb = 0.0;
  absa=fabs(a);
  absb=fabs(b);
  if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
  else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
-}
+ }
-*/
+ */
 
 /* ************************************************************************* */
-double pythag(double a, double b)
+double pythag(double a, double b) {
-{
 	double absa = 0.0, absb = 0.0;
-  absa=fabs(a);
+	absa = fabs(a);
-  absb=fabs(b);
+	absb = fabs(b);
-  if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
+	if (absa > absb)
+		return absa * sqrt(1.0 + SQR(absb/absa));
 	else {
-    if(absb == 0.0)
+		if (absb == 0.0)
 			return 0.0;
 		else
 			return (absb * sqrt(1.0 + SQR(absa/absb)));
@@ -51,10 +50,9 @@ double pythag(double a, double b)
 }
 
 /* ************************************************************************* */
-void svdcmp(double **a, int m, int n, double w[], double **v)
+void svdcmp(double **a, int m, int n, double w[], double **v, bool sort) {
-{
+	int flag, i, its, j, jj, k, l, nm;
-  int flag,i,its,j,jj,k,l,nm;
+	double anorm, c, f, g, h, s, scale, x, y, z;
-  double anorm,c,f,g,h,s,scale,x,y,z;
 
 	//vector sizes:
 	// w[n] - q-1 passed in
@@ -66,187 +64,241 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
 
 	double *rv1 = new double[n];
 
-  g= 0.0;
+	g = 0.0;
-  scale= 0.0;
+	scale = 0.0;
-  anorm= 0.0;
+	anorm = 0.0;
-  for (i=1;i<=n;i++) {
+	for (i = 1; i <= n; i++) {
-    l=i+1;
+		l = i + 1;
-    rv1[i-1]=scale*g;
+		rv1[i - 1] = scale * g;
-    g=s=scale=0.0;
+		g = s = scale = 0.0;
 		if (i <= m) {
-      for (k=i;k<=m;k++) scale += fabs(a[k][i]); 
+			for (k = i; k <= m; k++)
+				scale += fabs(a[k][i]);
 			if (scale) {
-	for (k=i;k<=m;k++) {
+				for (k = i; k <= m; k++) {
 					a[k][i] /= scale;
-	  s += a[k][i]*a[k][i];
+					s += a[k][i] * a[k][i];
 				}
-	f=a[i][i];
+				f = a[i][i];
 				g = -SIGN(sqrt(s),f);
-	h=f*g-s;
+				h = f * g - s;
-	a[i][i]=f-g;
+				a[i][i] = f - g;
-	for (j=l;j<=n;j++) {
+				for (j = l; j <= n; j++) {
-	  for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
+					for (s = 0.0, k = i; k <= m; k++)
-	  f=s/h;
+						s += a[k][i] * a[k][j];
-	  for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
+					f = s / h;
+					for (k = i; k <= m; k++)
+						a[k][j] += f * a[k][i];
 				}
-	for (k=i;k<=m;k++) a[k][i] *= scale;
+				for (k = i; k <= m; k++)
+					a[k][i] *= scale;
 			}
 		}
-    w[i]=scale *g; 
+		w[i] = scale * g;
-    g=s=scale=0.0;
+		g = s = scale = 0.0;
 		if (i <= m && i != n) {
-      for (k=l;k<=n;k++) scale += fabs(a[i][k]);
+			for (k = l; k <= n; k++)
+				scale += fabs(a[i][k]);
 			if (scale) {
-	for (k=l;k<=n;k++) {
+				for (k = l; k <= n; k++) {
 					a[i][k] /= scale;
-	  s += a[i][k]*a[i][k];
+					s += a[i][k] * a[i][k];
 				}
-	f=a[i][l];
+				f = a[i][l];
 				g = -SIGN(sqrt(s),f);
-	h=f*g-s;
+				h = f * g - s;
-	a[i][l]=f-g;
+				a[i][l] = f - g;
-	for (k=l;k<=n;k++) 
+				for (k = l; k <= n; k++) {
-	  {
+					rv1[k - 1] = a[i][k] / h;
-	    rv1[k-1]=a[i][k]/h;
 				}
 
-	for (j=l;j<=m;j++) {
+				for (j = l; j <= m; j++) {
-	  for (s=0.0,k=l;k<=n;k++) 
+					for (s = 0.0, k = l; k <= n; k++)
-	    s += a[j][k]*a[i][k];
+						s += a[j][k] * a[i][k];
-	  for (k=l;k<=n;k++) 
+					for (k = l; k <= n; k++) {
-	    {
+						a[j][k] += s * rv1[k - 1];
-	      a[j][k] += s*rv1[k-1];
 					}
 				}
-	for (k=l;k<=n;k++) a[i][k] *= scale;
+				for (k = l; k <= n; k++)
+					a[i][k] *= scale;
 			}
 		}
-    anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1]))); 
+		anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1])));
 
 	}
-  for (i=n;i>=1;i--) {
+	for (i = n; i >= 1; i--) {
 		if (i < n) {
 			if (g) {
-	for (j=l;j<=n;j++)
+				for (j = l; j <= n; j++)
-	  v[j][i]=(a[i][j]/a[i][l])/g;
+					v[j][i] = (a[i][j] / a[i][l]) / g;
-	for (j=l;j<=n;j++) {
+				for (j = l; j <= n; j++) {
-	  for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
+					for (s = 0.0, k = l; k <= n; k++)
-	  for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
+						s += a[i][k] * v[k][j];
+					for (k = l; k <= n; k++)
+						v[k][j] += s * v[k][i];
 				}
 			}
-      for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
+			for (j = l; j <= n; j++)
+				v[i][j] = v[j][i] = 0.0;
 		}
-    v[i][i]=1.0;
+		v[i][i] = 1.0;
-    g=rv1[i-1];
+		g = rv1[i - 1];
-    l=i;
+		l = i;
 	}
-  for (i=IMIN(m,n);i>=1;i--) {
+	for (i = IMIN(m,n); i >= 1; i--) {
-    l=i+1;
+		l = i + 1;
-    g=w[i];
+		g = w[i];
-    for (j=l;j<=n;j++) a[i][j]=0.0;
+		for (j = l; j <= n; j++)
+			a[i][j] = 0.0;
 		if (g) {
-      g=1.0/g;
+			g = 1.0 / g;
-      for (j=l;j<=n;j++) {
+			for (j = l; j <= n; j++) {
-	for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
+				for (s = 0.0, k = l; k <= m; k++)
-	f=(s/a[i][i])*g;
+					s += a[k][i] * a[k][j];
-	for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
+				f = (s / a[i][i]) * g;
+				for (k = i; k <= m; k++)
+					a[k][j] += f * a[k][i];
 			}
-      for (j=i;j<=m;j++) a[j][i] *= g;
+			for (j = i; j <= m; j++)
-    } else for (j=i;j<=m;j++) a[j][i]=0.0;
+				a[j][i] *= g;
+		} else
+			for (j = i; j <= m; j++)
+				a[j][i] = 0.0;
 		++a[i][i];
 	}
-  for (k=n;k>=1;k--) {
+	for (k = n; k >= 1; k--) {
-    for (its=1;its<=30;its++) {
+		for (its = 1; its <= 30; its++) {
-      flag=1;
+			flag = 1;
-      for (l=k;l>=1;l--) {
+			for (l = k; l >= 1; l--) {
-	nm=l-1;
+				nm = l - 1;
-	if ((double)(fabs(rv1[l-1])+anorm) == anorm) {
+				if ((double) (fabs(rv1[l - 1]) + anorm) == anorm) {
-	  flag=0;
+					flag = 0;
 					break;
 				}
-	if ((double)(fabs(w[nm])+anorm) == anorm) break; 
+				if ((double) (fabs(w[nm]) + anorm) == anorm)
+					break;
 			}
 			if (flag) {
-	c=0.0;
+				c = 0.0;
-	s=1.0;
+				s = 1.0;
-	for (i=l;i<=k;i++) {
+				for (i = l; i <= k; i++) {
-	  f=s*rv1[i-1];
+					f = s * rv1[i - 1];
-	  rv1[i-1]=c*rv1[i-1];
+					rv1[i - 1] = c * rv1[i - 1];
-	  if ((double)(fabs(f)+anorm) == anorm) break;
+					if ((double) (fabs(f) + anorm) == anorm)
-	  g=w[i];  
+						break;
-	  h=pythag(f,g);
+					g = w[i];
-	  w[i]=h;
+					h = pythag(f, g);
-	  h=1.0/h;
+					w[i] = h;
-	  c=g*h;
+					h = 1.0 / h;
-	  s = -f*h;
+					c = g * h;
-	  for (j=1;j<=m;j++) {
+					s = -f * h;
-	    y=a[j][nm];
+					for (j = 1; j <= m; j++) {
-	    z=a[j][i];
+						y = a[j][nm];
-	    a[j][nm]=y*c+z*s;
+						z = a[j][i];
-	    a[j][i]=z*c-y*s;
+						a[j][nm] = y * c + z * s;
+						a[j][i] = z * c - y * s;
 					}
 				}
 			}
-      z=w[k]; 
+			z = w[k];
 			if (l == k) {
 				if (z < 0.0) {
 					w[k] = -z;
-	  for (j=1;j<=n;j++) v[j][k] = -v[j][k];
+					for (j = 1; j <= n; j++)
+						v[j][k] = -v[j][k];
 				}
 				break;
 			}
-      if (its == 30) throw(std::domain_error("no convergence in 30 svdcmp iterations"));
+			if (its == 30)
-      x=w[l];  
+				throw(std::domain_error(
-      nm=k-1;
+						"no convergence in 30 svdcmp iterations"));
-      y=w[nm];  
+			x = w[l];
-      g=rv1[nm-1] ;
+			nm = k - 1;
-      h=rv1[k-1];
+			y = w[nm];
-      f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
+			g = rv1[nm - 1];
-      g=pythag(f,1.0);
+			h = rv1[k - 1];
-      f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
+			f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
-      c=s=1.0;
+			g = pythag(f, 1.0);
-      for (j=l;j<=nm;j++) {
+			f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g,f))) - h)) / x;
-	i=j+1;
+			c = s = 1.0;
-	g=rv1[i-1];
+			for (j = l; j <= nm; j++) {
-	y=w[i]; 
+				i = j + 1;
-	h=s*g;
+				g = rv1[i - 1];
-	g=c*g;
+				y = w[i];
-	z=pythag(f,h);
+				h = s * g;
-	rv1[j-1]=z;
+				g = c * g;
-	c=f/z;
+				z = pythag(f, h);
-	s=h/z;
+				rv1[j - 1] = z;
-	f=x*c+g*s;
+				c = f / z;
-	g = g*c-x*s;
+				s = h / z;
-	h=y*s;
+				f = x * c + g * s;
+				g = g * c - x * s;
+				h = y * s;
 				y *= c;
-	for (jj=1;jj<=n;jj++) {
+				for (jj = 1; jj <= n; jj++) {
-	  x=v[jj][j];
+					x = v[jj][j];
-	  z=v[jj][i];
+					z = v[jj][i];
-	  v[jj][j]=x*c+z*s;
+					v[jj][j] = x * c + z * s;
-	  v[jj][i]=z*c-x*s;
+					v[jj][i] = z * c - x * s;
 				}
-	z=pythag(f,h);
+				z = pythag(f, h);
-	w[j]=z; 
+				w[j] = z;
 				if (z) {
-	  z=1.0/z;
+					z = 1.0 / z;
-	  c=f*z;
+					c = f * z;
-	  s=h*z;
+					s = h * z;
 				}
-	f=c*g+s*y;
+				f = c * g + s * y;
-	x=c*y-s*g;
+				x = c * y - s * g;
-	for (jj=1;jj<=m;jj++) {
+				for (jj = 1; jj <= m; jj++) {
-	  y=a[jj][j];
+					y = a[jj][j];
-	  z=a[jj][i];
+					z = a[jj][i];
-	  a[jj][j]=y*c+z*s;
+					a[jj][j] = y * c + z * s;
-	  a[jj][i]=z*c-y*s;
+					a[jj][i] = z * c - y * s;
 				}
 			}
-      rv1[l-1]=0.0;
+			rv1[l - 1] = 0.0;
-      rv1[k-1]=f;
+			rv1[k - 1] = f;
-      w[k]=x; 
+			w[k] = x;
 		}
 	}
 
+	if (sort) {
+		int *indices1 = new int[n];
+		int *indices2 = new int[n];
+		for (int i1 = 1; i1 <= n; i1++) {
+			indices1[i1] = i1;
+			indices2[i1] = i1;
+		}
+
+		for (int i1 = 1; i1 <= n; i1++) {
+			for (int i2 = i1; i2 <= n; i2++) {
+				if (w[i1] < w[i2]) {
+					double temp = w[i1];
+					w[i1] = w[i2];
+					w[i2] = temp;
+					int ind = indices2[i1];
+					indices2[i1] = indices2[i2];
+					indices2[i2] = ind;
+				}
+			}
+		}
+
+		for (int i1 = 1; i1 <= n; i1++) {
+			if (indices1[i1] != indices2[i1]) {
+				int src = indices1[i1], dst = indices2[i1];
+				for (int j = 1; j <= n; j++) {
+					double temp = v[j][src];
+					v[j][src] = v[j][dst];
+					v[j][dst] = temp;
+				}
+				indices1[dst] = src;
+			}
+		}
+		delete[] indices1;
+		delete[] indices2;
+	}
+
 	delete[] rv1;
 
 }

cpp/svdcmp.h

@@ -10,5 +10,5 @@
 #pragma once
 
 /** SVD decomposition */
-void svdcmp(double **a, int m, int n, double w[], double **v);
+void svdcmp(double **a, int m, int n, double w[], double **v, bool sort = true);