
DIRECTORY
LinearSystem, RealFns, Rope, SVD;

SVDImpl: CEDAR PROGRAM
IMPORTS RealFns
EXPORTS SVD
~ BEGIN

RowN: TYPE ~ LinearSystem.RowN;
ColumnN: TYPE ~ LinearSystem.ColumnN;
MatrixN: TYPE ~ LinearSystem.MatrixN;

badShape: PUBLIC ERROR [msg: Rope.ROPE] ~ CODE;
noConvergence: PUBLIC SIGNAL [msg: Rope.ROPE] ~ CODE;

SVDecomp: PUBLIC PROC [a: MatrixN, m, n: INT, w: ColumnN, v: MatrixN]
~ {
Pythag: PROC [a, b: REAL] RETURNS [REAL]
~ {
at: REAL ~ ABS[a];
bt: REAL ~ ABS[b];
SELECT bt FROM
< at => {ct: REAL ~ bt/at; RETURN[at*RealFns.SqRt[1.0 + ct*ct]]};
# 0.0 => {ct: REAL ~ at/bt; RETURN[bt*RealFns.SqRt[1.0 + ct*ct]]};
ENDCASE => RETURN[0.0];
};
Sign: PROC [a, b: REAL] RETURNS [REAL]
~ {IF b >= 0.0 THEN RETURN[ABS[a]] ELSE RETURN[-ABS[a]]};
nm, el, k, j, its, i, jj: INT;
z, y, x, scale, s, h, g, f, c, aNorm: REAL;
rv1: ColumnN м NEW[LinearSystem.VecSeq[n]];	-- w, rv1 are diag, superdiag of reduced A

IF m < n THEN ERROR badShape["You must add enough zero rows to make A square."];

g м scale м aNorm м 0.0;
FOR i IN [0..n) DO
el м i+1;	-- Invariant until start diagonalizing reduction
rv1[i] м scale*g;	-- Finish row reduction from bottom of this loop
g м s м scale м 0.0;
IF i < m THEN {
FOR k IN [i..m) DO  scale м scale + ABS[a[k][i]];  ENDLOOP;
IF scale # 0.0 THEN {
FOR k IN [i..m) DO  a[k][i] м a[k][i]/scale;  s м s + a[k][i]*a[k][i];  ENDLOOP;
f м a[i][i];
g м -Sign[RealFns.SqRt[s], f];
h м f*g - s;
a[i][i] м f - g;
FOR j IN [el..n) DO
s м 0.0;
FOR k IN [i..m) DO  s м s + a[k][i]*a[k][j];  ENDLOOP;
f м s/h;
FOR k IN [i..m) DO  a[k][j] м a[k][j] + f*a[k][i];  ENDLOOP;
ENDLOOP;
FOR k IN [i..m) DO  a[k][i] м scale*a[k][i];  ENDLOOP;
};
};
w[i] м scale*g;
g м s м scale м 0.0;
IF i < m AND i < n-1 THEN {
FOR k IN [el..n) DO  scale м scale + ABS[a[i][k]]  ENDLOOP;
IF scale # 0.0 THEN {
FOR k IN [el..n) DO  a[i][k] м a[i][k]/scale;  s м s + a[i][k]*a[i][k];  ENDLOOP;
f м a[i][el];
g м -Sign[RealFns.SqRt[s], f];
h м f*g - s;
a[i][el] м f - g;
FOR k IN [el..n) DO  rv1[k] м a[i][k]/h;  ENDLOOP;
IF i < m-1 THEN {
FOR j IN [el..m) DO
s м 0.0;
FOR k IN [el..n) DO  s м s + a[j][k]*a[i][k];  ENDLOOP;
FOR k IN [el..n) DO  a[j][k] м a[j][k] + s*rv1[k];  ENDLOOP;
ENDLOOP;
};
FOR k IN [el..n) DO  a[i][k] м scale*a[i][k];  ENDLOOP;
};
};
aNorm м MAX[aNorm, ABS[w[i]] + ABS[rv1[i]]];
ENDLOOP;
FOR i DECREASING IN [0..n) DO
IF i < n-1 THEN {
IF g # 0.0 THEN {	-- Double division to avoid possible underflow:
FOR j IN [el..n) DO  v[j][i] м (a[i][j]/a[i][el])/g;  ENDLOOP;
FOR j IN [el..n) DO
s м 0.0;
FOR k IN [el..n) DO  s м s + a[i][k]*v[k][j];  ENDLOOP;
FOR k IN [el..n) DO  v[k][j] м v[k][j] + s*v[k][i];  ENDLOOP;
ENDLOOP;
};
FOR j IN [el..n) DO  v[i][j] м v[j][i] м 0.0;  ENDLOOP;
};
v[i][i] м 1.0;
g м rv1[i];
el м i;
ENDLOOP;
FOR i DECREASING IN [0..n) DO
el м i+1;
g м w[i];
FOR j IN [el..n) DO  a[i][j] м 0.0;  ENDLOOP;
IF g # 0.0
THEN {
g м 1.0/g;
FOR j IN [el..n) DO
s м 0.0;
FOR k IN [el..m) DO  s м s + a[k][i]*a[k][j];  ENDLOOP;
f м (s/a[i][i])*g;
FOR k IN [i..m) DO  a[k][j] м a[k][j] + f*a[k][i];  ENDLOOP;
ENDLOOP;
FOR j IN [i..m) DO  a[j][i] м a[j][i]*g;  ENDLOOP;
}
ELSE {FOR j IN [i..m) DO  a[j][i] м 0.0;  ENDLOOP};
a[i][i] м a[i][i] + 1.0;
ENDLOOP;
FOR k DECREASING IN [0..n) DO			-- Loop over singular values.
FOR its IN [1..30] DO					-- Loop over allowed iterations.
FOR el DECREASING IN [0..k] DO		-- Test for splitting:
nm м el-1;								-- Note that rv1[0] is always zero.
IF (ABS[rv1[el]] + aNorm) = aNorm THEN EXIT;	-- Thus exit here, never fall through
IF (ABS[w[nm]] + aNorm) = aNorm THEN GO TO SVNegligible;
REPEAT
SVNegligible => {						-- Cancellation of rv1[el], if el > 0 :
c м 0.0;
s м 1.0;
FOR i IN [el..k] DO
f м s*rv1[i];
IF (ABS[f] + aNorm) # aNorm THEN {
g м w[i];
h м Pythag[f, g];
w[i] м h;
h м 1.0/h;
c м g*h;
s м -(f*h);
FOR j IN [0..m) DO
y м a[j][nm];
z м a[j][i];
a[j][nm] м y*c + z*s;
a[j][i] м z*c - y*s;
ENDLOOP;
};
ENDLOOP;
};
ENDLOOP;
z м w[k];
IF el = k THEN {							-- Convergence.
IF z < 0.0 THEN {							-- Singular value is made nonnegative.
w[k] м -z;
FOR j IN [0..n) DO  v[j][k] м -v[j][k];  ENDLOOP;
};
EXIT;		-- No more iterations needed for this singular value.
};
IF its = 30
THEN {SIGNAL noConvergence["No convergence in 30 SVDecomp iterations"]};
x м w[el];						-- 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 м Pythag[f, 1.0];
f м ((x-z)*(x+z) + h*((y/(f+Sign[g, f])) - h))/x;
c м 1.0;
s м 1.0;
FOR j IN [el..nm] DO
i м j+1;
g м rv1[i];
y м w[i];
h м s*g;
g м c*g;
z м Pythag[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 м y*c;
FOR jj IN [0..n) DO
x м v[jj][j];
z м v[jj][i];
v[jj][j] м x*c + z*s;
v[jj][i] м z*c - x*s;
ENDLOOP;
z м Pythag[f, h];
w[j] м z;						-- Rotation can be arbitrary if z = 0.
IF z # 0.0 THEN {z м 1.0/z;  c м f*z;  s м h*z};
f м c*g + s*y;
x м  c*y - s*g;
FOR jj IN [0..m) DO
y м a[jj][j];
z м a[jj][i];
a[jj][j] м y*c + z*s;
a[jj][i] м z*c - y*s;
ENDLOOP;
ENDLOOP;
rv1[el] м 0.0;
rv1[k] м f;
w[k] м x;
ENDLOOP;
ENDLOOP;
};

