home *** CD-ROM | disk | FTP | other *** search
/ Frozen Fish 1: Amiga / FrozenFish-Apr94.iso / bbs / alib / d3xx / d386 / xlispstat.lha / XLispStat / src2.lzh / XLisp-Stat / svdecomp.c < prev    next >
Encoding:
C/C++ Source or Header  |  1990-10-02  |  7.2 KB  |  279 lines
  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 "xlisp.h"
  9. #include "osdef.h"
  10. #ifdef ANSI
  11. #include "xlproto.h"
  12. #include "xlsproto.h"
  13. #else
  14. #include "xlfun.h"
  15. #include "xlsfun.h"
  16. #endif ANSI
  17.  
  18. #ifdef ANSI
  19. double PYTHAG(double,double);
  20. void sort_sv(int,int,int,RMatrix,RVector,RMatrix);
  21. #else
  22. double PYTHAG();
  23. void sort_sv();
  24. #endif ANSI
  25.  
  26. static double PYTHAG(a, b)
  27.     double a, b;
  28. {
  29.   double at = fabs(a), bt = fabs(b), ct, result;
  30.  
  31.   if (at > bt)       { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
  32.   else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
  33.   else result = 0.0;
  34.   return(result);
  35. }
  36.  
  37. #define SWAPD(a, b) (temp = (a), (a) = (b), (b) = temp)
  38.  
  39. static void sort_sv(m, n, k, a, w, v)
  40.     int m, n, k;
  41.     RMatrix a, v;
  42.     RVector w;
  43. {
  44.   int i, j;
  45.   double temp;
  46.   
  47.   for (i = k; (i < n - 1) && (w[i] < w[i+1]); i++) {
  48.     SWAPD(w[i], w[i+1]);
  49.     for (j = 0; j < m; j++) SWAPD(a[j][i], a[j][i+1]);
  50.     for (j = 0; j < n; j++) SWAPD(v[j][i], v[j][i+1]);
  51.   }
  52. }
  53.  
  54. static double maxarg1, maxarg2;
  55. #undef Max  /* defined in xlsdef.h JKL */
  56. #define Max(a, b) (maxarg1 = (a), maxarg2 = (b), (maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
  57. #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
  58.  
  59. svdcmp(a, m, n, w, v)
  60.     RMatrix a, v;
  61.     RVector w;
  62.     int m, n;
  63. {
  64.   int flag, i, its, j, jj, k, l, nm;
  65.   double c, f, h, s, x, y, z;
  66.   double anorm = 0.0, g = 0.0, scale = 0.0;
  67.   RVector rv1;
  68.   
  69.   if (m < n) return(FALSE);  /* flag an error if m < n */
  70.   
  71.   rv1 = rvector(n);
  72.  
  73.   /* Householder reduction to bidiagonal form */
  74.   for (i = 0; i < n; i++) {
  75.   
  76.     /* left-hand reduction */
  77.     l = i + 1;
  78.     rv1[i] = scale * g;
  79.     g = s = scale = 0.0;
  80.     if (i < m) {
  81.       for (k = i; k < m; k++) scale += fabs(a[k][i]);
  82.       if (scale) {
  83.         for (k = i; k < m; k++) {
  84.           a[k][i] /= scale;
  85.           s += a[k][i] * a[k][i];
  86.         }
  87.         f = a[i][i];
  88.         g = -SIGN(sqrt(s), f);
  89.         h = f * g - s;
  90.         a[i][i] = f - g;
  91.         if (i != n - 1) {
  92.           for (j = l; j < n; j++) {
  93.             for (s = 0.0, k = i; k < m; k++) s += a[k][i] * a[k][j];
  94.             f = s / h;
  95.             for (k = i; k < m; k++) a[k][j] += f * a[k][i];
  96.           }
  97.         }
  98.         for (k = i; k < m; k++) a[k][i] *= scale;
  99.       }
  100.     }
  101.     w[i] = scale * g;
  102.     
  103.     /* right-hand reduction */
  104.     g = s = scale = 0.0;
  105.     if (i < m && i != n - 1) {
  106.       for (k = l; k < n; k++) scale += fabs(a[i][k]);
  107.       if (scale) {
  108.         for (k = l; k < n; k++) {
  109.           a[i][k] /= scale;
  110.           s += a[i][k] * a[i][k];
  111.         }
  112.         f = a[i][l];
  113.         g = -SIGN(sqrt(s), f);
  114.         h = f * g - s;
  115.         a[i][l] = f - g;
  116.         for (k = l; k < n; k++) rv1[k] = a[i][k] / h;
  117.         if (i != m - 1) {
  118.           for (j = l; j < m; j++) {
  119.             for (s = 0.0, k = l; k < n; k++) s += a[j][k] * a[i][k];
  120.             for (k = l; k < n; k++) a[j][k] += s * rv1[k];
  121.           }
  122.         }
  123.         for (k = l; k < n; k++) a[i][k] *= scale;
  124.       }
  125.     }
  126.     anorm = Max(anorm, (fabs(w[i]) + fabs(rv1[i])));
  127.   }
  128.   
  129.   /* accumulate the right-hand transformation */
  130.   for (i = n - 1; i >= 0; i--) {
  131.     if (i < n - 1) {
  132.       if (g) {
  133.         for (j = l; j < n; j++)
  134.           v[j][i] = (a[i][j] / a[i][l]) / g;
  135.         for (j = l; j < n; j++) {
  136.           for (s = 0.0, k = l; k < n; k++) s += a[i][k] * v[k][j];
  137.           for (k = l; k < n; k++) v[k][j] += s * v[k][i];
  138.         }
  139.       }
  140.       for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
  141.     }
  142.     v[i][i] = 1.0;
  143.     g = rv1[i];
  144.     l = i;
  145.   }
  146.   
  147.   /* accumulate the left-hand transformation */
  148.   for (i = n - 1; i >= 0; i--) {
  149.     l = i + 1;
  150.     g = w[i];
  151.     if (i < n - 1) 
  152.       for (j = l; j < n; j++) a[i][j] = 0.0;
  153.     if (g) {
  154.       g = 1.0 / g;
  155.       if (i != n - 1) {
  156.         for (j = l; j < n; j++) {
  157.           for (s = 0.0, k = l; k < m; k++) s += a[k][i] * a[k][j];
  158.           f = (s / a[i][i]) * g;
  159.           for (k = i; k < m; k++) a[k][j] += f * a[k][i];
  160.         }
  161.       }
  162.       for (j = i; j < m; j++) a[j][i] *= g;
  163.     }
  164.     else {
  165.       for (j = i; j < m; j++) a[j][i] = 0.0;
  166.     }
  167.     ++a[i][i];
  168.   }
  169.  
  170.   /* diagonalize the bidiagonal form */
  171.   for (k = n - 1; k >= 0; k--) {       /* loop over singular values */
  172.     for (its = 0; its < 30; its++) {   /* loop over allowed iterations */
  173.       flag = 1;
  174.       for (l = k; l >= 0; l--) {       /* test for splitting */
  175.         nm = l - 1;
  176.         if (fabs(rv1[l]) + anorm == anorm) {
  177.           flag = 0;
  178.           break;
  179.         }
  180.         if (fabs(w[nm]) + anorm == anorm) break;
  181.       }
  182.       if (flag) {
  183.         c = 0.0;
  184.         s = 1.0;
  185.         for (i = l; i <= k; i++) {
  186.           f = s * rv1[i];
  187.           if (fabs(f) + anorm != anorm) {
  188.             g = w[i];
  189.             h = PYTHAG(f, g);
  190.             w[i] = h; 
  191.             if (h == 0.0) {
  192.               char s[100];
  193.               sprintf(s, "h = %f, f = %f, g = %f\n", f, g);
  194.               stdputstr(s);
  195.             }
  196.             h = 1.0 / h;
  197.             c = g * h;
  198.             s = (- f * h);
  199.             for (j = 0; j < m; j++) {
  200.               y = a[j][nm];
  201.               z = a[j][i];
  202.               a[j][nm] = y * c + z * s;
  203.               a[j][i] = z * c - y * s;
  204.             }
  205.           }
  206.         }
  207.       }
  208.       z = w[k];
  209.       if (l == k) {        /* convergence */
  210.         if (z < 0.0) {     /* make singular value nonnegative */
  211.           w[k] = -z;
  212.           for (j = 0; j < n; j++) v[j][k] = (-v[j][k]);
  213.         }
  214.         sort_sv(m, n, k, a, w, v);
  215.         break;
  216.       }
  217.       if (its >= 30) {
  218.         free_vector((Vector)rv1); /* cast added  JKL */
  219.         return(FALSE);     /* return an error flag */
  220.       }
  221.  
  222.       /* shift from bottom 2 x 2 minor */
  223.       x = w[l];
  224.       nm = k - 1;
  225.       y = w[nm];
  226.       g = rv1[nm];
  227.       h = rv1[k];
  228.       f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
  229.       g = PYTHAG(f, 1.0);
  230.       f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
  231.       
  232.       /* next QR transformation */
  233.       c = s = 1.0;
  234.       for (j = l; j <= nm; j++) {
  235.         i = j + 1;
  236.         g = rv1[i];
  237.         y = w[i];
  238.         h = s * g;
  239.         g = c * g;
  240.         z = PYTHAG(f, h);
  241.         rv1[j] = z;
  242.         c = f / z;
  243.         s = h / z;
  244.         f = x * c + g * s;
  245.         g = g * c - x * s;
  246.         h = y * s;
  247.         y = y * c;
  248.         for (jj = 0; jj < n; jj++) {
  249.           x = v[jj][j];
  250.           z = v[jj][i];
  251.           v[jj][j] = x * c + z * s;
  252.           v[jj][i] = z * c - x * s;
  253.         }
  254.         z = PYTHAG(f, h);
  255.         w[j] = z;
  256.         if (z) {
  257.           z = 1.0 / z;
  258.           c = f * z;
  259.           s = h * z;
  260.         }
  261.         f = (c * g) + (s * y);
  262.         x = (c * y) - (s * g);
  263.         for (jj = 0; jj < m; jj++) {
  264.           y = a[jj][j];
  265.           z = a[jj][i];
  266.           a[jj][j] = y * c + z * s;
  267.           a[jj][i] = z * c - y * s;
  268.         }
  269.       }
  270.       rv1[l] = 0.0;
  271.       rv1[k] = f;
  272.       w[k] = x;
  273.     }
  274.   }
  275.   free_vector((Vector)rv1); /* cast added  JKL */
  276.   return(TRUE);
  277. }
  278.  
  279.