home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / bdfactor < prev    next >
Encoding:
Text File  |  1994-05-17  |  13.1 KB  |  600 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.   Band matrix factorisation routines
  29.   */
  30.  
  31. /* bdfactor.c  18/11/93 */
  32. static    char    rcsid[] = "$Id: bdfactor.c,v 1.5 1994/05/17 23:18:32 des Exp $";
  33.  
  34. #include    <stdio.h>
  35. #include    <math.h>
  36. #include        "matrix2.h"
  37.  
  38.  
  39. /* generate band matrix 
  40.    for a matrix  with n columns,
  41.    lb subdiagonals and ub superdiagonals;
  42.  
  43.    Way of saving a band of a matrix:
  44.    first we save subdiagonals (from 0 to lb-1);
  45.    then main diagonal (in the lb row)
  46.    and then superdiagonals (from lb+1 to lb+ub)
  47.    in such a way that the elements which were previously
  48.    in one column are now also in one column
  49. */
  50.  
  51. BAND *bd_get(lb,ub,n)
  52. int lb, ub, n;
  53. {
  54.    BAND *A;
  55.  
  56.    if (lb < 0 || ub < 0 || n <= 0)
  57.      error(E_NEG,"bd_get");
  58.  
  59.    if ((A = NEW(BAND)) == (BAND *)NULL)
  60.      error(E_MEM,"bd_get");
  61.    else if (mem_info_is_on()) {
  62.       mem_bytes(TYPE_BAND,0,sizeof(BAND));
  63.       mem_numvar(TYPE_BAND,1);
  64.    }
  65.  
  66.    lb = A->lb = min(n-1,lb);
  67.    ub = A->ub = min(n-1,ub);
  68.    A->mat = m_get(lb+ub+1,n);
  69.    return A;
  70. }
  71.  
  72. int bd_free(A)
  73. BAND *A;
  74. {
  75.    if ( A == (BAND *)NULL || A->lb < 0 || A->ub < 0 )
  76.      /* don't trust it */
  77.      return (-1);
  78.  
  79.    if (A->mat) m_free(A->mat);
  80.  
  81.    if (mem_info_is_on()) {
  82.       mem_bytes(TYPE_BAND,sizeof(BAND),0);
  83.       mem_numvar(TYPE_BAND,-1);
  84.    }
  85.  
  86.    free((char *)A);
  87.    return 0;
  88. }
  89.  
  90.  
  91. /* resize band matrix */
  92.  
  93. BAND *bd_resize(A,new_lb,new_ub,new_n)
  94. BAND *A;
  95. int new_lb,new_ub,new_n;
  96. {
  97.    int lb,ub,i,j,l,shift,umin;
  98.    Real **Av;
  99.  
  100.    if (new_lb < 0 || new_ub < 0 || new_n <= 0)
  101.      error(E_NEG,"bd_resize");
  102.    if ( ! A )
  103.      return bd_get(new_lb,new_ub,new_n);
  104.     if ( A->lb+A->ub+1 > A->mat->m )
  105.     error(E_INTERN,"bd_resize");
  106.  
  107.    if ( A->lb == new_lb && A->ub == new_ub && A->mat->n == new_n )
  108.     return A;
  109.  
  110.    lb = A->lb;
  111.    ub = A->ub;
  112.    Av = A->mat->me;
  113.    umin = min(ub,new_ub);
  114.  
  115.     /* ensure that unused triangles at edges are zero'd */
  116.  
  117.    for ( i = 0; i < lb; i++ )
  118.       for ( j = A->mat->n - lb + i; j < A->mat->n; j++ )
  119.     Av[i][j] = 0.0;  
  120.     for ( i = lb+1,l=1; l <= umin; i++,l++ )
  121.       for ( j = 0; j < l; j++ )
  122.     Av[i][j] = 0.0; 
  123.  
  124.    new_lb = A->lb = min(new_lb,new_n-1);
  125.    new_ub = A->ub = min(new_ub,new_n-1);
  126.    A->mat = m_resize(A->mat,new_lb+new_ub+1,new_n);
  127.    Av = A->mat->me;
  128.  
  129.    /* if new_lb != lb then move the rows to get the main diag 
  130.       in the new_lb row */
  131.  
  132.    if (new_lb > lb) {
  133.       shift = new_lb-lb;
  134.  
  135.       for (i=lb+umin, l=i+shift; i >= 0; i--,l--)
  136.     MEM_COPY(Av[i],Av[l],new_n*sizeof(Real));
  137.       for (l=shift-1; l >= 0; l--)
  138.     __zero__(Av[l],new_n);
  139.    }
  140.    else { /* new_lb < lb */
  141.       shift = lb - new_lb;
  142.  
  143.       for (i=shift, l=0; i <= lb+umin; i++,l++)
  144.     MEM_COPY(Av[i],Av[l],new_n*sizeof(Real));
  145.       for (i=lb+umin+1; i <= new_lb+new_ub; i++)
  146.     __zero__(Av[i],new_n);
  147.    }
  148.  
  149.    return A;
  150. }
  151.  
  152.  
  153.  
  154. BAND *bd_copy(A,B)
  155. BAND *A,*B;
  156. {
  157.    int lb,ub,i,j,n;
  158.    
  159.    if ( !A )
  160.      error(E_NULL,"bd_copy");
  161.  
  162.    if (A == B) return B;
  163.    
  164.    n = A->mat->n;
  165.    if ( !B )
  166.      B = bd_get(A->lb,A->ub,n);
  167.    else if (B->lb != A->lb || B->ub != A->ub || B->mat->n != n )
  168.      B = bd_resize(B,A->lb,A->ub,n);
  169.    
  170.    if (A->mat == B->mat) return B;
  171.    ub = B->ub = A->ub;
  172.    lb = B->lb = A->lb;
  173.  
  174.    for ( i=0, j=n-lb; i <= lb; i++, j++ )
  175.      MEM_COPY(A->mat->me[i],B->mat->me[i],j*sizeof(Real));   
  176.  
  177.    for ( i=lb+1, j=1; i <= lb+ub; i++, j++ )
  178.      MEM_COPY(A->mat->me[i]+j,B->mat->me[i]+j,(n - j)*sizeof(Real));     
  179.  
  180.    return B;
  181. }
  182.  
  183.  
  184. /* copy band matrix to a square matrix */
  185. MAT *band2mat(bA,A)
  186. BAND *bA;
  187. MAT *A;
  188. {
  189.    int i,j,l,n,n1;
  190.    int lb, ub;
  191.    Real **bmat;
  192.  
  193.    if ( !bA || !A)
  194.      error(E_NULL,"band2mat");
  195.    if ( bA->mat == A )
  196.      error(E_INSITU,"band2mat");
  197.  
  198.    ub = bA->ub;
  199.    lb = bA->lb;
  200.    n = bA->mat->n;
  201.    n1 = n-1;
  202.    bmat = bA->mat->me;
  203.  
  204.    A = m_resize(A,n,n);
  205.    m_zero(A);
  206.  
  207.    for (j=0; j < n; j++)
  208.      for (i=min(n1,j+lb),l=lb+j-i; i >= max(0,j-ub); i--,l++)
  209.        A->me[i][j] = bmat[l][j];
  210.  
  211.    return A;
  212. }
  213.  
  214. /* copy a square matrix to a band matrix with 
  215.    lb subdiagonals and ub superdiagonals */
  216. BAND *mat2band(A,lb,ub,bA)
  217. BAND *bA;
  218. MAT *A;
  219. int lb, ub;
  220. {
  221.    int i, j, l, n1;
  222.    Real **bmat;
  223.    
  224.    if (! A || ! bA)
  225.      error(E_NULL,"mat2band");
  226.    if (ub < 0 || lb < 0)
  227.      error(E_SIZES,"mat2band");
  228.    if (bA->mat == A)
  229.      error(E_INSITU,"mat2band");
  230.  
  231.    n1 = A->n-1;
  232.    lb = min(n1,lb);
  233.    ub = min(n1,ub);
  234.    bA = bd_resize(bA,lb,ub,n1+1);
  235.    bmat = bA->mat->me;
  236.  
  237.    for (j=0; j <= n1; j++)
  238.      for (i=min(n1,j+lb),l=lb+j-i; i >= max(0,j-ub); i--,l++)
  239.        bmat[l][j] = A->me[i][j];
  240.  
  241.    return bA;
  242. }
  243.  
  244.  
  245.  
  246. /* transposition of matrix in;
  247.    out - matrix after transposition;
  248.    can be done in situ
  249. */
  250.  
  251. BAND *bd_transp(in,out)
  252. BAND *in, *out;
  253. {
  254.    int i, j, jj, l, k, lb, ub, lub, n, n1;
  255.    int in_situ;
  256.    Real  **in_v, **out_v;
  257.    
  258.    if ( in == (BAND *)NULL || in->mat == (MAT *)NULL )
  259.      error(E_NULL,"bd_transp");
  260.  
  261.    lb = in->lb;
  262.    ub = in->ub;
  263.    lub = lb+ub;
  264.    n = in->mat->n;
  265.    n1 = n-1;
  266.  
  267.    in_situ = ( in == out );
  268.    if ( ! in_situ )
  269.        out = bd_resize(out,ub,lb,n);
  270.    else
  271.    {   /* only need to swap lb and ub fields */
  272.        out->lb = ub;
  273.        out->ub = lb;
  274.    }
  275.  
  276.    in_v = in->mat->me;
  277.    
  278.    if (! in_situ) {
  279.       int sh_in,sh_out; 
  280.  
  281.       out_v = out->mat->me;
  282.       for (i=0, l=lub, k=lb-i; i <= lub; i++,l--,k--) {
  283.      sh_in = max(-k,0);
  284.      sh_out = max(k,0);
  285.      MEM_COPY(&(in_v[i][sh_in]),&(out_v[l][sh_out]),
  286.           (n-sh_in-sh_out)*sizeof(Real));
  287.      /**********************************
  288.      for (j=n1-sh_out, jj=n1-sh_in; j >= sh_in; j--,jj--) {
  289.         out_v[l][jj] = in_v[i][j];
  290.      }
  291.      **********************************/
  292.       }
  293.    }
  294.    else if (ub == lb) {
  295.       Real tmp;
  296.  
  297.       for (i=0, l=lub, k=lb-i; i < lb; i++,l--,k--) {
  298.      for (j=n1-k, jj=n1; j >= 0; j--,jj--) {
  299.         tmp = in_v[l][jj];
  300.         in_v[l][jj] = in_v[i][j];
  301.         in_v[i][j] = tmp;
  302.      }
  303.       }
  304.    }
  305.    else if (ub > lb) {  /* hence i-ub <= 0 & l-lb >= 0 */
  306.       int p,pp,lbi;
  307.       
  308.       for (i=0, l=lub; i < (lub+1)/2; i++,l--) {
  309.      lbi = lb-i;
  310.      for (j=l-lb, jj=0, p=max(-lbi,0), pp = max(l-ub,0); j <= n1; 
  311.           j++,jj++,p++,pp++) {
  312.         in_v[l][pp] = in_v[i][p];
  313.         in_v[i][jj] = in_v[l][j];
  314.      }
  315.      for (  ; p <= n1-max(lbi,0); p++,pp++)
  316.        in_v[l][pp] = in_v[i][p];
  317.       }
  318.       
  319.       if (lub%2 == 0) { /* shift only */
  320.      i = lub/2;
  321.      for (j=max(i-lb,0), jj=0; jj <= n1-ub+i; j++,jj++) 
  322.        in_v[i][jj] = in_v[i][j];
  323.       }
  324.    }
  325.    else {      /* ub < lb, hence ub-l <= 0 & lb-i >= 0 */
  326.       int p,pp,ubi;
  327.  
  328.       for (i=0, l=lub; i < (lub+1)/2; i++,l--) {
  329.      ubi = i-ub;
  330.      for (j=n1-max(lb-l,0), jj=n1-max(-ubi,0), p=n1-lb+i, pp=n1;
  331.           p >= 0; j--, jj--, pp--, p--) {
  332.         in_v[i][jj] = in_v[l][j];
  333.         in_v[l][pp] = in_v[i][p];
  334.      }
  335.      for (  ; jj >= max(ubi,0); j--, jj--)
  336.        in_v[i][jj] = in_v[l][j];
  337.       }
  338.  
  339.       if (lub%2 == 0) {  /* shift only */
  340.      i = lub/2;
  341.      for (j=n1-lb+i, jj=n1-max(ub-i,0); j >= 0; j--, jj--) 
  342.         in_v[i][jj] = in_v[i][j];
  343.       }
  344.    }
  345.  
  346.    return out;
  347. }
  348.  
  349.  
  350.  
  351. /* bdLUfactor -- gaussian elimination with partial pivoting
  352.    -- on entry, the matrix A in band storage with elements 
  353.       in rows 0 to lb+ub; 
  354.       The jth column of A is stored in the jth column of 
  355.       band A (bA) as follows:
  356.       bA->mat->me[lb+j-i][j] = A->me[i][j] for 
  357.       max(0,j-lb) <= i <= min(A->n-1,j+ub);
  358.    -- on exit: U is stored as an upper triangular matrix
  359.       with lb+ub superdiagonals in rows lb to 2*lb+ub, 
  360.       and the matrix L is stored in rows 0 to lb-1.
  361.       Matrix U is permuted, whereas L is not permuted !!!
  362.       Therefore we save some memory.
  363.    */
  364. BAND    *bdLUfactor(bA,pivot)
  365. BAND    *bA;
  366. PERM    *pivot;
  367. {
  368.    int    i, j, k, l, n, n1, lb, ub, lub, k_end, k_lub;
  369.    int    i_max, shift;
  370.    Real    **bA_v;
  371.    Real max1, temp;
  372.    
  373.    if ( bA==(BAND *)NULL || pivot==(PERM *)NULL )
  374.      error(E_NULL,"bdLUfactor");
  375.  
  376.    lb = bA->lb;
  377.    ub = bA->ub;
  378.    lub = lb+ub;
  379.    n = bA->mat->n;
  380.    n1 = n-1;
  381.    lub = lb+ub;
  382.  
  383.    if ( pivot->size != n )
  384.      error(E_SIZES,"bdLUfactor");
  385.  
  386.    
  387.    /* initialise pivot with identity permutation */
  388.    for ( i=0; i < n; i++ )
  389.      pivot->pe[i] = i;
  390.  
  391.    /* extend band matrix */
  392.    /* extended part is filled with zeros */
  393.    bA = bd_resize(bA,lb,min(n1,lub),n);
  394.    bA_v = bA->mat->me;
  395.  
  396.  
  397.    /* main loop */
  398.  
  399.    for ( k=0; k < n1; k++ )
  400.    {
  401.       k_end = max(0,lb+k-n1);
  402.       k_lub = min(k+lub,n1);
  403.  
  404.       /* find the best pivot row */
  405.       
  406.       max1 = 0.0;    
  407.       i_max = -1;
  408.       for ( i=lb; i >= k_end; i-- ) {
  409.      temp = fabs(bA_v[i][k]);
  410.      if ( temp > max1 )
  411.      { max1 = temp;    i_max = i; }
  412.       }
  413.       
  414.       /* if no pivot then ignore column k... */
  415.       if ( i_max == -1 )
  416.     continue;
  417.       
  418.       /* do we pivot ? */
  419.       if ( i_max != lb )    /* yes we do... */
  420.       {
  421.      /* save transposition using non-shifted indices */
  422.      shift = lb-i_max;
  423.      px_transp(pivot,k+shift,k);
  424.      for ( i=lb, j=k; j <= k_lub; i++,j++ )
  425.      {
  426.         temp = bA_v[i][j];
  427.         bA_v[i][j] = bA_v[i-shift][j];
  428.         bA_v[i-shift][j] = temp;
  429.      }
  430.       }
  431.       
  432.       /* row operations */
  433.       for ( i=lb-1; i >= k_end; i-- ) {
  434.      temp = bA_v[i][k] /= bA_v[lb][k];
  435.      shift = lb-i;
  436.      for ( j=k+1,l=i+1; j <= k_lub; l++,j++ )
  437.        bA_v[l][j] -= temp*bA_v[l+shift][j];
  438.       }
  439.    }
  440.    
  441.    return bA;
  442. }
  443.  
  444.  
  445. /* bdLUsolve -- given an LU factorisation in bA, solve bA*x=b */
  446. /* pivot is changed upon return  */
  447. VEC    *bdLUsolve(bA,pivot,b,x)
  448. BAND    *bA;
  449. PERM    *pivot;
  450. VEC    *b,*x;
  451. {
  452.    int i,j,l,n,n1,pi,lb,ub,jmin, maxj;
  453.    Real c;
  454.    Real **bA_v;
  455.  
  456.    if ( bA==(BAND *)NULL || b==(VEC *)NULL || pivot==(PERM *)NULL )
  457.      error(E_NULL,"bdLUsolve");
  458.    if ( bA->mat->n != b->dim || bA->mat->n != pivot->size)
  459.      error(E_SIZES,"bdLUsolve");
  460.  
  461.    lb = bA->lb;
  462.    ub = bA->ub;
  463.    n = b->dim;
  464.    n1 = n-1;
  465.    bA_v = bA->mat->me;
  466.  
  467.    x = v_resize(x,b->dim);
  468.    px_vec(pivot,b,x);
  469.  
  470.    /* solve Lx = b; implicit diagonal = 1 
  471.       L is not permuted, therefore it must be permuted now
  472.     */
  473.    
  474.    px_inv(pivot,pivot);
  475.    for (j=0; j < n; j++) {
  476.       jmin = j+1;
  477.       c = x->ve[j];
  478.       maxj = max(0,j+lb-n1);
  479.       for (i=jmin,l=lb-1; l >= maxj; i++,l--) {
  480.      if ( (pi = pivot->pe[i]) < jmin) 
  481.        pi = pivot->pe[i] = pivot->pe[pi];
  482.      x->ve[pi] -= bA_v[l][j]*c;
  483.       }
  484.    }
  485.  
  486.    /* solve Ux = b; explicit diagonal */
  487.  
  488.    x->ve[n1] /= bA_v[lb][n1];
  489.    for (i=n-2; i >= 0; i--) {
  490.       c = x->ve[i];
  491.       for (j=min(n1,i+ub), l=lb+j-i; j > i; j--,l--)
  492.     c -= bA_v[l][j]*x->ve[j];
  493.       x->ve[i] = c/bA_v[lb][i];
  494.    }
  495.    
  496.    return (x);
  497. }
  498.  
  499. /* LDLfactor -- L.D.L' factorisation of A in-situ;
  500.    A is a band matrix
  501.    it works using only lower bandwidth & main diagonal
  502.    so it is possible to set A->ub = 0
  503.  */
  504.  
  505. BAND *bdLDLfactor(A)
  506. BAND *A;
  507. {
  508.    int i,j,k,n,n1,lb,ki,jk,ji,lbkm,lbkp;
  509.    Real **Av;
  510.    Real c, cc;
  511.  
  512.    if ( ! A )
  513.      error(E_NULL,"bdLDLfactor");
  514.  
  515.    if (A->lb == 0) return A;
  516.  
  517.    lb = A->lb;
  518.    n = A->mat->n;
  519.    n1 = n-1;
  520.    Av = A->mat->me;
  521.    
  522.    for (k=0; k < n; k++) {    
  523.       lbkm = lb-k;
  524.       lbkp = lb+k;
  525.  
  526.       /* matrix D */
  527.       c = Av[lb][k];
  528.       for (j=max(0,-lbkm), jk=lbkm+j; j < k; j++, jk++) {
  529.      cc = Av[jk][j];
  530.      c -= Av[lb][j]*cc*cc;
  531.       }
  532.       if (c == 0.0)
  533.     error(E_SING,"bdLDLfactor");
  534.       Av[lb][k] = c;
  535.  
  536.       /* matrix L */
  537.       
  538.       for (i=min(n1,lbkp), ki=lbkp-i; i > k; i--,ki++) {
  539.      c = Av[ki][k];
  540.      for (j=max(0,i-lb), ji=lb+j-i, jk=lbkm+j; j < k;
  541.           j++, ji++, jk++)
  542.        c -= Av[lb][j]*Av[ji][j]*Av[jk][j];
  543.      Av[ki][k] = c/Av[lb][k];
  544.       }
  545.    }
  546.    
  547.    return A;
  548. }
  549.  
  550. /* solve A*x = b, where A is factorized by 
  551.    Choleski LDL^T factorization */
  552. VEC    *bdLDLsolve(A,b,x)
  553. BAND   *A;
  554. VEC    *b, *x;
  555. {
  556.    int i,j,l,n,n1,lb,ilb;
  557.    Real **Av, *Avlb;
  558.    Real c;
  559.  
  560.    if ( ! A || ! b )
  561.      error(E_NULL,"bdLDLsolve");
  562.    if ( A->mat->n != b->dim )
  563.      error(E_SIZES,"bdLDLsolve");
  564.  
  565.    n = A->mat->n;
  566.    n1 = n-1;
  567.    x = v_resize(x,n);
  568.    lb = A->lb;
  569.    Av = A->mat->me;  
  570.    Avlb = Av[lb];
  571.    
  572.    /* solve L*y = b */
  573.    x->ve[0] = b->ve[0];
  574.    for (i=1; i < n; i++) {
  575.       ilb = i-lb;
  576.       c = b->ve[i];
  577.       for (j=max(0,ilb), l=j-ilb; j < i; j++,l++)
  578.     c -= Av[l][j]*x->ve[j];
  579.       x->ve[i] = c;
  580.    }
  581.  
  582.    /* solve D*z = y */
  583.    for (i=0; i < n; i++) 
  584.      x->ve[i] /= Avlb[i];
  585.  
  586.    /* solve L^T*x = z */
  587.    for (i=n-2; i >= 0; i--) {
  588.       ilb = i+lb;
  589.       c = x->ve[i];
  590.       for (j=min(n1,ilb), l=ilb-j; j > i; j--,l++)
  591.     c -= Av[l][i]*x->ve[j];
  592.       x->ve[i] = c;
  593.    }
  594.  
  595.    return x;
  596. }
  597.  
  598.  
  599.  
  600.