/* From bryant@sioux.stanford.edu Sat Apr  3 14:57:54 1993
Return-Path: <bryant@sioux.stanford.edu>
Received: from sioux.stanford.edu by alnitak.usc.edu (4.1/SMI-4.1+ucs-3.6)
        id AA12724; Sat, 3 Apr 93 14:57:52 PST
Received: from oglala.ice (oglala.Stanford.EDU) by sioux.stanford.edu (4.1/inc-1.0)
        id AA07300; Sat, 3 Apr 93 14:53:25 PST
Date: Sat, 3 Apr 93 14:53:25 PST
From: bryant@sioux.stanford.edu (Bryant Marks)
Message-Id: <9304032253.AA07300@sioux.stanford.edu>
To: ajayshah@rcf.usc.edu
Subject: Re:  SVD
Status: ORr


> Hi!   Long ago you sent me an svd routine in C based on code
> from Nash in Pascal.  Has this changed any over the years?  (Your
> email is dated July 1992).  Is your code available by anon ftp?

Hi Ajay,

I don't think I have changed the code -- but here's my most recent
version of the code, you can check to see if it's any different.
Currently it's not available via anonymous ftp but feel free to
redistribute the code -- it seems to work well in the application
I'm using it in.


Bryant
*/

/*This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
  for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
  Modifications include:
       Translation from Pascal to ANSI C.
       Array indexing from 0 rather than 1.
       Float replaced by double everywhere.
       Support for the Matrix structure.
       I changed the array indexing so that the matricies (float [][])
          could be replaced be a single list (double *) for more
          efficient communication with Mathematica.
*/

/* rjrw 7/7/99: changed z back to a vector, moved one line... */

/*
  Derek A. Lamb 2016: added comments to aid understanding, since
  Nash's book will only get more difficult to find in the future.
*/

/*
  Form a singular value decomposition of matrix A.
  The SVD is thin. To get a full SVD, zero-pad A on the bottom,
  transposing if is tall, so nCol == nRow. If zero-pad on right per
  some literature, the column rotation doesn't happen since it
  thinks it's already finished.
  Input:
    A is stored in the nRow*nCol elements of U
  Upon return:
    array U will become the U of an svd.
    array V will become the square (nCol*nCol) matrix V of the svd.
    S will contain the nCol singular values.

  The input matrix A must have nRow >= nCol. If it does
  not, one should input the transpose of A, A", to find the the svd
  of A, since A = U x S x V" and A" = V x S x U". (The " means transpose.)
*/

#include <math.h>
#define TOLERANCE 1.0e-22
#define SVD_ROTATE_COL(M, colind, colind2, rowind, ncols, nrows, cos0, sin0) \
 for (rowind=0; rowind<nrows; rowind++) \
   SVD_ROTATE_VALS(M[ncols*rowind+colind], M[ncols*rowind+colind2], cos0, sin0)
#define SVD_ROTATE_VALS(colval1, colval2, cos0, sin0) \
 { \
   double d1 = colval1, d2 = colval2; \
   colval1 = d1*cos0 + d2*sin0; colval2 = -d1*sin0 + d2*cos0; \
 }
