home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / bkpfacto < prev    next >
Encoding:
Text File  |  1994-01-13  |  8.3 KB  |  312 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. static    char    rcsid[] = "$Id: bkpfacto.c,v 1.7 1994/01/13 05:45:50 des Exp $";
  32.  
  33. #include    <stdio.h>
  34. #include    <math.h>
  35. #include    "matrix.h"
  36. #include        "matrix2.h"
  37.  
  38. #define    btos(x)    ((x) ? "TRUE" : "FALSE")
  39.  
  40. /* Most matrix factorisation routines are in-situ unless otherwise specified */
  41.  
  42. #define alpha    0.6403882032022076 /* = (1+sqrt(17))/8 */
  43.  
  44. /* sqr -- returns square of x -- utility function */
  45. double    sqr(x)
  46. double    x;
  47. {    return x*x;    }
  48.  
  49. /* interchange -- a row/column swap routine */
  50. static void interchange(A,i,j)
  51. MAT    *A;    /* assumed != NULL & also SQUARE */
  52. int    i, j;    /* assumed in range */
  53. {
  54.     Real    **A_me, tmp;
  55.     int    k, n;
  56.  
  57.     A_me = A->me;    n = A->n;
  58.     if ( i == j )
  59.         return;
  60.     if ( i > j )
  61.     {    k = i;    i = j;    j = k;    }
  62.     for ( k = 0; k < i; k++ )
  63.     {
  64.         /* tmp = A_me[k][i]; */
  65.         tmp = m_entry(A,k,i);
  66.         /* A_me[k][i] = A_me[k][j]; */
  67.         m_set_val(A,k,i,m_entry(A,k,j));
  68.         /* A_me[k][j] = tmp; */
  69.         m_set_val(A,k,j,tmp);
  70.     }
  71.     for ( k = j+1; k < n; k++ )
  72.     {
  73.         /* tmp = A_me[j][k]; */
  74.         tmp = m_entry(A,j,k);
  75.         /* A_me[j][k] = A_me[i][k]; */
  76.         m_set_val(A,j,k,m_entry(A,i,k));
  77.         /* A_me[i][k] = tmp; */
  78.         m_set_val(A,i,k,tmp);
  79.     }
  80.     for ( k = i+1; k < j; k++ )
  81.     {
  82.         /* tmp = A_me[k][j]; */
  83.         tmp = m_entry(A,k,j);
  84.         /* A_me[k][j] = A_me[i][k]; */
  85.         m_set_val(A,k,j,m_entry(A,i,k));
  86.         /* A_me[i][k] = tmp; */
  87.         m_set_val(A,i,k,tmp);
  88.     }
  89.     /* tmp = A_me[i][i]; */
  90.     tmp = m_entry(A,i,i);
  91.     /* A_me[i][i] = A_me[j][j]; */
  92.     m_set_val(A,i,i,m_entry(A,j,j));
  93.     /* A_me[j][j] = tmp; */
  94.     m_set_val(A,j,j,tmp);
  95. }
  96.  
  97. /* BKPfactor -- Bunch-Kaufman-Parlett factorisation of A in-situ
  98.     -- A is factored into the form P'AP = MDM' where 
  99.     P is a permutation matrix, M lower triangular and D is block
  100.     diagonal with blocks of size 1 or 2
  101.     -- P is stored in pivot; blocks[i]==i iff D[i][i] is a block */
  102. MAT    *BKPfactor(A,pivot,blocks)
  103. MAT    *A;
  104. PERM    *pivot, *blocks;
  105. {
  106.     int    i, j, k, n, onebyone, r;
  107.     Real    **A_me, aii, aip1, aip1i, lambda, sigma, tmp;
  108.     Real    det, s, t;
  109.  
  110.     if ( ! A || ! pivot || ! blocks )
  111.         error(E_NULL,"BKPfactor");
  112.     if ( A->m != A->n )
  113.         error(E_SQUARE,"BKPfactor");
  114.     if ( A->m != pivot->size || pivot->size != blocks->size )
  115.         error(E_SIZES,"BKPfactor");
  116.  
  117.     n = A->n;
  118.     A_me = A->me;
  119.     px_ident(pivot);    px_ident(blocks);
  120.  
  121.     for ( i = 0; i < n; i = onebyone ? i+1 : i+2 )
  122.     {
  123.         /* printf("# Stage: %d\n",i); */
  124.         aii = fabs(m_entry(A,i,i));
  125.         lambda = 0.0;    r = (i+1 < n) ? i+1 : i;
  126.         for ( k = i+1; k < n; k++ )
  127.         {
  128.             tmp = fabs(m_entry(A,i,k));
  129.             if ( tmp >= lambda )
  130.             {
  131.             lambda = tmp;
  132.             r = k;
  133.             }
  134.         }
  135.         /* printf("# lambda = %g, r = %d\n", lambda, r); */
  136.         /* printf("# |A[%d][%d]| = %g\n",r,r,fabs(m_entry(A,r,r))); */
  137.  
  138.         /* determine if 1x1 or 2x2 block, and do pivoting if needed */
  139.         if ( aii >= alpha*lambda )
  140.         {
  141.             onebyone = TRUE;
  142.             goto dopivot;
  143.         }
  144.         /* compute sigma */
  145.         sigma = 0.0;
  146.         for ( k = i; k < n; k++ )
  147.         {
  148.             if ( k == r )
  149.             continue;
  150.             tmp = ( k > r ) ? fabs(m_entry(A,r,k)) :
  151.             fabs(m_entry(A,k,r));
  152.             if ( tmp > sigma )
  153.             sigma = tmp;
  154.         }
  155.         if ( aii*sigma >= alpha*sqr(lambda) )
  156.             onebyone = TRUE;
  157.         else if ( fabs(m_entry(A,r,r)) >= alpha*sigma )
  158.         {
  159.             /* printf("# Swapping rows/cols %d and %d\n",i,r); */
  160.             interchange(A,i,r);
  161.             px_transp(pivot,i,r);
  162.             onebyone = TRUE;
  163.         }
  164.         else
  165.         {
  166.             /* printf("# Swapping rows/cols %d and %d\n",i+1,r); */
  167.             interchange(A,i+1,r);
  168.             px_transp(pivot,i+1,r);
  169.             px_transp(blocks,i,i+1);
  170.             onebyone = FALSE;
  171.         }
  172.         /* printf("onebyone = %s\n",btos(onebyone)); */
  173.         /* printf("# Matrix so far (@checkpoint A) =\n"); */
  174.         /* m_output(A); */
  175.         /* printf("# pivot =\n");    px_output(pivot); */
  176.         /* printf("# blocks =\n");    px_output(blocks); */
  177.  
  178. dopivot:
  179.         if ( onebyone )
  180.         {   /* do one by one block */
  181.             if ( m_entry(A,i,i) != 0.0 )
  182.             {
  183.             aii = m_entry(A,i,i);
  184.             for ( j = i+1; j < n; j++ )
  185.             {
  186.                 tmp = m_entry(A,i,j)/aii;
  187.                 for ( k = j; k < n; k++ )
  188.                 m_sub_val(A,j,k,tmp*m_entry(A,i,k));
  189.                 m_set_val(A,i,j,tmp);
  190.             }
  191.             }
  192.         }
  193.         else /* onebyone == FALSE */
  194.         {   /* do two by two block */
  195.             det = m_entry(A,i,i)*m_entry(A,i+1,i+1)-sqr(m_entry(A,i,i+1));
  196.             /* Must have det < 0 */
  197.             /* printf("# det = %g\n",det); */
  198.             aip1i = m_entry(A,i,i+1)/det;
  199.             aii = m_entry(A,i,i)/det;
  200.             aip1 = m_entry(A,i+1,i+1)/det;
  201.             for ( j = i+2; j < n; j++ )
  202.             {
  203.             s = - aip1i*m_entry(A,i+1,j) + aip1*m_entry(A,i,j);
  204.             t = - aip1i*m_entry(A,i,j) + aii*m_entry(A,i+1,j);
  205.             for ( k = j; k < n; k++ )
  206.                 m_sub_val(A,j,k,m_entry(A,i,k)*s + m_entry(A,i+1,k)*t);
  207.             m_set_val(A,i,j,s);
  208.             m_set_val(A,i+1,j,t);
  209.             }
  210.         }
  211.         /* printf("# Matrix so far (@checkpoint B) =\n"); */
  212.         /* m_output(A); */
  213.         /* printf("# pivot =\n");    px_output(pivot); */
  214.         /* printf("# blocks =\n");    px_output(blocks); */
  215.     }
  216.  
  217.     /* set lower triangular half */
  218.     for ( i = 0; i < A->m; i++ )
  219.         for ( j = 0; j < i; j++ )
  220.         m_set_val(A,i,j,m_entry(A,j,i));
  221.  
  222.     return A;
  223. }
  224.  
  225. /* BKPsolve -- solves A.x = b where A has been factored a la BKPfactor()
  226.     -- returns x, which is created if NULL */
  227. VEC    *BKPsolve(A,pivot,block,b,x)
  228. MAT    *A;
  229. PERM    *pivot, *block;
  230. VEC    *b, *x;
  231. {
  232.     static VEC    *tmp=VNULL;    /* dummy storage needed */
  233.     int    i, j, n, onebyone;
  234.     Real    **A_me, a11, a12, a22, b1, b2, det, sum, *tmp_ve, tmp_diag;
  235.  
  236.     if ( ! A || ! pivot || ! block || ! b )
  237.         error(E_NULL,"BKPsolve");
  238.     if ( A->m != A->n )
  239.         error(E_SQUARE,"BKPsolve");
  240.     n = A->n;
  241.     if ( b->dim != n || pivot->size != n || block->size != n )
  242.         error(E_SIZES,"BKPsolve");
  243.     x = v_resize(x,n);
  244.     tmp = v_resize(tmp,n);
  245.     MEM_STAT_REG(tmp,TYPE_VEC);
  246.  
  247.     A_me = A->me;    tmp_ve = tmp->ve;
  248.  
  249.     px_vec(pivot,b,tmp);
  250.     /* solve for lower triangular part */
  251.     for ( i = 0; i < n; i++ )
  252.     {
  253.         sum = v_entry(tmp,i);
  254.         if ( block->pe[i] < i )
  255.             for ( j = 0; j < i-1; j++ )
  256.             sum -= m_entry(A,i,j)*v_entry(tmp,j);
  257.         else
  258.             for ( j = 0; j < i; j++ )
  259.             sum -= m_entry(A,i,j)*v_entry(tmp,j);
  260.         v_set_val(tmp,i,sum);
  261.     }
  262.     /* printf("# BKPsolve: solving L part: tmp =\n");    v_output(tmp); */
  263.     /* solve for diagonal part */
  264.     for ( i = 0; i < n; i = onebyone ? i+1 : i+2 )
  265.     {
  266.         onebyone = ( block->pe[i] == i );
  267.         if ( onebyone )
  268.         {
  269.             tmp_diag = m_entry(A,i,i);
  270.             if ( tmp_diag == 0.0 )
  271.             error(E_SING,"BKPsolve");
  272.             /* tmp_ve[i] /= tmp_diag; */
  273.             v_set_val(tmp,i,v_entry(tmp,i) / tmp_diag);
  274.         }
  275.         else
  276.         {
  277.             a11 = m_entry(A,i,i);
  278.             a22 = m_entry(A,i+1,i+1);
  279.             a12 = m_entry(A,i+1,i);
  280.             b1 = v_entry(tmp,i);    b2 = v_entry(tmp,i+1);
  281.             det = a11*a22-a12*a12;    /* < 0 : see BKPfactor() */
  282.             if ( det == 0.0 )
  283.             error(E_SING,"BKPsolve");
  284.             det = 1/det;
  285.             v_set_val(tmp,i,det*(a22*b1-a12*b2));
  286.             v_set_val(tmp,i+1,det*(a11*b2-a12*b1));
  287.         }
  288.     }
  289.     /* printf("# BKPsolve: solving D part: tmp =\n");    v_output(tmp); */
  290.     /* solve for transpose of lower traingular part */
  291.     for ( i = n-1; i >= 0; i-- )
  292.     {    /* use symmetry of factored form to get stride 1 */
  293.         sum = v_entry(tmp,i);
  294.         if ( block->pe[i] > i )
  295.             for ( j = i+2; j < n; j++ )
  296.             sum -= m_entry(A,i,j)*v_entry(tmp,j);
  297.         else
  298.             for ( j = i+1; j < n; j++ )
  299.             sum -= m_entry(A,i,j)*v_entry(tmp,j);
  300.         v_set_val(tmp,i,sum);
  301.     }
  302.  
  303.     /* printf("# BKPsolve: solving L^T part: tmp =\n");v_output(tmp); */
  304.     /* and do final permutation */
  305.     x = pxinv_vec(pivot,tmp,x);
  306.  
  307.     return x;
  308. }
  309.  
  310.         
  311.  
  312.