/*****

*Property of the University of British Columbia (UBC),

*Copyright 2001, by UBC.

*

*By receiving this code, you are agreeing to the following terms:

*1. You will use this code for academic purposes only.

*2. For academic use only, you may distribute the binary or executable code

*   to persons at UBC or the Univ. of Western Australia who have previously 

*   read and agreed to these terms, but you must distribute the SOURCE code

*   with it. 

*3. Each file of source code so distributed must have this header attached.

*4. If the code is revised, the programmer's name and revision date must be added

*   to the Revision List below, as well as the revisions identified in the code.

*5. You will not make this code more widely available via any method such as 

*   publishing in print, email mail-list, usenet posting, website etc.

*6. UBC reserves all rights to this work and all derivative works.

*

*For other proposed purposes please contact:

*The University-Industry Liaison Office 

*IRC Room 331 - 2194 Health Sciences Mall 

*University of British Columbia 

*Vancouver, BC, Canada V6T 1Z3 

*Tel: (604) 822-8580 

*Fax: (604) 822-8589 

*

*or contact: 

*Peter D. Lawrence, Professor at peterl@ece.ubc.ca or 

*Greg Z. Grudic, Assistant Professor, at grudic@cs.colorado.edu

*

*Revision List: 

*Greg Grudic, August 28, 1998.

*****/



/*

%

% File:		svd.c

% Program: 	Functions for Functional Approximation code	

%			

%

% Author:	Greg Grudic 

%

*/


#include <stdio.h>
#include <math.h>
#include "pc.h"

void svd(My_Real **A, My_Real *S2, int n)
{
	int  i, j, k, EstColRank = n, RotCount = n, SweepCount = 0,
		slimit = (n<120) ? 30 : n/4;
	My_Real eps = 1e-15, e2 = 10.0*n*eps*eps, tol = 0.1*eps, vt, p, x0,
		y0, q, r, c0, s0, d1, d2;
	
	for (i=0; i<n; i++)

	{ 

		for (j=0; j<n; j++) A[n+i][j] = 0.0; A[n+i][i] = 1.0; 

	}
	while (RotCount != 0 && SweepCount++ <= slimit)

	{
		RotCount = EstColRank*(EstColRank-1)/2;
		for (j=0; j<EstColRank-1; j++) 
			for (k=j+1; k<EstColRank; k++)

			{
				p = q = r = 0.0;
				for (i=0; i<n; i++)

				{
					x0 = A[i][j]; y0 = A[i][k];
					p += x0*y0; q += x0*x0; r += y0*y0;
				}
				S2[j] = q; S2[k] = r;
				if (q >= r) {
					if (q<=e2*S2[0] || fabs(p)<=tol*q)
						RotCount--;
					else 

					{
						p /= q; r = 1.0-r/q; vt = sqrt(4.0*p*p+r*r);
						c0 = sqrt(0.5*(1.0+r/vt)); s0 = p/(vt*c0);
						for (i=0; i<2*n; i++) 

						{
							d1 = A[i][j]; d2 = A[i][k];
							A[i][j] = d1*c0+d2*s0; A[i][k] = -d1*s0+d2*c0;
						}
					}
				} else 

				{
					p /= r; q = q/r-1.0; vt = sqrt(4.0*p*p+q*q);
					s0 = sqrt(0.5*(1.0-q/vt));
					if (p<0.0) s0 = -s0;
					c0 = p/(vt*s0);
					for (i=0; i<2*n; i++) 

					{
						d1 = A[i][j]; d2 = A[i][k];
						A[i][j] = d1*c0+d2*s0; A[i][k] = -d1*s0+d2*c0;
					}
				}
			}
			while (EstColRank>2 && S2[EstColRank-1]<=S2[0]*tol+tol*tol) EstColRank--;
	}
}