From 7767e4a63bd258b5853c7827d42a2c95fac836da Mon Sep 17 00:00:00 2001
From: cnr-isti-vclab <paolo.cignoni@isti.cnr.it>
Date: Mon, 18 Oct 2004 08:25:28 +0000
Subject: [PATCH] Added SingularValueDecomposition method

---
 vcg/math/lin_algebra.h | 263 ++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 260 insertions(+), 3 deletions(-)

diff --git a/vcg/math/lin_algebra.h b/vcg/math/lin_algebra.h
index bd301f7b..90e50a6f 100644
--- a/vcg/math/lin_algebra.h
+++ b/vcg/math/lin_algebra.h
@@ -13,7 +13,7 @@ namespace vcg
 	* \param nrot returns the number of Jacobi rotations that were required. 
 	*/
 	template <typename TYPE>
-	static void Jacobi(Matrix44<TYPE> &w, Point4<TYPE> &d, Matrix44<TYPE> &v, int &nrot) 
+		static void Jacobi(Matrix44<TYPE> &w, Point4<TYPE> &d, Matrix44<TYPE> &v, int &nrot) 
 	{ 
 		int j,iq,ip,i; 
 		//assert(w.IsSymmetric());
@@ -112,15 +112,259 @@ namespace vcg
 	/*!
 	*	Given a matrix <I>A<SUB>m×n</SUB></I>, this routine computes its singular value decomposition,
 	*	i.e. <I>A=U·W·V<SUP>T</SUP></I>. The matrix <I>A</I> will be destroyed!
-	*	\param A	...
+	*	(This is the implementation described in <I>Numerical Recipies</I>).
+	*	\param A	the matrix to be decomposed
 	*	\param W	the diagonal matrix of singular values <I>W</I>, stored as a vector <I>W[1...N]</I>
 	*	\param V	the matrix <I>V</I> (not the transpose <I>V<SUP>T</SUP></I>)
+	*	\param max_iters	max iteration number (default = 30).
+	*	\return 
 	*/
 	template <typename MATRIX_TYPE>
-		static void SingularValueDecomposition(MATRIX_TYPE &A, typename MATRIX_TYPE::ScalarType *W, MATRIX_TYPE &V)
+		static bool SingularValueDecomposition(MATRIX_TYPE &A, typename MATRIX_TYPE::ScalarType *W, MATRIX_TYPE &V, const int max_iters = 30)
 	{
 		typedef typename MATRIX_TYPE::ScalarType ScalarType;
+		int m = (int) A.RowsNumber();
+		int n = (int) A.ColumnsNumber();
+		int flag,i,its,j,jj,k,l,nm;
+		double anorm, c, f, g, h, s, scale, x, y, z, *rv1;
+		bool convergence = true;
 
+		rv1 = new double[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)
+			{
+				for (k = i; k<m; k++) 
+					scale += fabs(A[k][i]);
+				if (scale) 
+				{
+					for (k=i; k<m; k++) 
+					{
+						A[k][i] /= scale;
+						s += A[k][i]*A[k][i];
+					}
+					f=A[i][i];
+					g = -vcg::sign<double>( sqrt(s), f );
+					h = f*g - s;
+					A[i][i]=f-g;
+					for (j=l; j<n; j++) 
+					{
+						for (s=0.0, k=i; k<m; k++) 
+							s += A[k][i]*A[k][j];
+						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;
+				}
+			}
+			W[i] = scale *g;
+			g = s = scale = 0.0;
+			if (i < m && i != (n-1)) 
+			{
+				for (k=l; k<n; k++) 
+					scale += fabs(A[i][k]);
+				if (scale) 
+				{
+					for (k=l; k<n; k++) 
+					{
+						A[i][k] /= scale;
+						s += A[i][k]*A[i][k];
+					}
+					f = A[i][l];
+					g = -vcg::sign<double>(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++) 
+					{
+						for (s=0.0, k=l; k<n; k++) 
+							s += A[j][k]*A[i][k];
+						for (k=l; k<n; k++) 
+							A[j][k] += s*rv1[k];
+					}
+					for (k=l; k<n; k++) 
+						A[i][k] *= scale;
+				}
+			}
+			anorm=math::Max( anorm, (fabs(W[i])+fabs(rv1[i])) );
+		}
+		// Accumulation of right-hand transformations.
+		for (i=(n-1); i>=0; i--) 
+		{ 
+			//Accumulation of right-hand transformations.
+			if (i < (n-1)) 
+			{
+				if (g) 
+				{
+					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++) 
+					{
+						for (s=0.0, k=l; k<n; k++) 
+							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;
+			}
+			V[i][i] = 1.0;
+			g = rv1[i];
+			l = i;
+		}
+		// Accumulation of left-hand transformations.
+		for (i=math::Min(m,n)-1; i>=0; i--) 
+		{
+			l = i+1;
+			g = W[i];
+			for (j=l; j<n; j++) 
+				A[i][j]=0.0;
+			if (g) 
+			{
+				g = 1.0/g;
+				for (j=l; j<n; j++) 
+				{
+					for (s=0.0, k=l; k<m; k++) 
+						s += A[k][i]*A[k][j];
+					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;
+			} 
+			else 
+				for (j=i; j<m; j++) 
+					A[j][i] = 0.0;
+			++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++) 
+			{
+				flag=1;
+				for (l=k; l>=0; l--) 
+				{ 
+					// Test for splitting.
+					nm=l-1; 
+					// Note that rv1[1] is always zero.
+					if ((double)(fabs(rv1[l])+anorm) == anorm) 
+					{
+						flag=0;
+						break;
+					}
+					if ((double)(fabs(W[nm])+anorm) == anorm) 
+						break;
+				}
+				if (flag) 
+				{
+					c=0.0;  //Cancellation of rv1[l], if l > 1.
+					s=1.0;
+					for (i=l ;i<=k; i++) 
+					{
+						f = s*rv1[i];
+						rv1[i] = c*rv1[i];
+						if ((double)(fabs(f)+anorm) == anorm) 
+							break;
+						g = W[i];
+						h = pythagora< double >(f,g);
+						W[i] = h;
+						h = 1.0/h;
+						c = g*h;
+						s = -f*h;
+						for (j=0; j<m; j++) 
+						{
+							y = A[j][nm];
+							z = A[j][i];
+							A[j][nm]	= y*c + z*s;
+							A[j][i]		= z*c - y*s;
+						}
+					}
+				}
+				z = W[k];
+				if (l == k)  //Convergence.
+				{ 
+					if (z < 0.0) { // Singular value is made nonnegative.
+						W[k] = -z;
+						for (j=0; j<n; j++) 
+							V[j][k] = -V[j][k];
+					}
+					break;
+				}
+				if (its == max_iters)
+				{
+					printf("no convergence in %d SingularValueDecomposition iterations\n", max_iters);
+					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))/(2.0*h*y);
+				g = pythagora<double>(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<double>(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++) 
+					{
+						x = V[jj][j];
+						z = V[jj][i];
+						V[jj][j] = x*c + z*s;
+						V[jj][i] = z*c - x*s;
+					}
+					z = pythagora<double>(f,h);
+					W[j] = z;
+					// Rotation can be arbitrary if z = 0.
+					if (z) 
+					{
+						z = 1.0/z;
+						c = f*z;
+						s = h*z;
+					}
+					f = c*g + s*y;
+					x = c*y - s*g;
+					for (jj=0; jj<m; jj++) 
+					{
+						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;
+			}
+		}
+		delete []rv1;
+		return convergence;
 	};  
 
 
@@ -174,5 +418,18 @@ namespace vcg
 			return (abs_b == 0.0 ? 0.0 : abs_b*sqrt(1.0+sqr(abs_a/abs_b)));
 	};
 
+	template <typename TYPE>
+		inline TYPE sign(TYPE a, TYPE b)
+	{
+		return (b >= 0.0 ? fabs(a) : -fabs(a));
+	};
+
+	template <typename TYPE>
+		inline TYPE sqr(TYPE a)
+	{
+		TYPE sqr_arg = a;
+		return (sqr_arg == 0 ? 0 : sqr_arg*sqr_arg);
+	}
+
 	/*! @} */
 }; // end of namespace
\ No newline at end of file