SVBackSub: PUBLIC PROC [u: MatrixN, w: ColumnN, v: MatrixN, m, n: INT, b: ColumnN, x: ColumnN]
~ {
jj, j, i: INT;
s: REAL;
tmp: ColumnN м NEW[LinearSystem.VecSeq[n]];

FOR j IN [0..n) DO					-- Calculate UT B.
s м 0.0;
IF w[j] # 0.0 THEN {						-- Nonzero result only if wj is nonzero.
FOR i IN [0..m) DO  s м s + u[i][j]*b[i];  ENDLOOP;
s м s/w[j];					-- This is the divide by wj.
};
tmp[j] м s;
ENDLOOP;
FOR j IN [0..n) DO					-- Matrix multiply by V to get answer.
s м 0.0;
FOR jj IN [0..n) DO  s м s + v[j][jj]*tmp[jj];  ENDLOOP;
x[j] м s;
ENDLOOP;
};

END..


DIMENSION A[np][np], U[np][np], W[np], V[np][np], B[np], X[np];
. . .
FOR i IN [0..n) DO						-- Copy A into U if you don't want it to be destroyed.
FOR j IN [0..n) DO
U[i][j] м A[i][j];
ENDLOOP;
ENDLOOP;
SVDecomp[U, n, n, W, V];	-- SVD the square matrix A.
wMax м 0.0;								-- Will be the maximum singular value obtained.
FOR j IN [0..n) DO
IF W[j] > wMax THEN wMax м W[j];
ENDLOOP;
wMin м wMax*1.0E-6;					-- This is where we set the threshold for singular values
FOR j IN [0..n) DO						-- allowed to be nonzero.  The constant is typical, but not
IF W[j] < wMin THEN W[j] м 0.0;  -- universal.  You have to experiment with your own
ENDLOOP;							-- application.
SVBackSub[U, W, V, n, n, B, X];	-- Now we can backsubstitute.
   Є  
