home *** CD-ROM | disk | FTP | other *** search
/ vis-ftp.cs.umass.edu / vis-ftp.cs.umass.edu.tar / vis-ftp.cs.umass.edu / pub / Software / ASCENDER / ascendMar8.tar / UMass / Triangulate / svalue.c < prev    next >
C/C++ Source or Header  |  1995-04-13  |  12KB  |  547 lines
  1. /* @(#)svalue.c    1.10 12/20/94 */
  2. /*>>
  3.  Singular value decomposition routines taken from the book
  4. " Numerical Recipes in C " (Cambridge University Press 1988)
  5. <<*/
  6.  
  7. #include "cvar.h"
  8. #include <math.h>
  9. #include "rh_util.h"
  10. #include "alloc.h"
  11. #include "array_utils.s"
  12. #include "error_message.s"
  13.  
  14. /*>> 
  15. Singular value decomposition routines.
  16. <<*/
  17.  
  18. #define NUMITER 30
  19.  
  20. FUNCTION_DEF ( void svbksb, (
  21.         double **u,
  22.         double w[],
  23.         double **v,
  24.         int m,
  25.         int n,
  26.         double b[],
  27.         double x[]))
  28.  
  29.    /* Solves A.X = B for a vector X, where A is specified by the arrays 
  30.       u[1..m][1..n], w[1..n], v[1..n][1..n] as returned by svdcmp.
  31.       m and n are dimensions of A, and will be equal for square matrices.
  32.       b[1..m] is the input right-hand side.
  33.       x[1..n] is the output solution vector.
  34.       No input quantities are destroyed, so the routine may be called 
  35.       sequentially with different b's.
  36.    */
  37.  
  38.    {
  39.    int jj, j, i;
  40.    double s, *tmp;
  41.  
  42.    tmp = VECTOR(1,n,double);
  43.    for (j=1; j<=n; j++)
  44.       {
  45.       /* Calculate U^T.B */
  46.       s = 0.0;
  47.       if (w[j])        /* Nonzero result only if w[j] is non-zero. */
  48.      {
  49.      for (i=1; i<=m; i++)
  50.         s += u[i][j] * b[i];
  51.      s /= w[j];
  52.      }
  53.       tmp[j] = s;
  54.       }
  55.  
  56.    /* Matrix multiply by V to get answer */
  57.    for (j=1; j<=n; j++)
  58.       {
  59.       s = 0.0;
  60.       for (jj=1; jj<=n; jj++)
  61.      s += v[j][jj] * tmp[jj];
  62.       x[j] = s;
  63.       }
  64.  
  65.    /* Free temporaries */
  66.    FREE (tmp, 1);
  67.    }
  68.  
  69. static double at, bt, ct;
  70. /* Pythag computes (sqrt(a*a + b*b)) without destructive over or under flow */
  71. #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ?    \
  72.     (ct=bt/at, at*sqrt(1.0+ct*ct)) :        \
  73.     (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)):0.0))
  74.  
  75. static double maxarg1, maxarg2;
  76. #define FloatMAX(a,b) (maxarg1=(a), maxarg2=(b), (maxarg1) > (maxarg2) ? \
  77.     (maxarg1) : (maxarg2))
  78. #define SIGN(a,b)    ((b) >= 0.0 ? fabs(a) : -fabs(a))
  79.  
  80.  
  81. FUNCTION_DEF ( int svdcmp, (
  82.         double **a,
  83.         int m,
  84.         int n,
  85.         double *w,
  86.         double **v))
  87.  
  88.    /* Given a matrix a[1..m][1..n], this routine computes its singular value
  89.       decomposition, A = U.W.V^T.  The matrix U replaces a on output.  
  90.       The diagonal matrix of singular values is output as a vector w[1..n].
  91.       The matrix V (not the transpose V^T) is output as v[1..n][1..n].
  92.       m must be greater or equal to n;  If it is smaller, then a should
  93.       be filled up to square with zero rows.
  94.  
  95.       The routine returns 1 on success, 0 on failure.  In case of failure, 
  96.       a good thing to do is to permute the rows and try again.
  97.    */
  98.  
  99.    {
  100.    int flag, i, its, j, jj, k, l, nm;
  101.    double c, f, h, s, x, y, z;
  102.    double anorm = 0.0, g = 0.0, scale = 0.0;
  103.    double *rv1;
  104.    int returnval = 1;
  105.  
  106.    /* Check the bounds of the array */
  107.    if (m < n)
  108.       {
  109.       error_message ("SVDCMP : matrix has fewer rows (%d) than columns (%d)",
  110.          m, n);
  111.       bail_out (1);
  112.       }  
  113.  
  114.    /* Take a temporary array */
  115.    rv1 = VECTOR(1,n,double);
  116.  
  117.    /* Householder reduction to bidiagonal form */
  118.    for (i=1; i<=n; i++)
  119.       {
  120.       l = i+1;
  121.       rv1[i] = scale*g;
  122.       g = s = scale = 0.0;
  123.       if (i <= m)
  124.      {
  125.      for (k=i; k<=m; k++) scale += fabs(a[k][i]);
  126.      if (scale)
  127.         {
  128.         for (k=i; k<=m; k++)
  129.            {
  130.            a[k][i] /= scale;
  131.            s += a[k][i] * a[k][i];
  132.            }
  133.         f = a[i][i];
  134.         g = - SIGN(sqrt(s), f);
  135.         h = f*g-s;
  136.             a[i][i] = f-g;
  137.         if (i != n)
  138.            {
  139.            for (j=l; j<=n; j++)
  140.           {
  141.             for (s=0.0, k=i; k<=m; k++) s += a[k][i] * a[k][j];
  142.           f = s/h;
  143.           for (k=i; k<=m; k++) a[k][j] += f*a[k][i];
  144.           }
  145.            }
  146.         for (k=i; k<=m; k++) a[k][i] *= scale;
  147.         }
  148.      }
  149.      
  150.       w[i] = scale*g;
  151.       g = s = scale = 0.0;
  152.       if (i<=m && i != n)
  153.      {
  154.      for (k=l; k<=n;k++) scale += fabs (a[i][k]);
  155.      if (scale)
  156.         {
  157.         for (k=l; k<=n; k++) 
  158.            {
  159.            a[i][k] /= scale;
  160.            s += a[i][k] * a[i][k];
  161.            }
  162.         f = a[i][l];
  163.         g = -SIGN(sqrt(s),f);
  164.         h = f*g-s;
  165.         a[i][l] = f-g;
  166.         for (k=l; k<=n;k++)
  167.            rv1[k] = a[i][k]/h;
  168.         if (i != m)
  169.            {
  170.            for (j=l; j<=m; j++)
  171.           {
  172.           for (s=0.0, k=l; k<=n; k++) s += a[j][k] * a[i][k];
  173.           for (k=l; k<=n;k++) a[j][k] += s*rv1[k];
  174.           }
  175.            }
  176.         for (k=l; k<=n; k++) a[i][k] *= scale;
  177.         }
  178.      }
  179.       anorm = FloatMAX(anorm, (fabs(w[i]) + fabs(rv1[i])));
  180.       }
  181.  
  182.    /* Accumulation of right-hand transformations */
  183.    for (i=n; i>=1; i--)
  184.       {
  185.       if (i<n)
  186.      {
  187.      if (g)
  188.         {
  189.         for (j=l; j<=n; j++)
  190.            v[j][i] = (a[i][j] / a[i][l]) / g;
  191.         for (j=l; j<=n; j++)
  192.            {
  193.            for (s=0.0, k=l; k<=n; k++) s += a[i][k] * v[k][j];
  194.            for (k=l; k<=n; k++) v[k][j] += s*v[k][i];
  195.            }
  196.         }
  197.      for (j=l; j<=n; j++) v[i][j] = v[j][i] = 0.0;
  198.      }
  199.       v[i][i] = 1.0;
  200.       g = rv1[i];
  201.       l = i;
  202.       }
  203.  
  204.    /* Accumulation of left-hand transformations. */
  205.    for (i=n; i>=1; i--)
  206.       {
  207.       l = i+1;
  208.       g = w[i];
  209.       if (i < n )
  210.      for (j=l; j<=n; j++) a[i][j]= 0.0;
  211.       if (g) 
  212.      {
  213.      g = 1.0/g;
  214.      if (i != n)
  215.         {
  216.         for (j=l; j<=n; j++)
  217.            {
  218.            for (s = 0.0, k=l; k<=m; k++) s += a[k][i]*a[k][j];
  219.            f = (s/a[i][i]) * g;
  220.            for (k=i; k<=m; k++) a[k][j] += f * a[k][i];
  221.            }
  222.         }
  223.      for (j=i; j<=m; j++) a[j][i] *= g;
  224.      }
  225.       else
  226.      {
  227.      for (j=i; j<=m; j++) a[j][i] = 0.0;
  228.      }
  229.       ++a[i][i];
  230.       }
  231.  
  232.    /* diagonalization of the bidiagonal form */
  233.    for (k=n; k>=1; k--)            /* Loop over singular values */
  234.       {
  235.       for (its = 1; its <=NUMITER; its++)    /* Loop over allowed iterations */
  236.      {
  237.      flag = 1;
  238.      for (l=k; l>=1; l--)        /* Test for splitting */
  239.         {
  240.         double rv_plus_anorm, m_plus_anorm;
  241.  
  242.         nm = l-1;            /* Note that rv1[1] is always zero */
  243.  
  244.         /* Bug fix, RIH : Change of the convergence criteria */
  245.         rv_plus_anorm = (double)(fabs(rv1[l]) + anorm);
  246.         if ((rv_plus_anorm == anorm) 
  247.         || ((its==NUMITER) && (rv_plus_anorm <= 1.00001 * anorm))) 
  248.            {
  249.            flag = 0;
  250.            break;
  251.            }
  252.  
  253.         m_plus_anorm = (double)(fabs(w[nm]) + anorm);
  254.         if ((m_plus_anorm == anorm) ||
  255.          ((its==NUMITER) && (m_plus_anorm <= 1.00001 * anorm)))
  256.            break; /* Bug fix, RIH */
  257.         }
  258.      if (flag)
  259.         {
  260.         c = 0.0;
  261.         s = 1.0;
  262.         for (i=l; i<=k; i++)
  263.            {
  264.            f = s*rv1[i];
  265.            if ((double)(fabs(f) + anorm) != anorm) /* Bug fix, RIH */
  266.           {
  267.           g = w[i];
  268.           h = PYTHAG(f, g);
  269.           w[i] = h;
  270.           h = 1.0/h;
  271.           c = g*h;
  272.           s = (-f*h);
  273.           for (j=1; j <=m; j++)
  274.              {
  275.                y = a[j][nm];
  276.              z = a[j][i];
  277.              a[j][nm] = y*c + z*s;
  278.              a[j][i] = z*c - y*s;
  279.              }
  280.           }
  281.            }
  282.         }
  283.  
  284.      z = w[k];
  285.      if ( l == k)
  286.         {
  287.         /* Convergence */
  288.         if (z < 0.0)
  289.            {
  290.            /* Singular value is made non-negative */
  291.            w[k] = -z;
  292.            for (j = 1; j<=n; j++) v[j][k] = (-v[j][k]);
  293.            }
  294.         break;
  295.         }
  296.      if (its == NUMITER) 
  297.        {
  298.        returnval = 0;
  299.        goto cleanup;
  300.        }
  301.      x = w[l];        /* Shift from bottom 2-by-2 minor */
  302.      nm = k-1;
  303.      y = w[nm];
  304.      g = rv1[nm];
  305.      h = rv1[k];
  306.      f = ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
  307.      g = PYTHAG(f, 1.0);
  308.      f = ((x-z)*(x+z) + h*((y/(f+SIGN(g,f)))-h))/x;
  309.  
  310.      /* Next QR transformation */
  311.      c = s = 1.0;
  312.      for (j=l; j<=nm; j++)
  313.         {
  314.         i = j+1;
  315.         g = rv1[i];
  316.         y = w[i];
  317.         h = s*g;
  318.         g = c*g;
  319.         z = PYTHAG(f,h);
  320.         rv1[j] = z;
  321.         c = f/z;
  322.         s = h/z;
  323.         f = x*c + g*s;
  324.         g = g*c - x*s;
  325.         h = y*s;
  326.         y = y*c;
  327.         for (jj = 1; jj <= n; jj++)
  328.            {
  329.            x = v[jj][j];
  330.            z = v[jj][i];
  331.            v[jj][j] = x*c + z*s;
  332.            v[jj][i] = z*c - x*s;
  333.            }
  334.         z = PYTHAG(f,h);
  335.         w[j] = z;
  336.         if (z)
  337.            {
  338.            z = 1.0 / z;
  339.            c = f*z;
  340.            s = h*z;
  341.            }
  342.         f = (c*g) + (s*y);
  343.         x = (c*y) - (s*g);
  344.         for (jj=1; jj<=m; jj++)
  345.            {
  346.            y = a[jj][j];
  347.            z = a[jj][i];
  348.            a[jj][j] = y*c + z*s;
  349.            a[jj][i] = z*c - y*s;
  350.            }
  351.         }
  352.      rv1[l] = 0.0;
  353.      rv1[k] = f;
  354.      w[k] = x;
  355.      }
  356.       }
  357.  
  358.    /* Free the temporaries */
  359. cleanup: 
  360.    FREE (rv1, 1);
  361.  
  362.    /* Return success */
  363.    return (returnval);
  364.    }
  365.  
  366. FUNCTION_DEF ( int solve, (
  367.         double **a,
  368.         double *x,
  369.         double *b,
  370.         int nrow,
  371.         int ncol))
  372.    {
  373.    /* Find the least-squares solution to an over-constrained linear system.
  374.     * The inputs are 
  375.     *    a[1..nrow][1..ncol]
  376.     *    b[1..nrow]
  377.     * The output is 
  378.     *    x[1..ncol]
  379.     *
  380.     * The routine returns 1 on success, 0 on failure.  It fails if the SVD
  381.     * fails.
  382.     */
  383.    int i, j;
  384.    double *w, **v, **u, *w2;
  385.    double besterror = HUGE;
  386.    double bestthreshold = 0.0;
  387.    int returnval = 1;
  388.    
  389.    /* Allocate matrices */
  390.    u = MATRIX (1, nrow, 1, ncol, double);
  391.    w = VECTOR (1, ncol, double);
  392.    w2 = VECTOR (1, ncol, double);
  393.    v = MATRIX (1, ncol, 1, ncol, double);
  394.  
  395.    /* Copy a to u */
  396.    for (i=1; i<=nrow; i++)
  397.       for (j=1; j<=ncol; j++)
  398.         u[i][j] = a[i][j];
  399.  
  400.    /* Now do the singular value decomposition */
  401.    if (! svdcmp(u, nrow, ncol, w, v)) 
  402.       {
  403.       returnval = 0;
  404.       goto cleanup;
  405.       }
  406.  
  407.    /* Go into a loop of back-substitution, to find the best */
  408.    for (j=1; j<=ncol; j++)
  409.       {
  410.       /* Threshold on the i-th singular value */
  411.       double wmin = rhAbs(w[j]);
  412.  
  413.       /* Set near-zero values to zero */
  414.       for (i=1; i<=ncol; i++)
  415.      {
  416.          if ((rhAbs(w[i]) < wmin)) w2[i] = 0.0;
  417.          else w2[i] = w[i];
  418.      }
  419.  
  420.       /* Back substitute */
  421.       svbksb (u, w2, v, nrow, ncol, b, x); 
  422.  
  423.       /* Calculate the difference */
  424.          {
  425.          int i, k;
  426.      double error = 0.0;
  427.          for (i=1; i<=nrow; i++)
  428.         {
  429.         double bt = 0.0;
  430.         for (k=1; k<=ncol; k++) bt += a[i][k] * x[k];
  431.         error += (bt-b[i]) * (bt-b[i]);
  432.         }
  433.  
  434.      /* If this is better than the best error, then record */
  435.      if (error <= besterror)
  436.         {
  437.         besterror = error;
  438.         bestthreshold = wmin;
  439.         }
  440.          }
  441.       }
  442.  
  443.    /* Repeat the calculation with the best values */
  444.       {
  445.       double wmin = bestthreshold;
  446.    
  447.       /* Set near-zero values to zero */
  448.       for (i=1; i<=ncol; i++)
  449.      {
  450.          if ((rhAbs(w[i]) < wmin)) w2[i] = 0.0;
  451.          else w2[i] = w[i];
  452.      }
  453.  
  454.       /* Back substitute */
  455.       svbksb (u, w2, v, nrow, ncol, b, x); 
  456.       }
  457.  
  458.    /* Free the temporaries */
  459. cleanup: 
  460.    FREE (u, 1);
  461.    FREE (w, 1);
  462.    FREE (w2, 1);
  463.    FREE (v, 1);
  464.  
  465.    /* return success */
  466.    return (returnval);
  467.    }
  468.  
  469. #define NEAR_ZERO 1.0e-12
  470.  
  471. FUNCTION_DEF ( int solve_simple, (
  472.         double **a,
  473.         double *x,
  474.         double *b,
  475.         int nrow,
  476.         int ncol))
  477.    {
  478.    /* Find the least-squares solution to an over-constrained linear system.
  479.     * The inputs are 
  480.     *    a[1..nrow][1..ncol]
  481.     *    b[1..nrow]
  482.     * The output is 
  483.     *    x[1..ncol]
  484.     * It is allowable for x and b to be the same vector.
  485.     *
  486.     * The routine returns 1 on success, 0 on failure.  It fails if the SVD
  487.     * fails.
  488.     */
  489.    int i, j;
  490.    double *w, **v, **u, *w2;
  491.    double wmax;
  492.    int returnval = 1;
  493.    
  494.    /* Allocate matrices */
  495.    u = MATRIX (1, nrow, 1, ncol, double);
  496.    w = VECTOR (1, ncol, double);
  497.    w2 = VECTOR (1, ncol, double);
  498.    v = MATRIX (1, ncol, 1, ncol, double);
  499.  
  500.    /* Copy a to u */
  501.    for (i=1; i<=nrow; i++)
  502.       for (j=1; j<=ncol; j++)
  503.         u[i][j] = a[i][j];
  504.  
  505.    /* Now do the singular value decomposition */
  506.    if (! svdcmp(u, nrow, ncol, w, v)) 
  507.       {
  508.       returnval = 0;
  509.       goto cleanup;
  510.       }
  511.  
  512.    /* Find the maximum singular value */
  513.    wmax = -1.0;
  514.    for (j=1; j<=ncol; j++)
  515.       {
  516.       /* Threshold on the i-th singular value */
  517.       double wcurr = rhAbs(w[j]);
  518.       if (wcurr > wmax) wmax = wcurr;
  519.       }
  520.  
  521.    /* Repeat the calculation with the best values */
  522.       {
  523.       double wmin = wmax * NEAR_ZERO;
  524.    
  525.       /* Set near-zero values to zero */
  526.       for (i=1; i<=ncol; i++)
  527.      {
  528.          if ((rhAbs(w[i]) < wmin)) w2[i] = 0.0;
  529.          else w2[i] = w[i];
  530.      }
  531.  
  532.       /* Back substitute */
  533.       svbksb (u, w2, v, nrow, ncol, b, x); 
  534.       }
  535.  
  536.    /* Free the temporaries */
  537. cleanup: 
  538.    FREE (u, 1);
  539.    FREE (w, 1);
  540.    FREE (w2, 1);
  541.    FREE (v, 1);
  542.  
  543.    /* return success */
  544.    return (returnval);
  545.    }
  546.  
  547.