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 #include <stdio.h>
  11
  12 static double PYTHAG(double a, double b)
  13 {
  14   double at = fabs(a), bt = fabs(b), ct, result;
  15
  16   if (at > bt)       { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
  17   else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
  18   else result = 0.0;
  19   return(result);
  20 }
  21
  22 #define SWAPD(a, b) (temp = (a), (a) = (b), (b) = temp)
  23
  24 static void
  25 sort_sv(size_t m, size_t n, size_t k,
  26         double **a, double *w, double **v)
  27 {
  28   size_t 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 int
  43 svdcmp(double **a, size_t m, size_t n, double *w, double **v)
  44 {
  45   int flag, its;
  46   long i, k, l;  /* These must be signed */
  47   size_t j, jj, 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
 119         for (j = l; j < n; j++) {
 120           for (s = 0.0, k = l; k < n; k++) s += a[i][k] * v[k][j];
 121           for (k = l; k < n; k++) v[k][j] += s * v[k][i];
 122         }
 123       }
 124       for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
 125     }
 126     v[i][i] = 1.0;
 127     g = rv1[i];
 128     l = i;
 129   }
 130
 131   /* accumulate the left-hand transformation */
 132   for (i = n - 1; i >= 0; i--) {
 133     l = i + 1;
 134     g = w[i];
 135     if (i < n - 1)
 136       for (j = l; j < n; j++) a[i][j] = 0.0;
 137     if (g) {
 138       g = 1.0 / g;
 139       if (i != n - 1) {
 140         for (j = l; j < n; j++) {
 141           for (s = 0.0, k = l; k < m; k++) s += a[k][i] * a[k][j];
 142           f = (s / a[i][i]) * g;
 143           for (k = i; k < m; k++) a[k][j] += f * a[k][i];
 144         }
 145       }
 146       for (j = i; j < m; j++) a[j][i] *= g;
 147     }
 148     else {
 149       for (j = i; j < m; j++) a[j][i] = 0.0;
 150     }
 151     ++a[i][i];
 152   }
 153
 154   /* diagonalize the bidiagonal form */
 155   for (k = n - 1; k >= 0; k--) {       /* loop over singular values */
 156     for (its = 0; its < 30; its++) {   /* loop over allowed iterations */
 157       flag = 1;
 158       for (l = k; l >= 0; l--) {       /* test for splitting */
 159         nm = l - 1;
 160         if (fabs(rv1[l]) + anorm == anorm) {
 161           flag = 0;
 162           break;
 163         }
 164         if (fabs(w[nm]) + anorm == anorm) break;
 165       }
 166       if (flag) {
 167         c = 0.0;
 168         s = 1.0;
 169         for (i = l; i <= k; i++) {
 170           f = s * rv1[i];
 171           if (fabs(f) + anorm != anorm) {
 172             g = w[i];
 173             h = PYTHAG(f, g);
 174             w[i] = h;
 175             if (h == 0.0) {
 176               char s[100];
 177               sprintf(s, "h = %f, f = %f, g = %f\n", h, f, g);
 178               fprintf(stdout,"%s",s);
 179             }
 180             h = 1.0 / h;
 181             c = g * h;
 182             s = (- f * h);
 183             for (j = 0; j < m; j++) {
 184               y = a[j][nm];
 185               z = a[j][i];
 186               a[j][nm] = y * c + z * s;
 187               a[j][i] = z * c - y * s;
 188             }
 189           }
 190         }
 191       }
 192       z = w[k];
 193       if (l == k) {        /* convergence */
 194         if (z < 0.0) {     /* make singular value nonnegative */
 195           w[k] = -z;
 196           for (j = 0; j < n; j++) v[j][k] = (-v[j][k]);
 197         }
 198                 sort_sv(m, n, k, a, w, v);
 199         break;
 200       }
 201       if (its >= 30) {
 202         free_vector(rv1);
 203         return(FALSE);     /* return an error flag */
 204       }
 205
 206       /* shift from bottom 2 x 2 minor */
 207       x = w[l];
 208       nm = k - 1;
 209       y = w[nm];
 210       g = rv1[nm];
 211       h = rv1[k];
 212       f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
 213       g = PYTHAG(f, 1.0);
 214       f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
 215
 216       /* next QR transformation */
 217       c = s = 1.0;
 218       for (j = l; j <= nm; j++) {
 219         i = j + 1;
 220         g = rv1[i];
 221         y = w[i];
 222         h = s * g;
 223         g = c * g;
 224         z = PYTHAG(f, h);
 225         rv1[j] = z;
 226         c = f / z;
 227         s = h / z;
 228         f = x * c + g * s;
 229         g = g * c - x * s;
 230         h = y * s;
 231         y = y * c;
 232         for (jj = 0; jj < n; jj++) {
 233           x = v[jj][j];
 234           z = v[jj][i];
 235           v[jj][j] = x * c + z * s;
 236           v[jj][i] = z * c - x * s;
 237         }
 238         z = PYTHAG(f, h);
 239         w[j] = z;
 240         if (z) {
 241           z = 1.0 / z;
 242           c = f * z;
 243           s = h * z;
 244         }
 245         f = (c * g) + (s * y);
 246         x = (c * y) - (s * g);
 247         for (jj = 0; jj < m; jj++) {
 248           y = a[jj][j];
 249           z = a[jj][i];
 250           a[jj][j] = y * c + z * s;
 251           a[jj][i] = z * c - y * s;
 252         }
 253       }
 254       rv1[l] = 0.0;
 255       rv1[k] = f;
 256       w[k] = x;
 257     }
 258   }
 259
 260   free_vector(rv1);
 261   return(TRUE);
 262 }
 263