lib/svdecomp.c

   1 /* svdecomp - SVD decomposition  routines.                             */
   2 /* Taken from Numerical Recipies.                                      */
   3 /* XLISP-STAT 2.1 Copyright (c) 1990, by Luke Tierney                  */
   4 /* Additions to Xlisp 2.1, Copyright (c) 1989 by David Michael Betz    */
   5 /* You may give out copies of this software; for conditions see the    */
   6 /* file COPYING included with this distribution.                       */
   7
   8 #include "linalg.h"
   9
  10 static double PYTHAG(a, b)
  11         double a, b;
  12 {
  13   double at = fabs(a), bt = fabs(b), ct, result;
  14
  15   if (at > bt)       { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
  16   else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
  17   else result = 0.0;
  18   return(result);
  19 }
  20
  21 #define SWAPD(a, b) (temp = (a), (a) = (b), (b) = temp)
  22
  23 static void
  24 sort_sv(int m, int n, int k,
  25         double **a, double *w, double **v)
  26 {
  27   int i, j;
  28   double temp;
  29
  30   for (i = k; (i < n - 1) && (w[i] < w[i+1]); i++) {
  31     SWAPD(w[i], w[i+1]);
  32         for (j = 0; j < m; j++) SWAPD(a[j][i], a[j][i+1]);
  33         for (j = 0; j < n; j++) SWAPD(v[j][i], v[j][i+1]);
  34   }
  35 }
  36
  37 static double maxarg1, maxarg2;
  38 #define Max(a, b) (maxarg1 = (a), maxarg2 = (b), (maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
  39 #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
  40
  41 int
  42 svdcmp(double **a, int m, int n, double *w, double **v)
  43 {
  44   int flag, i, its, j, jj, k, l, nm;
  45   double c, f, h, s, x, y, z;
  46   double anorm = 0.0, g = 0.0, scale = 0.0;
  47   RVector rv1;
  48
  49   if (m < n) return(FALSE);  /* flag an error if m < n */
  50
  51   rv1 = rvector(n);
  52
  53   /* Householder reduction to bidiagonal form */
  54   for (i = 0; i < n; i++) {
  55
  56     /* left-hand reduction */
  57     l = i + 1;
  58     rv1[i] = scale * g;
  59     g = s = scale = 0.0;
  60     if (i < m) {
  61       for (k = i; k < m; k++) scale += fabs(a[k][i]);
  62       if (scale) {
  63         for (k = i; k < m; k++) {
  64           a[k][i] /= scale;
  65           s += a[k][i] * a[k][i];
  66         }
  67         f = a[i][i];
  68         g = -SIGN(sqrt(s), f);
  69         h = f * g - s;
  70         a[i][i] = f - g;
  71         if (i != n - 1) {
  72           for (j = l; j < n; j++) {
  73             for (s = 0.0, k = i; k < m; k++) s += a[k][i] * a[k][j];
  74             f = s / h;
  75             for (k = i; k < m; k++) a[k][j] += f * a[k][i];
  76           }
  77         }
  78         for (k = i; k < m; k++) a[k][i] *= scale;
  79       }
  80     }
  81     w[i] = scale * g;
  82
  83     /* right-hand reduction */
  84     g = s = scale = 0.0;
  85     if (i < m && i != n - 1) {
  86       for (k = l; k < n; k++) scale += fabs(a[i][k]);
  87       if (scale) {
  88         for (k = l; k < n; k++) {
  89           a[i][k] /= scale;
  90           s += a[i][k] * a[i][k];
  91         }
  92         f = a[i][l];
  93         g = -SIGN(sqrt(s), f);
  94         h = f * g - s;
  95         a[i][l] = f - g;
  96         for (k = l; k < n; k++) rv1[k] = a[i][k] / h;
  97         if (i != m - 1) {
  98           for (j = l; j < m; j++) {
  99             for (s = 0.0, k = l; k < n; k++) s += a[j][k] * a[i][k];
 100             for (k = l; k < n; k++) a[j][k] += s * rv1[k];
 101           }
 102         }
 103         for (k = l; k < n; k++) a[i][k] *= scale;
 104       }
 105     }
 106     anorm = Max(anorm, (fabs(w[i]) + fabs(rv1[i])));
 107   }
 108
 109   /* accumulate the right-hand transformation */
 110   for (i = n - 1; i >= 0; i--) {
 111     if (i < n - 1) {
 112       if (g) {
 113         for (j = l; j < n; j++)
 114           v[j][i] = (a[i][j] / a[i][l]) / g;
 115         for (j = l; j < n; j++) {
 116           for (s = 0.0, k = l; k < n; k++) s += a[i][k] * v[k][j];
 117           for (k = l; k < n; k++) v[k][j] += s * v[k][i];
 118         }
 119       }
 120       for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
 121     }
 122     v[i][i] = 1.0;
 123     g = rv1[i];
 124     l = i;
 125   }
 126
 127   /* accumulate the left-hand transformation */
 128   for (i = n - 1; i >= 0; i--) {
 129     l = i + 1;
 130     g = w[i];
 131     if (i < n - 1)
 132       for (j = l; j < n; j++) a[i][j] = 0.0;
 133     if (g) {
 134       g = 1.0 / g;
 135       if (i != n - 1) {
 136         for (j = l; j < n; j++) {
 137           for (s = 0.0, k = l; k < m; k++) s += a[k][i] * a[k][j];
 138           f = (s / a[i][i]) * g;
 139           for (k = i; k < m; k++) a[k][j] += f * a[k][i];
 140         }
 141       }
 142       for (j = i; j < m; j++) a[j][i] *= g;
 143     }
 144     else {
 145       for (j = i; j < m; j++) a[j][i] = 0.0;
 146     }
 147     ++a[i][i];
 148   }
 149
 150   /* diagonalize the bidiagonal form */
 151   for (k = n - 1; k >= 0; k--) {       /* loop over singular values */
 152     for (its = 0; its < 30; its++) {   /* loop over allowed iterations */
 153       flag = 1;
 154       for (l = k; l >= 0; l--) {       /* test for splitting */
 155         nm = l - 1;
 156         if (fabs(rv1[l]) + anorm == anorm) {
 157           flag = 0;
 158           break;
 159         }
 160         if (fabs(w[nm]) + anorm == anorm) break;
 161       }
 162       if (flag) {
 163         c = 0.0;
 164         s = 1.0;
 165         for (i = l; i <= k; i++) {
 166           f = s * rv1[i];
 167           if (fabs(f) + anorm != anorm) {
 168             g = w[i];
 169             h = PYTHAG(f, g);
 170             w[i] = h;
 171             if (h == 0.0) {
 172               char s[100];
 173               sprintf(s, "h = %f, f = %f, g = %f\n", h, f, g);
 174               fprintf(stdout,"%s",s);
 175             }
 176             h = 1.0 / h;
 177             c = g * h;
 178             s = (- f * h);
 179             for (j = 0; j < m; j++) {
 180               y = a[j][nm];
 181               z = a[j][i];
 182               a[j][nm] = y * c + z * s;
 183               a[j][i] = z * c - y * s;
 184             }
 185           }
 186         }
 187       }
 188       z = w[k];
 189       if (l == k) {        /* convergence */
 190         if (z < 0.0) {     /* make singular value nonnegative */
 191           w[k] = -z;
 192           for (j = 0; j < n; j++) v[j][k] = (-v[j][k]);
 193         }
 194                 sort_sv(m, n, k, a, w, v);
 195         break;
 196       }
 197       if (its >= 30) {
 198         free_vector(rv1);
 199         return(FALSE);     /* return an error flag */
 200       }
 201
 202       /* shift from bottom 2 x 2 minor */
 203       x = w[l];
 204       nm = k - 1;
 205       y = w[nm];
 206       g = rv1[nm];
 207       h = rv1[k];
 208       f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
 209       g = PYTHAG(f, 1.0);
 210       f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
 211
 212       /* next QR transformation */
 213       c = s = 1.0;
 214       for (j = l; j <= nm; j++) {
 215         i = j + 1;
 216         g = rv1[i];
 217         y = w[i];
 218         h = s * g;
 219         g = c * g;
 220         z = PYTHAG(f, h);
 221         rv1[j] = z;
 222         c = f / z;
 223         s = h / z;
 224         f = x * c + g * s;
 225         g = g * c - x * s;
 226         h = y * s;
 227         y = y * c;
 228         for (jj = 0; jj < n; jj++) {
 229           x = v[jj][j];
 230           z = v[jj][i];
 231           v[jj][j] = x * c + z * s;
 232           v[jj][i] = z * c - x * s;
 233         }
 234         z = PYTHAG(f, h);
 235         w[j] = z;
 236         if (z) {
 237           z = 1.0 / z;
 238           c = f * z;
 239           s = h * z;
 240         }
 241         f = (c * g) + (s * y);
 242         x = (c * y) - (s * g);
 243         for (jj = 0; jj < m; jj++) {
 244           y = a[jj][j];
 245           z = a[jj][i];
 246           a[jj][j] = y * c + z * s;
 247           a[jj][i] = z * c - y * s;
 248         }
 249       }
 250       rv1[l] = 0.0;
 251       rv1[k] = f;
 252       w[k] = x;
 253     }
 254   }
 255   free_vector(rv1);
 256   return(TRUE);
 257 }
 258