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