#define PRINTMAT(M, colind, rowind, ncols, nrows) \
  printf(#M ":\n"); \
  for (rowind=0;rowind<nrows;rowind++) { \
    for (colind=0;colind<ncols;colind++) \
      printf("%4.2g ",M[rowind*ncols + colind]); \
    printf("\n"); \
  }
#define PRINTVEC(V, colind, ncols) \
  printf(#V " vector:\n"); \
  for (colind=0;colind<ncols;colind++) \
    printf("%4.2g ",V[colind]); \
  printf("\n");

#if DEBUG
#include <stdio.h>
#endif

void SVD(double *U, double *V, double *S, int nRow, int nCol)
{
  int i, j, k, EstColRank, RotCount, SweepCount, slimit;
  double eps, e2, tol, vt, p, x0, y0, q, r, c0, s0;
  eps = TOLERANCE;
  /*set a limit on the number of sweeps allowed. A suggested limit is slimit=max([nCol/4],6).*/
  slimit = nCol/4;
  if (slimit < 6.0)
    slimit = 6;
  SweepCount = 0;
  e2 = 10.0*nRow*eps*eps;
  tol = eps*.1; /*set convergence tolerances */
  EstColRank = nCol; /* current estimate of rank */
  /* Set V matrix to the unit matrix of order nCol. */
  for (i=0; i<nCol; i++)
    for (j=0; j<nCol; j++)
      V[ nCol*i + j ] = i == j ? 1.0 : 0.0;
#if DEBUG
  printf("BEFORE "); PRINTMAT(U, j, i, nCol, nRow)
  printf("BEFORE "); PRINTMAT(V, j, i, nCol, nCol)
#endif
  RotCount = EstColRank*(EstColRank-1)/2;
  /*until convergence is achieved or too many sweeps are carried out*/
  while (RotCount != 0 && SweepCount <= slimit) {
    RotCount = EstColRank*(EstColRank-1)/2; /* rotation counter */
    SweepCount++;
    for (j=0; j<EstColRank-1; j++) { /* main cyclic Jacobi sweep */
      for (k=j+1; k<EstColRank; k++) {
        p = q = r = 0.0;
        /*
          Some helpful definitions from Nash's book (pg 34) to
          understand this p, q, & r rotation business:
          p = x"y; (" means transpose)
          q = x"x - y"y;
          v = sqrt(4*p^2+q^2);
          if (q>=0) {
            cos(phi) = sqrt((v+q)/(2*v));
            sin(phi) = p/(v*cos(phi));
          } else if (q<0) {,
            (sgn(p)=+1 for p>=0, -1 for p<0);
            sin(phi) = sgn(p)*sqrt((v-q)/(2*v));
            cos(phi) = p/(v*sin(phi));
          }
          This formulation avoids the subtraction of two nearly
          equal numbers, which is bound to happen to q and v as
          the matrix approaches orthogonality.
        */
        for (i=0; i<nRow; i++) {
          x0 = U[nCol*i+j]; y0 = U[nCol*i+k];
          p += x0*y0; q += x0*x0; r += y0*y0;
        }
        S[j] = q; S[k] = r;
        /* Convergence test: will rotation exchange order of columns?*/
        if (q >= r) { /* check if columns are ordered */
          /* columns are ordered, so try convergence test */
          if (q<=e2*S[0] || fabs(p)<=tol*q) {
            /* There is no more work on this particular pair of
               columns in the current sweep. The first condition
               checks for very small column norms in BOTH
               columns, for which no rotation makes sense. The
               second condition determines if the inner product
               is small with respect to the larger of the
               columns, which implies a very small rotation
               angle. */
            RotCount--;
          } else {
            /* columns are in order, but their inner product is not small */
            p /= q; r = 1 - r/q; vt = sqrt(4*p*p+r*r);
            /* DAL thinks the fabs is unnecessary in the
               next line: vt is non-negative; q>=r, r/q <=1
               so after the assignment 0<= r <=1, so r/vt is
               positive, so everything inside the sqrt
               should be positive even without the fabs. abs
               isn't in Nash's book. c0 and s0 are cos(phi) and sin(phi) as above for q>=0*/
            c0 = sqrt(fabs(.5*(1+r/vt))); s0 = p/(vt*c0);
            SVD_ROTATE_COL(U, j, k, i, nCol, nRow, c0, s0)
            SVD_ROTATE_COL(V, j, k, i, nCol, nCol, c0, s0)
          }
        } else {
        /* columns out of order -- must rotate */
        /* note: r>q, and cannot be zero since both are sums
             of squares for the svd. In the case of a real
             symmetric matrix, this assumption must be
             questioned. */
          p /= r; q = q/r-1; vt = sqrt(4*p*p+q*q);
          s0 = sqrt(fabs(.5*(1-q/vt)));
          /* DAL wondering about the fabs again, since after
             assignment q is <0 and vt is again positive, so
             everything inside the fabs should be positive
             already */
          if (p<0) s0 = -s0; /*s0 and c0 are sin(phi) and cos(phi) as above for q<0 */
          c0 = p/(vt*s0);
          SVD_ROTATE_COL(U, j, k, i, nCol, nRow, c0, s0)
          SVD_ROTATE_COL(V, j, k, i, nCol, nCol, c0, s0)
        }
        /* Both angle calculations have been set up so that
           large numbers do not occur in intermediate
           quantities. This is easy in the svd case, since
           quantities x2,y2 cannot be negative.*/
      } /* loop on k */
    } /* loop on j */
    while (EstColRank>=3 && S[EstColRank-1]<=S[0]*tol+tol*tol)
      EstColRank--;
  }
  for (j = 0; j < nCol; j++)
    S[j] = sqrt(S[j]);
  for (i=0; i < nRow; i++)
    for (j=0; j < nCol; j++)
      if (U[ nCol*i + j ])
        U[ nCol*i + j ] /= S[j];
#if DEBUG
  if (SweepCount > slimit)
    fprintf(stderr, "Sweeps = %d\n", SweepCount);
  printf("AFTER "); PRINTMAT(U, j, i, nCol, nRow)
  printf("AFTER "); PRINTMAT(V, j, i, nCol, nCol)
  PRINTVEC(S, j, nCol)
#endif
}

/* build this with `cc -DMAIN lib/PDL/MatrixOps/svd.c` */
#ifdef MAIN
#include <stdio.h>
#define NC 2
#define NR 2
#define NC2 2
#define NR2 3
int main()
{
  int i,j;
  double u[NC*NR] = { 1, 3, -4, 3 }, v[NC*NC], z[NC];
  double u2[NC2*NR2] = { 3, 1, -1, 3, -2, 3 }, v2[NC2*NC2], z2[NC2];
  printf("BEFORE "); PRINTMAT(u, j, i, NC, NR)
  SVD(u, v, z, NR, NC);
  PRINTMAT(u, j, i, NC, NR)
  PRINTMAT(v, j, i, NC, NC)
  PRINTVEC(z, j, NC)
  printf("BEFORE "); PRINTMAT(u2, j, i, NC2, NR2)
  SVD(u2, v2, z2, NR2, NC2);
  PRINTMAT(u2, j, i, NC2, NR2)
  PRINTMAT(v2, j, i, NC2, NC2)
  PRINTVEC(z2, j, NC2)
  return 0;
}
#endif
