home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / lufactor < prev    next >
Encoding:
Text File  |  1994-03-13  |  6.4 KB  |  269 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.     Matrix factorisation routines to work with the other matrix files.
  29. */
  30.  
  31. /* LUfactor.c 1.5 11/25/87 */
  32. static    char    rcsid[] = "$Id: lufactor.c,v 1.7 1994/03/13 23:08:25 des Exp $";
  33.  
  34. #include    <stdio.h>
  35. #include    <math.h>
  36. #include    "matrix.h"
  37. #include        "matrix2.h"
  38.  
  39.  
  40.  
  41. /* Most matrix factorisation routines are in-situ unless otherwise specified */
  42.  
  43. /* LUfactor -- gaussian elimination with scaled partial pivoting
  44.         -- Note: returns LU matrix which is A */
  45. MAT    *LUfactor(A,pivot)
  46. MAT    *A;
  47. PERM    *pivot;
  48. {
  49.     u_int    i, j, k, k_max, m, n;
  50.     int    i_max;
  51.     Real    **A_v, *A_piv, *A_row;
  52.     Real  max1, temp;
  53.     static    VEC    *scale = VNULL;
  54.  
  55.     if ( A==(MAT *)NULL || pivot==(PERM *)NULL )
  56.         error(E_NULL,"LUfactor");
  57.     if ( pivot->size != A->m )
  58.         error(E_SIZES,"LUfactor");
  59.     m = A->m;    n = A->n;
  60.     scale = v_resize(scale,A->m);
  61.     MEM_STAT_REG(scale,TYPE_VEC);
  62.     A_v = A->me;
  63.  
  64.     /* initialise pivot with identity permutation */
  65.     for ( i=0; i<m; i++ )
  66.         pivot->pe[i] = i;
  67.  
  68.     /* set scale parameters */
  69.     for ( i=0; i<m; i++ )
  70.     {
  71.         max1 = 0.0;
  72.         for ( j=0; j<n; j++ )
  73.         {
  74.             temp = fabs(A_v[i][j]);
  75.             max1 = max(max1,temp);
  76.         }
  77.         scale->ve[i] = max1;
  78.     }
  79.  
  80.     /* main loop */
  81.     k_max = min(m,n)-1;
  82.     for ( k=0; k<k_max; k++ )
  83.     {
  84.         /* find best pivot row */
  85.         max1 = 0.0;    i_max = -1;
  86.         for ( i=k; i<m; i++ )
  87.         if ( scale->ve[i] > 0.0 )
  88.         {
  89.             temp = fabs(A_v[i][k])/scale->ve[i];
  90.             if ( temp > max1 )
  91.             { max1 = temp;    i_max = i;    }
  92.         }
  93.         
  94.         /* if no pivot then ignore column k... */
  95.         if ( i_max == -1 )
  96.         continue;
  97.         
  98.         /* do we pivot ? */
  99.         if ( i_max != k )    /* yes we do... */
  100.         {
  101.         px_transp(pivot,i_max,k);
  102.         for ( j=0; j<n; j++ )
  103.         {
  104.             temp = A_v[i_max][j];
  105.             A_v[i_max][j] = A_v[k][j];
  106.             A_v[k][j] = temp;
  107.         }
  108.         }
  109.         
  110.         /* row operations */
  111.         for ( i=k+1; i<m; i++ )    /* for each row do... */
  112.         {    /* Note: divide by zero should never happen */
  113.         temp = A_v[i][k] = A_v[i][k]/A_v[k][k];
  114.         A_piv = &(A_v[k][k+1]);
  115.         A_row = &(A_v[i][k+1]);
  116.         if ( k+1 < n )
  117.             __mltadd__(A_row,A_piv,-temp,(int)(n-(k+1)));
  118.         /*********************************************
  119.           for ( j=k+1; j<n; j++ )
  120.           A_v[i][j] -= temp*A_v[k][j];
  121.           (*A_row++) -= temp*(*A_piv++);
  122.           *********************************************/
  123.         }
  124.         
  125.     }
  126.  
  127.     return A;
  128. }
  129.  
  130.  
  131. /* LUsolve -- given an LU factorisation in A, solve Ax=b */
  132. VEC    *LUsolve(A,pivot,b,x)
  133. MAT    *A;
  134. PERM    *pivot;
  135. VEC    *b,*x;
  136. {
  137.     if ( A==(MAT *)NULL || b==(VEC *)NULL || pivot==(PERM *)NULL )
  138.         error(E_NULL,"LUsolve");
  139.     if ( A->m != A->n || A->n != b->dim )
  140.         error(E_SIZES,"LUsolve");
  141.  
  142.     x = v_resize(x,b->dim);
  143.     px_vec(pivot,b,x);    /* x := P.b */
  144.     Lsolve(A,x,x,1.0);    /* implicit diagonal = 1 */
  145.     Usolve(A,x,x,0.0);    /* explicit diagonal */
  146.  
  147.     return (x);
  148. }
  149.  
  150. /* LUTsolve -- given an LU factorisation in A, solve A^T.x=b */
  151. VEC    *LUTsolve(LU,pivot,b,x)
  152. MAT    *LU;
  153. PERM    *pivot;
  154. VEC    *b,*x;
  155. {
  156.     if ( ! LU || ! b || ! pivot )
  157.         error(E_NULL,"LUTsolve");
  158.     if ( LU->m != LU->n || LU->n != b->dim )
  159.         error(E_SIZES,"LUTsolve");
  160.  
  161.     x = v_copy(b,x);
  162.     UTsolve(LU,x,x,0.0);    /* explicit diagonal */
  163.     LTsolve(LU,x,x,1.0);    /* implicit diagonal = 1 */
  164.     pxinv_vec(pivot,x,x);    /* x := P^T.tmp */
  165.  
  166.     return (x);
  167. }
  168.  
  169. /* m_inverse -- returns inverse of A, provided A is not too rank deficient
  170.     -- uses LU factorisation */
  171. MAT    *m_inverse(A,out)
  172. MAT    *A, *out;
  173. {
  174.     int    i;
  175.     static VEC    *tmp = VNULL, *tmp2 = VNULL;
  176.     static MAT    *A_cp = MNULL;
  177.     static PERM    *pivot = PNULL;
  178.  
  179.     if ( ! A )
  180.         error(E_NULL,"m_inverse");
  181.     if ( A->m != A->n )
  182.         error(E_SQUARE,"m_inverse");
  183.     if ( ! out || out->m < A->m || out->n < A->n )
  184.         out = m_resize(out,A->m,A->n);
  185.  
  186.     A_cp = m_copy(A,MNULL);
  187.     tmp = v_resize(tmp,A->m);
  188.     tmp2 = v_resize(tmp2,A->m);
  189.     pivot = px_resize(pivot,A->m);
  190.     MEM_STAT_REG(A_cp,TYPE_MAT);
  191.     MEM_STAT_REG(tmp, TYPE_VEC);
  192.     MEM_STAT_REG(tmp2,TYPE_VEC);
  193.     MEM_STAT_REG(pivot,TYPE_PERM);
  194.     tracecatch(LUfactor(A_cp,pivot),"m_inverse");
  195.     for ( i = 0; i < A->n; i++ )
  196.     {
  197.         v_zero(tmp);
  198.         tmp->ve[i] = 1.0;
  199.         tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
  200.         set_col(out,i,tmp2);
  201.     }
  202.  
  203.     return out;
  204. }
  205.  
  206. /* LUcondest -- returns an estimate of the condition number of LU given the
  207.     LU factorisation in compact form */
  208. double    LUcondest(LU,pivot)
  209. MAT    *LU;
  210. PERM    *pivot;
  211. {
  212.     static    VEC    *y = VNULL, *z = VNULL;
  213.     Real    cond_est, L_norm, U_norm, sum;
  214.     int        i, j, n;
  215.  
  216.     if ( ! LU || ! pivot )
  217.     error(E_NULL,"LUcondest");
  218.     if ( LU->m != LU->n )
  219.     error(E_SQUARE,"LUcondest");
  220.     if ( LU->n != pivot->size )
  221.     error(E_SIZES,"LUcondest");
  222.  
  223.     n = LU->n;
  224.     y = v_resize(y,n);
  225.     z = v_resize(z,n);
  226.     MEM_STAT_REG(y,TYPE_VEC);
  227.     MEM_STAT_REG(z,TYPE_VEC);
  228.  
  229.     for ( i = 0; i < n; i++ )
  230.     {
  231.     sum = 0.0;
  232.     for ( j = 0; j < i; j++ )
  233.         sum -= LU->me[j][i]*y->ve[j];
  234.     sum -= (sum < 0.0) ? 1.0 : -1.0;
  235.     if ( LU->me[i][i] == 0.0 )
  236.         return HUGE_VAL;
  237.     y->ve[i] = sum / LU->me[i][i];
  238.     }
  239.  
  240.     LTsolve(LU,y,y,1.0);
  241.     LUsolve(LU,pivot,y,z);
  242.  
  243.     /* now estimate norm of A (even though it is not directly available) */
  244.     /* actually computes ||L||_inf.||U||_inf */
  245.     U_norm = 0.0;
  246.     for ( i = 0; i < n; i++ )
  247.     {
  248.     sum = 0.0;
  249.     for ( j = i; j < n; j++ )
  250.         sum += fabs(LU->me[i][j]);
  251.     if ( sum > U_norm )
  252.         U_norm = sum;
  253.     }
  254.     L_norm = 0.0;
  255.     for ( i = 0; i < n; i++ )
  256.     {
  257.     sum = 1.0;
  258.     for ( j = 0; j < i; j++ )
  259.         sum += fabs(LU->me[i][j]);
  260.     if ( sum > L_norm )
  261.         L_norm = sum;
  262.     }
  263.  
  264.     tracecatch(cond_est = U_norm*L_norm*v_norm_inf(z)/v_norm_inf(y),
  265.            "LUcondest");
  266.  
  267.     return cond_est;
  268. }
  269.