home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 21 / IOPROG_21.ISO / SOFT / LIBMAT.ZIP / MATSVD.CPP < prev    next >
Encoding:
C/C++ Source or Header  |  1992-10-16  |  13.0 KB  |  432 lines
  1. /**************************************************/
  2. /*      matsvd.c source for SVD functions         */
  3. /**************************************************/
  4.  
  5.  
  6. /**************************************************/
  7. /*            MatClass Source File                */
  8. /*       Copyright of C. R. Birchenhall           */
  9. /*       University of Manchester, UK.            */
  10. /*   MatClass is freeware. This file should be    */
  11. /* made freely available to users of any software */
  12. /* whose creation is wholly or partly dependent   */
  13. /*                on this file.                   */
  14. /**************************************************/
  15.  
  16. #include "matsvd.hpp"
  17.  
  18. #include <math.h>
  19.  
  20. static DOUBLE at, bt, ct ;
  21.  
  22. DOUBLE PYTHAG( DOUBLE a, DOUBLE b )
  23. {
  24.   return
  25.     ( ( at = fabs(a) )  > ( bt = fabs(b) ) )     ?
  26.     ( ct = bt/at, at * sqrt( 1.0 + ct * ct ) )   :
  27.     ( bt ? ( ct = at/bt, bt * sqrt( 1.0 + ct * ct ) ) : 0.0 )
  28.   ;
  29. } // inline pythag
  30.  
  31. static void biDiag( matrix& a, matrix& w, matrix& rv1, REAL& anorm )
  32. {
  33.    REAL f, g = 0.0, h, r, s = 0.0, scale = 0.0 ;
  34.    INDEX m = a.nRows(), n = a.nCols(), i, j, k, l ;
  35.    anorm = 0.0 ;
  36.    for ( i = 1 ; i <= n ; i++ ) {
  37.       l = i+1;
  38.       rv1(i) = scale * g ;
  39.       g = s = scale = 0.0 ;
  40.       if ( i <= m ) {
  41.          for ( k = i ; k <= m ; k++ )
  42.             scale += fabs( a(k,i) ) ;
  43.          if ( scale ) {
  44.             for ( k = i ; k <= m ; k++ ) {
  45.                a(k,i) /= scale;
  46.                s += a(k,i) * a(k,i) ;
  47.             } // for k
  48.             f = a(i,i) ;
  49.             g = sqrt(s) ;
  50.             if ( f >= 0.0 )
  51.                g = - g ;
  52.             h = f * g - s ;
  53.             a(i,i) = f - g ;
  54.             if ( i != n ) {
  55.                for ( j = l ; j <= n ; j++ ) {
  56.                   for ( s = 0.0 , k = i ; k <= m ; k++ )
  57.                       s += a(k,i) * a(k,j) ;
  58.                   f = s / h ;
  59.                   for ( k = i ; k <= m ; k++ )
  60.                       a(k,j) += f * a(k,i) ;
  61.                } // for j
  62.             } // if i != n
  63.             for ( k = i ; k <= m ; k++ )
  64.                 a(k,i) *= scale ;
  65.          } // if scale
  66.       } // if i <= m
  67.       w(i) = scale * g ;
  68.       g = s = scale = 0.0 ;
  69.       if ( i <= m && i != n ) {
  70.          for ( k = l ; k <= n ; k++ )
  71.              scale += fabs( a(i,k) ) ;
  72.          if ( scale ) {
  73.             for ( k = l ; k <= n ; k++ ) {
  74.                a(i,k) /= scale ;
  75.                s += a(i,k) * a(i,k) ;
  76.             } // for k
  77.             f = a(i,l) ;
  78.             g = sqrt(s) ;
  79.             if ( f >= 0.0 )
  80.                g = -g ;
  81.             h = f * g - s ;
  82.             a(i,l) = f - g ;
  83.             for ( k = l ; k <= n ; k++ )
  84.                 rv1(k) = a(i,k) / h ;
  85.             if ( i != m ) {
  86.                for ( j = l ; j <= m ; j++ ) {
  87.                   for ( s = 0.0 , k = l ; k <= n ; k++ )
  88.                       s += a(j,k) * a(i,k);
  89.                   for ( k = l ; k <= n ; k++ )
  90.                       a(j,k) += s * rv1(k);
  91.                } // for j
  92.             } // if i != m
  93.             for ( k = l ; k <= n ; k++ )
  94.                 a(i,k) *= scale;
  95.          } // if scale
  96.       } // if i != m && i != n
  97.       r = fabs( w(i) ) + fabs( rv1(i) ) ;
  98.       if ( r > anorm )
  99.          anorm = r ;
  100.    } // for i
  101. } // biDiag
  102.  
  103. static void initialWV( matrix& a, matrix& w, matrix& v, matrix& rv1 )
  104. {
  105.    INDEX m = a.nRows(), n = a.nCols(), i, j, k, l ;
  106.    REAL f, g = 0.0, s ;
  107.    for ( i = n ; i >= 1 ; i-- ) {
  108.       l = i + 1 ;
  109.       if ( i < n ) {
  110.          if ( g ) {
  111.             for ( j = l ; j <= n ; j++ )
  112.                v(j,i) = ( a(i,j) / a(i,l) ) / g ;
  113.                // double division to reduce underflow
  114.             for ( j = l ; j <= n ; j++ ) {
  115.                for ( s = 0.0, k =l ; k <= n ; k++ )
  116.                   s += a(i,k) * v(k,j) ;
  117.                for ( k = l ; k <= n ; k++ )
  118.                   v(k,j) += s * v(k,i) ;
  119.             } // for j
  120.          } // if g
  121.          for ( j = l ; j <= n ; j++ )
  122.             v(i,j) = v(j,i) = 0.0 ;
  123.       } // if i < n
  124.       v(i,i) = 1.0 ;
  125.       g = rv1(i) ;
  126.    } // for i
  127.    for ( i = n ; i >= 1 ; i-- ) {
  128.       l = i + 1 ;
  129.       g = w(i) ;
  130.       if ( i < n ) {
  131.          for ( j = l ; j <= n ; j++ )
  132.             a(i,j)=0.0 ;
  133.       } // if
  134.       if ( g ) {
  135.          g = 1.0 / g ;
  136.          if ( i != n ) {
  137.             for ( j = l ; j <= n ; j++ ) {
  138.                for ( s = 0.0 , k = l ; k <= m ; k++ )
  139.                   s += a(k,i) * a(k,j);
  140.                f = ( s / a(i,i) ) * g ;
  141.                for ( k = i ; k <= m ; k++ )
  142.                   a(k,j) += f * a(k,i) ;
  143.             } // for j
  144.          } // if i != n
  145.          for ( j = i ; j <= m ; j++ )
  146.             a(j,i) *= g ;
  147.       } else {
  148.          for ( j = i ; j <= m ; j++ )
  149.             a(j,i) = 0.0 ;
  150.       } // else
  151.       a(i,i) += 1.0 ;
  152.    } // for i
  153. } // initialWV
  154.  
  155. INDEX svdcmp( matrix& a, matrix& w, matrix& v, matError& error )
  156. {
  157.    static char *mName = "svdcmp" ;
  158.    matFunc func( mName ) ; a.debugInfo( func ) ;
  159.    INDEX m = a.nRows(), n = a.nCols() ;
  160.    INDEX  flag, i, its, j, jj, k, l, nm ;
  161.    REAL   c, f, h, s, x, y, z ;
  162.    REAL   anorm=0.0, g=0.0, r ;
  163.    matrix rv1(n) ;
  164.  
  165.    error = NOERROR ;
  166.    if ( m < n ) {
  167.       error = NRANG ;
  168.       return 0 ;
  169.    } // if
  170.  
  171.    w.reset( n ) ;
  172.    v.reset( n, n ) ;
  173.  
  174.    biDiag( a, w, rv1, anorm ) ;
  175.    initialWV( a, w, v, rv1 ) ;
  176.  
  177. /* 
  178.  
  179.    for ( i = 1 ; i <= n ; i++ ) {
  180.       l = i+1;
  181.       rv1(i) = scale * g ;
  182.       g = s = scale = 0.0 ;
  183.       if ( i <= m ) {
  184.          for ( k = i ; k <= m ; k++ )
  185.             scale += fabs( a(k,i) ) ;
  186.          if ( scale ) {
  187.             for ( k = i ; k <= m ; k++ ) {
  188.                a(k,i) /= scale;
  189.                s += a(k,i) * a(k,i) ;
  190.             } // for k
  191.             f = a(i,i) ;
  192.             g = sqrt(s) ;
  193.             if ( f >= 0.0 )
  194.                g = - g ;
  195.             h = f * g - s ;
  196.             a(i,i) = f - g ;
  197.             if ( i != n ) {
  198.                for ( j = l ; j <= n ; j++ ) {
  199.                   for ( s = 0.0 , k = i ; k <= m ; k++ )
  200.                       s += a(k,i) * a(k,j) ;
  201.                   f = s / h ;
  202.                   for ( k = i ; k <= m ; k++ )
  203.                       a(k,j) += f * a(k,i) ;
  204.                } // for j
  205.             } // if i != n
  206.             for ( k = i ; k <= m ; k++ )
  207.                 a(k,i) *= scale ;
  208.          } // if scale
  209.       } // if i <= m
  210.       w(i) = scale * g ;
  211.       g = s = scale = 0.0 ;
  212.       if ( i <= m && i != n ) {
  213.          for ( k = l ; k <= n ; k++ )
  214.              scale += fabs( a(i,k) ) ;
  215.          if ( scale ) {
  216.             for ( k = l ; k <= n ; k++ ) {
  217.                a(i,k) /= scale ;
  218.                s += a(i,k) * a(i,k) ;
  219.             } // for k
  220.             f = a(i,l) ;
  221.             g = sqrt(s) ;
  222.             if ( f >= 0.0 )
  223.                g = -g ;
  224.             h = f * g - s ;
  225.             a(i,l) = f - g ;
  226.             for ( k = l ; k <= n ; k++ )
  227.                 rv1(k) = a(i,k) / h ;
  228.             if ( i != m ) {
  229.                for ( j = l ; j <= m ; j++ ) {
  230.                   for ( s = 0.0 , k = l ; k <= n ; k++ )
  231.                       s += a(j,k) * a(i,k);
  232.                   for ( k = l ; k <= n ; k++ )
  233.                       a(j,k) += s * rv1(k);
  234.                } // for j
  235.             } // if i != m
  236.             for ( k = l ; k <= n ; k++ )
  237.                 a(i,k) *= scale;
  238.          } // if scale
  239.       } // if i != m && i != n
  240.       r = fabs( w(i) ) + fabs( rv1(i) ) ;
  241.       if ( r > anorm )
  242.          anorm = r ;
  243.    } // for i
  244.    for ( i = n ; i >= 1 ; i-- ) {
  245.       l = i + 1 ;
  246.       if ( i < n ) {
  247.          if ( g ) {
  248.             for ( j = l ; j <= n ; j++ )
  249.                v(j,i) = ( a(i,j) / a(i,l) ) / g ;
  250.                // double division to reduce underflow
  251.             for ( j = l ; j <= n ; j++ ) {
  252.                for ( s = 0.0, k =l ; k <= n ; k++ )
  253.                   s += a(i,k) * v(k,j) ;
  254.                for ( k = l ; k <= n ; k++ )
  255.                   v(k,j) += s * v(k,i) ;
  256.             } // for j
  257.          } // if g
  258.          for ( j = l ; j <= n ; j++ )
  259.             v(i,j) = v(j,i) = 0.0 ;
  260.       } // if i < n
  261.       v(i,i) = 1.0 ;
  262.       g = rv1(i) ;
  263.    } // for i
  264.    for ( i = n ; i >= 1 ; i-- ) {
  265.       l = i + 1 ;
  266.       g = w(i) ;
  267.       if ( i < n ) {
  268.          for ( j = l ; j <= n ; j++ )
  269.             a(i,j)=0.0 ;
  270.       } // if
  271.       if ( g ) {
  272.          g = 1.0 / g ;
  273.          if ( i != n ) {
  274.             for ( j = l ; j <= n ; j++ ) {
  275.                for ( s = 0.0 , k = l ; k <= m ; k++ )
  276.                   s += a(k,i) * a(k,j);
  277.                f = ( s / a(i,i) ) * g ;
  278.                for ( k = i ; k <= m ; k++ )
  279.                   a(k,j) += f * a(k,i) ;
  280.             } // for j
  281.          } // if i != n
  282.          for ( j = i ; j <= m ; j++ )
  283.             a(j,i) *= g ;
  284.       } else {
  285.          for ( j = i ; j <= m ; j++ )
  286.             a(j,i) = 0.0 ;
  287.       } // else
  288.       a(i,i) += 1.0 ;
  289.    } // for i
  290.  
  291. */
  292.  
  293.    for ( k = n ; k >= 1 ; k-- ) {
  294.       for ( its = 1 ; its <= 30 ; its++ ) {
  295.          flag = 1 ;
  296.          for ( l = k ; l >= 1 ; l-- ) {
  297.             nm = l - 1 ;
  298.             if ( fabs( rv1(l) ) + anorm == anorm) {
  299.                flag = 0 ;
  300.                break;
  301.             } // if
  302.             if ( fabs( w(nm) ) + anorm == anorm )
  303.                break;
  304.          } // for l
  305.          if ( flag ) {
  306.             c = 0.0 ;
  307.             s = 1.0;
  308.             for ( i = l ; i <= k ; i++ ) {
  309.                f = s * rv1(i) ;
  310.                if ( fabs(f) + anorm != anorm ) {
  311.                   g = w(i) ;
  312.                   h = PYTHAG(f,g) ;
  313.                   w(i) = h ;
  314.                   h = 1.0 / h ;
  315.                   c = g * h ;
  316.                   s = ( -f * h ) ;
  317.                   for ( j = 1 ; j <= m ; j++ ) {
  318.                      y = a(j,nm) ;
  319.                      z = a(j,i) ;
  320.                      a(j,nm) = y * c + z * s ;
  321.                      a(j,i) = z * c - y * s ;
  322.                   } // for j
  323.                } // if fabs(f)
  324.             } // for i
  325.          } // if flag
  326.          z = w(k) ;
  327.          if ( l == k ) {
  328.             if ( z < 0.0 ) {
  329.                w(k) = -z ;
  330.                for ( j = 1 ; j <= n ; j++ )
  331.                   v(j,k) = (-v(j,k)) ;
  332.             } // if z < 0
  333.             break;
  334.          } // if l == k
  335.          if ( its == 30 ) {
  336.            error = NCONV ;
  337.            return 0 ;
  338.      } // if  
  339.          x = w(l);
  340.          nm = k - 1 ;
  341.          y = w(nm) ;
  342.          g = rv1(nm) ;
  343.          h = rv1(k) ;
  344.          f = ( (y-z)*(y+z) + (g-h)*(g+h) ) / ( 2.0 * h * y ) ;
  345.          g = PYTHAG(f,1.0) ;
  346.          r = ( f >= 0.0 ? g : - g ) ;
  347.          f= ( (x-z)*(x+z) + h * ( ( y / ( f + r ) ) - h ) ) / x ;
  348.          c = s = 1.0 ;
  349.          for ( j = l ; j <= nm ; j++ ) {
  350.             i = j + 1 ;
  351.             g = rv1(i) ;
  352.             y = w(i) ;
  353.             h = s * g ;
  354.             g = c * g ;
  355.             z = PYTHAG(f,h) ;
  356.             rv1(j) = z ;
  357.             c = f / z ;
  358.             s = h / z ;
  359.             f = x * c + g * s ;
  360.             g = g * c - x * s ;
  361.             h = y * s ;
  362.             y = y * c ;
  363.             for ( jj = 1 ; jj <= n ; jj++ ) {
  364.                x = v(jj,j) ;
  365.                z = v(jj,i);
  366.                v(jj,j) = x * c + z * s ;
  367.                v(jj,i)= z * c - x * s ;
  368.             } // for jj
  369.             z = PYTHAG(f,h) ;
  370.             w(j) = z ;
  371.             if (z) {
  372.                z = 1.0 / z ;
  373.                c = f * z ;
  374.                s = h * z ;
  375.             } // if
  376.             f = ( c * g ) + ( s * y ) ;
  377.             x = ( c * y ) - ( s * g ) ;
  378.             for ( jj = 1 ; jj <= m ; jj++ ) {
  379.                y = a(jj,j) ;
  380.                z = a(jj,i) ;
  381.                a(jj,j) = y * c + z * s ;
  382.                a(jj,i) = z * c - y * s ;
  383.             } // for jj
  384.          } // for j
  385.          rv1(l) = 0.0 ;
  386.          rv1(k) = f ;
  387.          w(k) = x ;
  388.       } // for its
  389.    } // for k
  390.    return OK ;
  391. } // svdcmp
  392.  
  393. INDEX svdBackSub( const matrix& a, const matrix& w, 
  394.                   const matrix& v, const matrix& b, 
  395.                   matrix& x, matError& error )
  396. /**************************************************************
  397.    Assumes a, w and v are svd decomp of A. Solves Ax=b.
  398.    Ignores components with zero singular values.
  399.    Overwrites x with solution.
  400. **************************************************************/
  401. {
  402.    static char *mName = "svdBackSub" ;
  403.    matFunc func( mName ) ; a.debugInfo( func ) ;
  404.    error = NOERROR ;
  405.    INDEX nc = a.nCols(), nr = a.nRows() ;
  406.    if ( nc != w.nRows() || nc != v.nRows() || 
  407.         nc != v.nCols() || nr != b.nRows() ) {
  408.       error = NEDIM ;
  409.       return 0 ;
  410.    } // if
  411.    if ( b.nCols() != 1 ) {
  412.       error = NOTVC ;
  413.       return 1 ;
  414.    } // if
  415.    // form tmp = w^-1 a'b, passing over zeros in w 
  416.    matrix tmp( nc ) ;
  417.    refMatrix m( a ) ;
  418.    REAL r ;
  419.    INDEX i ;
  420.    for ( i = 1 ; i <= nc ; i++ ) {
  421.       r = 0.0 ;
  422.       if ( w(i) != 0.0 ) {
  423.          m.refCol(i) ;
  424.          r = m.inner(b) ;
  425.      r /= w(i) ;
  426.       } // if
  427.       tmp(i) = r ;
  428.    } // if
  429.    x.multOf( v, tmp ) ;
  430.    return OK ;
  431. } // svdBackSub
  432.