home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / svd < prev    next >
Encoding:
Text File  |  1994-01-13  |  9.7 KB  |  401 lines
  1.  
  2. /**************************************************************************
  3. **
  4. ** Copyright (C) 1993 David E. Steward & Zbigniew Leyk, all rights reserved.
  5. **
  6. **                 Meschach Library
  7. ** 
  8. ** This Meschach Library is provided "as is" without any express 
  9. ** or implied warranty of any kind with respect to this software. 
  10. ** In particular the authors shall not be liable for any direct, 
  11. ** indirect, special, incidental or consequential damages arising 
  12. ** in any way from use of the software.
  13. ** 
  14. ** Everyone is granted permission to copy, modify and redistribute this
  15. ** Meschach Library, provided:
  16. **  1.  All copies contain this copyright notice.
  17. **  2.  All modified copies shall carry a notice stating who
  18. **      made the last modification and the date of such modification.
  19. **  3.  No charge is made for this software or works derived from it.  
  20. **      This clause shall not be construed as constraining other software
  21. **      distributed on the same medium as this software, nor is a
  22. **      distribution fee considered a charge.
  23. **
  24. ***************************************************************************/
  25.  
  26.  
  27. /*
  28.     File containing routines for computing the SVD of matrices
  29. */
  30.  
  31. #include    <stdio.h>
  32. #include    <math.h>
  33. #include    "matrix.h"
  34. #include        "matrix2.h"
  35.  
  36.  
  37. static char rcsid[] = "$Id: svd.c,v 1.6 1994/01/13 05:44:16 des Exp $";
  38.  
  39.  
  40.  
  41. #define    sgn(x)    ((x) >= 0 ? 1 : -1)
  42. #define    MAX_STACK    100
  43.  
  44. /* fixsvd -- fix minor details about SVD
  45.     -- make singular values non-negative
  46.     -- sort singular values in decreasing order
  47.     -- variables as for bisvd()
  48.     -- no argument checking */
  49. static void    fixsvd(d,U,V)
  50. VEC    *d;
  51. MAT    *U, *V;
  52. {
  53.     int        i, j, k, l, r, stack[MAX_STACK], sp;
  54.     Real    tmp, v;
  55.  
  56.     /* make singular values non-negative */
  57.     for ( i = 0; i < d->dim; i++ )
  58.     if ( d->ve[i] < 0.0 )
  59.     {
  60.         d->ve[i] = - d->ve[i];
  61.         if ( U != MNULL )
  62.         for ( j = 0; j < U->m; j++ )
  63.             U->me[i][j] = - U->me[i][j];
  64.     }
  65.  
  66.     /* sort singular values */
  67.     /* nonrecursive implementation of quicksort due to R.Sedgewick,
  68.        "Algorithms in C", p. 122 (1990) */
  69.     sp = -1;
  70.     l = 0;    r = d->dim - 1;
  71.     for ( ; ; )
  72.     {
  73.     while ( r > l )
  74.     {
  75.         /* i = partition(d->ve,l,r) */
  76.         v = d->ve[r];
  77.  
  78.         i = l - 1;        j = r;
  79.         for ( ; ; )
  80.         {    /* inequalities are "backwards" for **decreasing** order */
  81.         while ( d->ve[++i] > v )
  82.             ;
  83.         while ( d->ve[--j] < v )
  84.             ;
  85.         if ( i >= j )
  86.             break;
  87.         /* swap entries in d->ve */
  88.         tmp = d->ve[i];      d->ve[i] = d->ve[j];    d->ve[j] = tmp;
  89.         /* swap rows of U & V as well */
  90.         if ( U != MNULL )
  91.             for ( k = 0; k < U->n; k++ )
  92.             {
  93.             tmp = U->me[i][k];
  94.             U->me[i][k] = U->me[j][k];
  95.             U->me[j][k] = tmp;
  96.             }
  97.         if ( V != MNULL )
  98.             for ( k = 0; k < V->n; k++ )
  99.             {
  100.             tmp = V->me[i][k];
  101.             V->me[i][k] = V->me[j][k];
  102.             V->me[j][k] = tmp;
  103.             }
  104.         }
  105.         tmp = d->ve[i];    d->ve[i] = d->ve[r];    d->ve[r] = tmp;
  106.         if ( U != MNULL )
  107.         for ( k = 0; k < U->n; k++ )
  108.         {
  109.             tmp = U->me[i][k];
  110.             U->me[i][k] = U->me[r][k];
  111.             U->me[r][k] = tmp;
  112.         }
  113.         if ( V != MNULL )
  114.         for ( k = 0; k < V->n; k++ )
  115.         {
  116.             tmp = V->me[i][k];
  117.             V->me[i][k] = V->me[r][k];
  118.             V->me[r][k] = tmp;
  119.         }
  120.         /* end i = partition(...) */
  121.         if ( i - l > r - i )
  122.         {    stack[++sp] = l;    stack[++sp] = i-1;    l = i+1;    }
  123.         else
  124.         {    stack[++sp] = i+1;  stack[++sp] = r;    r = i-1;    }
  125.     }
  126.     if ( sp < 0 )
  127.         break;
  128.     r = stack[sp--];    l = stack[sp--];
  129.     }
  130. }
  131.  
  132.  
  133. /* bisvd -- svd of a bidiagonal m x n matrix represented by d (diagonal) and
  134.             f (super-diagonals)
  135.     -- returns with d set to the singular values, f zeroed
  136.     -- if U, V non-NULL, the orthogonal operations are accumulated
  137.         in U, V; if U, V == I on entry, then SVD == U^T.A.V
  138.         where A is initial matrix
  139.     -- returns d on exit */
  140. VEC    *bisvd(d,f,U,V)
  141. VEC    *d, *f;
  142. MAT    *U, *V;
  143. {
  144.     int    i, j, n;
  145.     int    i_min, i_max, split;
  146.     Real    c, s, shift, size, z;
  147.     Real    d_tmp, diff, t11, t12, t22, *d_ve, *f_ve;
  148.  
  149.     if ( ! d || ! f )
  150.         error(E_NULL,"bisvd");
  151.     if ( d->dim != f->dim + 1 )
  152.         error(E_SIZES,"bisvd");
  153.     n = d->dim;
  154.     if ( ( U && U->n < n ) || ( V && V->m < n ) )
  155.         error(E_SIZES,"bisvd");
  156.     if ( ( U && U->m != U->n ) || ( V && V->m != V->n ) )
  157.         error(E_SQUARE,"bisvd");
  158.  
  159.  
  160.     if ( n == 1 )
  161.         return d;
  162.     d_ve = d->ve;    f_ve = f->ve;
  163.  
  164.     size = v_norm_inf(d) + v_norm_inf(f);
  165.  
  166.     i_min = 0;
  167.     while ( i_min < n )    /* outer while loop */
  168.     {
  169.         /* find i_max to suit;
  170.         submatrix i_min..i_max should be irreducible */
  171.         i_max = n - 1;
  172.         for ( i = i_min; i < n - 1; i++ )
  173.         if ( d_ve[i] == 0.0 || f_ve[i] == 0.0 )
  174.         {   i_max = i;
  175.             if ( f_ve[i] != 0.0 )
  176.             {
  177.             /* have to ``chase'' f[i] element out of matrix */
  178.             z = f_ve[i];    f_ve[i] = 0.0;
  179.             for ( j = i; j < n-1 && z != 0.0; j++ )
  180.             {
  181.                 givens(d_ve[j+1],z, &c, &s);
  182.                 s = -s;
  183.                 d_ve[j+1] =  c*d_ve[j+1] - s*z;
  184.                 if ( j+1 < n-1 )
  185.                 {
  186.                 z         = s*f_ve[j+1];
  187.                 f_ve[j+1] = c*f_ve[j+1];
  188.                 }
  189.                 if ( U )
  190.                 rot_rows(U,i,j+1,c,s,U);
  191.             }
  192.             }
  193.             break;
  194.         }
  195.         if ( i_max <= i_min )
  196.         {
  197.         i_min = i_max + 1;
  198.         continue;
  199.         }
  200.         /* printf("bisvd: i_min = %d, i_max = %d\n",i_min,i_max); */
  201.  
  202.         split = FALSE;
  203.         while ( ! split )
  204.         {
  205.         /* compute shift */
  206.         t11 = d_ve[i_max-1]*d_ve[i_max-1] +
  207.             (i_max > i_min+1 ? f_ve[i_max-2]*f_ve[i_max-2] : 0.0);
  208.         t12 = d_ve[i_max-1]*f_ve[i_max-1];
  209.         t22 = d_ve[i_max]*d_ve[i_max] + f_ve[i_max-1]*f_ve[i_max-1];
  210.         /* use e-val of [[t11,t12],[t12,t22]] matrix
  211.                 closest to t22 */
  212.         diff = (t11-t22)/2;
  213.         shift = t22 - t12*t12/(diff +
  214.             sgn(diff)*sqrt(diff*diff+t12*t12));
  215.  
  216.         /* initial Givens' rotation */
  217.         givens(d_ve[i_min]*d_ve[i_min]-shift,
  218.             d_ve[i_min]*f_ve[i_min], &c, &s);
  219.  
  220.         /* do initial Givens' rotations */
  221.         d_tmp         = c*d_ve[i_min] + s*f_ve[i_min];
  222.         f_ve[i_min]   = c*f_ve[i_min] - s*d_ve[i_min];
  223.         d_ve[i_min]   = d_tmp;
  224.         z             = s*d_ve[i_min+1];
  225.         d_ve[i_min+1] = c*d_ve[i_min+1];
  226.         if ( V )
  227.             rot_rows(V,i_min,i_min+1,c,s,V);
  228.         /* 2nd Givens' rotation */
  229.         givens(d_ve[i_min],z, &c, &s);
  230.         d_ve[i_min]   = c*d_ve[i_min] + s*z;
  231.         d_tmp         = c*d_ve[i_min+1] - s*f_ve[i_min];
  232.         f_ve[i_min]   = s*d_ve[i_min+1] + c*f_ve[i_min];
  233.         d_ve[i_min+1] = d_tmp;
  234.         if ( i_min+1 < i_max )
  235.         {
  236.             z             = s*f_ve[i_min+1];
  237.             f_ve[i_min+1] = c*f_ve[i_min+1];
  238.         }
  239.         if ( U )
  240.             rot_rows(U,i_min,i_min+1,c,s,U);
  241.  
  242.         for ( i = i_min+1; i < i_max; i++ )
  243.         {
  244.             /* get Givens' rotation for zeroing z */
  245.             givens(f_ve[i-1],z, &c, &s);
  246.             f_ve[i-1] = c*f_ve[i-1] + s*z;
  247.             d_tmp     = c*d_ve[i] + s*f_ve[i];
  248.             f_ve[i]   = c*f_ve[i] - s*d_ve[i];
  249.             d_ve[i]   = d_tmp;
  250.             z         = s*d_ve[i+1];
  251.             d_ve[i+1] = c*d_ve[i+1];
  252.             if ( V )
  253.             rot_rows(V,i,i+1,c,s,V);
  254.             /* get 2nd Givens' rotation */
  255.             givens(d_ve[i],z, &c, &s);
  256.             d_ve[i]   = c*d_ve[i] + s*z;
  257.             d_tmp     = c*d_ve[i+1] - s*f_ve[i];
  258.             f_ve[i]   = c*f_ve[i] + s*d_ve[i+1];
  259.             d_ve[i+1] = d_tmp;
  260.             if ( i+1 < i_max )
  261.             {
  262.             z         = s*f_ve[i+1];
  263.             f_ve[i+1] = c*f_ve[i+1];
  264.             }
  265.             if ( U )
  266.             rot_rows(U,i,i+1,c,s,U);
  267.         }
  268.         /* should matrix be split? */
  269.         for ( i = i_min; i < i_max; i++ )
  270.             if ( fabs(f_ve[i]) <
  271.                 MACHEPS*(fabs(d_ve[i])+fabs(d_ve[i+1])) )
  272.             {
  273.             split = TRUE;
  274.             f_ve[i] = 0.0;
  275.             }
  276.             else if ( fabs(d_ve[i]) < MACHEPS*size )
  277.             {
  278.             split = TRUE;
  279.             d_ve[i] = 0.0;
  280.             }
  281.             /* printf("bisvd: d =\n");    v_output(d); */
  282.             /* printf("bisvd: f = \n");    v_output(f); */
  283.         }
  284.     }
  285.     fixsvd(d,U,V);
  286.  
  287.     return d;
  288. }
  289.  
  290. /* bifactor -- perform preliminary factorisation for bisvd
  291.     -- updates U and/or V, which ever is not NULL */
  292. MAT    *bifactor(A,U,V)
  293. MAT    *A, *U, *V;
  294. {
  295.     int    k;
  296.     static VEC    *tmp1=VNULL, *tmp2=VNULL;
  297.     Real    beta;
  298.  
  299.     if ( ! A )
  300.         error(E_NULL,"bifactor");
  301.     if ( ( U && ( U->m != U->n ) ) || ( V && ( V->m != V->n ) ) )
  302.         error(E_SQUARE,"bifactor");
  303.     if ( ( U && U->m != A->m ) || ( V && V->m != A->n ) )
  304.         error(E_SIZES,"bifactor");
  305.     tmp1 = v_resize(tmp1,A->m);
  306.     tmp2 = v_resize(tmp2,A->n);
  307.     MEM_STAT_REG(tmp1,TYPE_VEC);
  308.     MEM_STAT_REG(tmp2,TYPE_VEC);
  309.  
  310.     if ( A->m >= A->n )
  311.         for ( k = 0; k < A->n; k++ )
  312.         {
  313.         get_col(A,k,tmp1);
  314.         hhvec(tmp1,k,&beta,tmp1,&(A->me[k][k]));
  315.         hhtrcols(A,k,k+1,tmp1,beta);
  316.         if ( U )
  317.             hhtrcols(U,k,0,tmp1,beta);
  318.         if ( k+1 >= A->n )
  319.             continue;
  320.         get_row(A,k,tmp2);
  321.         hhvec(tmp2,k+1,&beta,tmp2,&(A->me[k][k+1]));
  322.         hhtrrows(A,k+1,k+1,tmp2,beta);
  323.         if ( V )
  324.             hhtrcols(V,k+1,0,tmp2,beta);
  325.         }
  326.     else
  327.         for ( k = 0; k < A->m; k++ )
  328.         {
  329.         get_row(A,k,tmp2);
  330.         hhvec(tmp2,k,&beta,tmp2,&(A->me[k][k]));
  331.         hhtrrows(A,k+1,k,tmp2,beta);
  332.         if ( V )
  333.             hhtrcols(V,k,0,tmp2,beta);
  334.         if ( k+1 >= A->m )
  335.             continue;
  336.         get_col(A,k,tmp1);
  337.         hhvec(tmp1,k+1,&beta,tmp1,&(A->me[k+1][k]));
  338.         hhtrcols(A,k+1,k+1,tmp1,beta);
  339.         if ( U )
  340.             hhtrcols(U,k+1,0,tmp1,beta);
  341.         }
  342.  
  343.     return A;
  344. }
  345.  
  346. /* svd -- returns vector of singular values in d
  347.     -- also updates U and/or V, if one or the other is non-NULL
  348.     -- destroys A */
  349. VEC    *svd(A,U,V,d)
  350. MAT    *A, *U, *V;
  351. VEC    *d;
  352. {
  353.     static VEC    *f=VNULL;
  354.     int    i, limit;
  355.     MAT    *A_tmp;
  356.  
  357.     if ( ! A )
  358.         error(E_NULL,"svd");
  359.     if ( ( U && ( U->m != U->n ) ) || ( V && ( V->m != V->n ) ) )
  360.         error(E_SQUARE,"svd");
  361.     if ( ( U && U->m != A->m ) || ( V && V->m != A->n ) )
  362.         error(E_SIZES,"svd");
  363.  
  364.     A_tmp = m_copy(A,MNULL);
  365.     if ( U != MNULL )
  366.         m_ident(U);
  367.     if ( V != MNULL )
  368.         m_ident(V);
  369.     limit = min(A_tmp->m,A_tmp->n);
  370.     d = v_resize(d,limit);
  371.     f = v_resize(f,limit-1);
  372.     MEM_STAT_REG(f,TYPE_VEC);
  373.  
  374.     bifactor(A_tmp,U,V);
  375.     if ( A_tmp->m >= A_tmp->n )
  376.         for ( i = 0; i < limit; i++ )
  377.         {
  378.         d->ve[i] = A_tmp->me[i][i];
  379.         if ( i+1 < limit )
  380.             f->ve[i] = A_tmp->me[i][i+1];
  381.         }
  382.     else
  383.         for ( i = 0; i < limit; i++ )
  384.         {
  385.         d->ve[i] = A_tmp->me[i][i];
  386.         if ( i+1 < limit )
  387.             f->ve[i] = A_tmp->me[i+1][i];
  388.         }
  389.  
  390.  
  391.     if ( A_tmp->m >= A_tmp->n )
  392.         bisvd(d,f,U,V);
  393.     else
  394.         bisvd(d,f,V,U);
  395.  
  396.     M_FREE(A_tmp);
  397.  
  398.     return d;
  399. }
  400.  
  401.