SVDImpl.mesa
Copyright ╙ 1989, 1992 by Xerox Corporation.  All rights reserved.
Ken Shoemake, June 30, 1989 2:33:21 am PDT

This code is derived from that in Numerical Recipes by Press, Flannery, Teukolsky, and Vetterling, Cambridge University Press, 1986, third 1987 printing.
Chauser, January 28, 1992 12:03 pm PST
Given matrix A[0..m)[0..n), this routine computes its singular value decomposition,
A = U W VT .  The matrix U replaces A on output.  The diagonal matrix of singular values W is output as a vector W[0..n).  The matrix V (not the transpose VT) is output as V[0..n)[0..n).  The number of rows, m, must be greater or equal to the number of columns, n; if it is smaller, then a should be filled up to square with zero rows.
Pythag computes SqRt[a*a + b*b] without destructive overflow or underflow.
Householder reduction to bidiagonal form.
Zero column i below diagonal
Find norm of column i, avoiding underflow and overflow, for Householder reflection
Apply reflection I-2uuT to remaining columns
Zero row i beyond superdiagonal
Find norm of row i, avoiding underflow and overflow, for Householder reflection
Apply reflection I-2uuT to remaining rows
Accumulation of right-hand transformations.
Accumulation of left-hand transformations.
Diagonalization of the bidiagonal form.
Based on implicit QR reduction of symmetric tridiagonal BTB, for upper bidiagonal B
Use Givens rotations to chase superdiag out of row
Accumulate rotation in left-hand transformations
Accumulate reflection in right-hand transformations
Next QR transformation:
Use alternating right- and left-hand Givens rotations to do implicit shift QR
Rotate first two rows, then chase new off-bidiag non-zero back and forth 'til off matrix
Rotate rows to zero below diagonal
Accumulate Givens rotation in right-hand transformations
Rotate columns to zero above superdiagonal
Accumulate Givens rotation in left-hand transformations
Solves A x = B for a vector X, where A is specified by the arrays U, W, V, as returned by SVDecomp.  M and N are the logical dimensions of A, and will be equal for square matrices.  B[0..m) is the input right-hand side.  X[0..n) is the output solution vector.  No input quantities are destroyed, so the routine may be called sequentially with different B's.
Example of use
Э╩
"  ХNewlineDelimiter
Ц(cedarcode) styleЩ codeЪЬЩKЪЬ
╧eЬ7ЩBKЩ*KЪЬ#╧zЬfЩЪKЩ&ЧKШ Ъ╧k	Ш	KЪЬ!Ш!ЧKШ Ъ╨nxЬЯЬЯШKЪЯЬШKЪЯЬЯШЧKЪЬЯШШ KЪЬЯЬШKЪЬ	ЯЬШ%KЪЬ	ЯЬШ%KШ KЪ	Ь
ЯЬЯЬЯЬЯЬШ/KЪ	ЬЯЬЯЬЯЬЯЬШ5KШ KЪ╧nЬЯЬЯЬЯЬШEЪЬ]╧uЬТвЬ▓ЩгKШЪ
бЬЯЬЯЬЯЬЯЬШ(KЩJKШKЪЬЯЬЯЬШKЪЬЯЬЯЬШЪЯЬЯШKЪЬЯЬ
ЯЬ ШAKЪЬЯЬ
ЯЬ ШBKЪЯЬЯЬШЧKШЧЪ
бЬЯЬЯЬЯЬЯЬШ&KЪЬЯЬ
ЯЬЯЬЯЬЯЬЯЬЯЬШ9ЧKЪЬЯЬШKЪЬ&ЯЬШ+KЪЬЯЬ╧c*ШVKШ KЪЯЬЯЬЯЬ=ШPKШ KЪЬ)Щ)KШЪЯЬЯЬЯШKЪЬ
г0Ш:KЪЬг0ШBKЪЬЩKШЪЯЬЯЬШKЪ
ЯЬЯЬЯЬЯЬ
Я	ЬШ;ЪЯЬЯЬШKЪЬRЩRKЪЯЬЯЬЯЬ6ЯЬШPKШKШKШKЪЬ╨ouЬЩ,KШЪЯЬЯЬ	ЯШKШKЪЯЬЯЬЯЬЯ	ЬШ6KШKЪЯЬЯЬЯЬЯ	ЬШ<KЪЯЬШЧKЪЯЬЯЬЯЬЯ	ЬШ6KШЧKШЧKШKЪЬЩKШЪЯЬЯЬЯЬШKЪ
ЯЬЯЬ	ЯЬЯЬ	Я	ЬШ;ЪЯЬЯЬШKЪЬOЩOKЪ
ЯЬЯЬ	ЯЬЯЬЯ	ЬШQKШKШKШKЪЬдЬЩ)KШKЪЯЬЯЬ	ЯЬЯ	ЬШ2ЪЯЬЯЬШЪЯЬЯЬ	ЯШKШKЪЯЬЯЬ	ЯЬЯ	ЬШ7KЪЯЬЯЬ	ЯЬЯ	ЬШ<KЪЯЬШЧKШЧKЪЯЬЯЬ	ЯЬЯ	ЬШ7KШЧKШЧKЪЬЯЬЯЬ	ЯЬ
Ш,KЪЯЬШЧKЪЬ+Щ+ЪЯЬЯ
ЬЯЬЯШЪЯЬЯЬШЪЯЬЯЬг.ЬШAKЪЯЬЯЬ	ЯЬЯ	ЬШ>ЪЯЬЯЬ	ЯШKШKЪЯЬЯЬ	ЯЬЯ	ЬШ7KЪЯЬЯЬ	ЯЬЯ	ЬШ=KЪЯЬШЧKШЧKЪЯЬЯЬ	ЯЬЯ	ЬШ7KШЧKШKШKШKЪЯЬШЧKЪЬ*Щ*ЪЯЬЯ
ЬЯЬЯШKШ	KШ	KЪЯЬЯЬ	ЯЬЯ	ЬШ-ЪЯЬШ
ЪЯЬШKШ
ЪЯЬЯЬ	ЯШKШKЪЯЬЯЬ	ЯЬЯ	ЬШ7KШKЪЯЬЯЬЯЬЯ	ЬШ<KЪЯЬШЧKЪЯЬЯЬЯЬЯ	ЬШ2KШЧKЪ
ЯЬЯЬЯЬЯЬЯ	ЬШ3ЧKШKЪЯЬШЧKЪЬ'Щ'KЪЬ9дЬЩSЪ	ЯЬЯ
ЬЯЬЯЬгШ=ЪЯЬЯЬ	ЯЬг Ш:Ъ	ЯЬЯ
ЬЯЬЯЬгШ6KЪЬг#Ш5KЪ	ЯЬЯЬЯЬЯЬг%ШRKЪ
ЯЬЯЬЯЬЯЬЯЬШ8ЪЯШЪЬг'Ш>KЩ2KШKШЪЯЬЯЬ	ЯШKШЪЯЬЯЬЯЬШ"KШ	KШKШ	KШ
KШKШKЩ0ЪЯЬЯЬЯШKШKШKШKШKЪЯЬШЧKШЧKЪЯЬШЧKШЧЧKЪЯЬШЧKШ	ЪЯЬЯЬ	гШ&ЪЯЬ	ЯЬ	г&Ш>KШ
KЩ3KЪЯЬЯЬЯЬЯ	ЬШ1KШЧKЪЯЬг5Ш<KШЧЪЯЬ	ШKЪЯЬЯЬ<ШHЧKЪЬг"Ш2KШ	KШ
KШKШKШ*KШKШ1KЪЬЩKШKШKЩMKЩXЪЯЬЯЬ
ЯШKШKШKШ	KШKШKЩ"KШKШKШKШKШKШKШKШKЩ8ЪЯЬЯЬЯШKШKШKШKШKЪЯЬШЧKЩ*KШKЪЬг&Ш5KЪЯЬ	ЯЬ!Ш0KШKШKЩ7ЪЯЬЯЬЯШKШKШKШKШKЪЯЬШЧKЪЯЬШЧKШKШKШ	KЪЯЬШЧKЪЯЬШЧKШKШ ЧKЪб	ЬЯЬЯЬ,ЯЬШ^ЪЬхЩхKШKЪЬ
ЯЬШKЪЬЯЬШKЪЬЯЬШ+KШ Ъ	ЯЬЯЬЯЬг╨cuЬШ)KШЪЯЬЯЬг╨cdгШBKЪЯЬЯЬЯЬЯ	ЬШ3KЪЬгжгШ,KШЧKШKЪЯЬШЧЪЯЬЯЬЯЬг&Ш=KШKЪЯЬЯЬЯЬЯ	ЬШ8KШ	KЪЯЬШЧKШKШ ЧЧЪЯЬШKШ KЩKШ KШ?KШЪЯЬЯЬЯЬг6ШNЪЯЬЯЬЯШKШKЪЯЬШЧKЪЯЬШЧKЪЬгШ4KЪЬг/ШBЪЯЬЯЬЯШKЪЯЬЯЬШ KЪЯЬШЧKЪЬг9ШQЪЯЬЯЬЯЬг;ШSKЪЯЬЯЬг3ШTKЪЯЬгШЧKЪЬ гШ=ЧЧ ЕЧ    ╞  +┌  