home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 22 gnu / 22-gnu.zip / mesch12a.zip / bdfactor.c < prev    next >
C/C++ Source or Header  |  1994-02-14  |  14KB  |  599 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.4 1994/02/14 00:38:27 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]),n-sh_in-sh_out);
  286.      /**********************************
  287.      for (j=n1-sh_out, jj=n1-sh_in; j >= sh_in; j--,jj--) {
  288.         out_v[l][jj] = in_v[i][j];
  289.      }
  290.      **********************************/
  291.       }
  292.    }
  293.    else if (ub == lb) {
  294.       Real tmp;
  295.  
  296.       for (i=0, l=lub, k=lb-i; i < lb; i++,l--,k--) {
  297.      for (j=n1-k, jj=n1; j >= 0; j--,jj--) {
  298.         tmp = in_v[l][jj];
  299.         in_v[l][jj] = in_v[i][j];
  300.         in_v[i][j] = tmp;
  301.      }
  302.       }
  303.    }
  304.    else if (ub > lb) {  /* hence i-ub <= 0 & l-lb >= 0 */
  305.       int p,pp,lbi;
  306.       
  307.       for (i=0, l=lub; i < (lub+1)/2; i++,l--) {
  308.      lbi = lb-i;
  309.      for (j=l-lb, jj=0, p=max(-lbi,0), pp = max(l-ub,0); j <= n1; 
  310.           j++,jj++,p++,pp++) {
  311.         in_v[l][pp] = in_v[i][p];
  312.         in_v[i][jj] = in_v[l][j];
  313.      }
  314.      for (  ; p <= n1-max(lbi,0); p++,pp++)
  315.        in_v[l][pp] = in_v[i][p];
  316.       }
  317.       
  318.       if (lub%2 == 0) { /* shift only */
  319.      i = lub/2;
  320.      for (j=max(i-lb,0), jj=0; jj <= n1-ub+i; j++,jj++) 
  321.        in_v[i][jj] = in_v[i][j];
  322.       }
  323.    }
  324.    else {      /* ub < lb, hence ub-l <= 0 & lb-i >= 0 */
  325.       int p,pp,ubi;
  326.  
  327.       for (i=0, l=lub; i < (lub+1)/2; i++,l--) {
  328.      ubi = i-ub;
  329.      for (j=n1-max(lb-l,0), jj=n1-max(-ubi,0), p=n1-lb+i, pp=n1;
  330.           p >= 0; j--, jj--, pp--, p--) {
  331.         in_v[i][jj] = in_v[l][j];
  332.         in_v[l][pp] = in_v[i][p];
  333.      }
  334.      for (  ; jj >= max(ubi,0); j--, jj--)
  335.        in_v[i][jj] = in_v[l][j];
  336.       }
  337.  
  338.       if (lub%2 == 0) {  /* shift only */
  339.      i = lub/2;
  340.      for (j=n1-lb+i, jj=n1-max(ub-i,0); j >= 0; j--, jj--) 
  341.         in_v[i][jj] = in_v[i][j];
  342.       }
  343.    }
  344.  
  345.    return out;
  346. }
  347.  
  348.  
  349.  
  350. /* bdLUfactor -- gaussian elimination with partial pivoting
  351.    -- on entry, the matrix A in band storage with elements 
  352.       in rows 0 to lb+ub; 
  353.       The jth column of A is stored in the jth column of 
  354.       band A (bA) as follows:
  355.       bA->mat->me[lb+j-i][j] = A->me[i][j] for 
  356.       max(0,j-lb) <= i <= min(A->n-1,j+ub);
  357.    -- on exit: U is stored as an upper triangular matrix
  358.       with lb+ub superdiagonals in rows lb to 2*lb+ub, 
  359.       and the matrix L is stored in rows 0 to lb-1.
  360.       Matrix U is permuted, whereas L is not permuted !!!
  361.       Therefore we save some memory.
  362.    */
  363. BAND    *bdLUfactor(bA,pivot)
  364. BAND    *bA;
  365. PERM    *pivot;
  366. {
  367.    int    i, j, k, l, n, n1, lb, ub, lub, k_end, k_lub;
  368.    int    i_max, shift;
  369.    Real    **bA_v;
  370.    Real max1, temp;
  371.    
  372.    if ( bA==(BAND *)NULL || pivot==(PERM *)NULL )
  373.      error(E_NULL,"bdLUfactor");
  374.  
  375.    lb = bA->lb;
  376.    ub = bA->ub;
  377.    lub = lb+ub;
  378.    n = bA->mat->n;
  379.    n1 = n-1;
  380.    lub = lb+ub;
  381.  
  382.    if ( pivot->size != n )
  383.      error(E_SIZES,"bdLUfactor");
  384.  
  385.    
  386.    /* initialise pivot with identity permutation */
  387.    for ( i=0; i < n; i++ )
  388.      pivot->pe[i] = i;
  389.  
  390.    /* extend band matrix */
  391.    /* extended part is filled with zeros */
  392.    bA = bd_resize(bA,lb,min(n1,lub),n);
  393.    bA_v = bA->mat->me;
  394.  
  395.  
  396.    /* main loop */
  397.  
  398.    for ( k=0; k < n1; k++ )
  399.    {
  400.       k_end = max(0,lb+k-n1);
  401.       k_lub = min(k+lub,n1);
  402.  
  403.       /* find the best pivot row */
  404.       
  405.       max1 = 0.0;    
  406.       i_max = -1;
  407.       for ( i=lb; i >= k_end; i-- ) {
  408.      temp = fabs(bA_v[i][k]);
  409.      if ( temp > max1 )
  410.      { max1 = temp;    i_max = i; }
  411.       }
  412.       
  413.       /* if no pivot then ignore column k... */
  414.       if ( i_max == -1 )
  415.     continue;
  416.       
  417.       /* do we pivot ? */
  418.       if ( i_max != lb )    /* yes we do... */
  419.       {
  420.      /* save transposition using non-shifted indices */
  421.      shift = lb-i_max;
  422.      px_transp(pivot,k+shift,k);
  423.      for ( i=lb, j=k; j <= k_lub; i++,j++ )
  424.      {
  425.         temp = bA_v[i][j];
  426.         bA_v[i][j] = bA_v[i-shift][j];
  427.         bA_v[i-shift][j] = temp;
  428.      }
  429.       }
  430.       
  431.       /* row operations */
  432.       for ( i=lb-1; i >= k_end; i-- ) {
  433.      temp = bA_v[i][k] /= bA_v[lb][k];
  434.      shift = lb-i;
  435.      for ( j=k+1,l=i+1; j <= k_lub; l++,j++ )
  436.        bA_v[l][j] -= temp*bA_v[l+shift][j];
  437.       }
  438.    }
  439.    
  440.    return bA;
  441. }
  442.  
  443.  
  444. /* bdLUsolve -- given an LU factorisation in bA, solve bA*x=b */
  445. /* pivot is changed upon return  */
  446. VEC    *bdLUsolve(bA,pivot,b,x)
  447. BAND    *bA;
  448. PERM    *pivot;
  449. VEC    *b,*x;
  450. {
  451.    int i,j,l,n,n1,pi,lb,ub,jmin, maxj;
  452.    Real c;
  453.    Real **bA_v;
  454.  
  455.    if ( bA==(BAND *)NULL || b==(VEC *)NULL || pivot==(PERM *)NULL )
  456.      error(E_NULL,"bdLUsolve");
  457.    if ( bA->mat->n != b->dim || bA->mat->n != pivot->size)
  458.      error(E_SIZES,"bdLUsolve");
  459.  
  460.    lb = bA->lb;
  461.    ub = bA->ub;
  462.    n = b->dim;
  463.    n1 = n-1;
  464.    bA_v = bA->mat->me;
  465.  
  466.    x = v_resize(x,b->dim);
  467.    px_vec(pivot,b,x);
  468.  
  469.    /* solve Lx = b; implicit diagonal = 1 
  470.       L is not permuted, therefore it must be permuted now
  471.     */
  472.    
  473.    px_inv(pivot,pivot);
  474.    for (j=0; j < n; j++) {
  475.       jmin = j+1;
  476.       c = x->ve[j];
  477.       maxj = max(0,j+lb-n1);
  478.       for (i=jmin,l=lb-1; l >= maxj; i++,l--) {
  479.      if ( (pi = pivot->pe[i]) < jmin) 
  480.        pi = pivot->pe[i] = pivot->pe[pi];
  481.      x->ve[pi] -= bA_v[l][j]*c;
  482.       }
  483.    }
  484.  
  485.    /* solve Ux = b; explicit diagonal */
  486.  
  487.    x->ve[n1] /= bA_v[lb][n1];
  488.    for (i=n-2; i >= 0; i--) {
  489.       c = x->ve[i];
  490.       for (j=min(n1,i+ub), l=lb+j-i; j > i; j--,l--)
  491.     c -= bA_v[l][j]*x->ve[j];
  492.       x->ve[i] = c/bA_v[lb][i];
  493.    }
  494.    
  495.    return (x);
  496. }
  497.  
  498. /* LDLfactor -- L.D.L' factorisation of A in-situ;
  499.    A is a band matrix
  500.    it works using only lower bandwidth & main diagonal
  501.    so it is possible to set A->ub = 0
  502.  */
  503.  
  504. BAND *bdLDLfactor(A)
  505. BAND *A;
  506. {
  507.    int i,j,k,n,n1,lb,ki,jk,ji,lbkm,lbkp;
  508.    Real **Av;
  509.    Real c, cc;
  510.  
  511.    if ( ! A )
  512.      error(E_NULL,"bdLDLfactor");
  513.  
  514.    if (A->lb == 0) return A;
  515.  
  516.    lb = A->lb;
  517.    n = A->mat->n;
  518.    n1 = n-1;
  519.    Av = A->mat->me;
  520.    
  521.    for (k=0; k < n; k++) {    
  522.       lbkm = lb-k;
  523.       lbkp = lb+k;
  524.  
  525.       /* matrix D */
  526.       c = Av[lb][k];
  527.       for (j=max(0,-lbkm), jk=lbkm+j; j < k; j++, jk++) {
  528.      cc = Av[jk][j];
  529.      c -= Av[lb][j]*cc*cc;
  530.       }
  531.       if (c == 0.0)
  532.     error(E_SING,"bdLDLfactor");
  533.       Av[lb][k] = c;
  534.  
  535.       /* matrix L */
  536.       
  537.       for (i=min(n1,lbkp), ki=lbkp-i; i > k; i--,ki++) {
  538.      c = Av[ki][k];
  539.      for (j=max(0,i-lb), ji=lb+j-i, jk=lbkm+j; j < k;
  540.           j++, ji++, jk++)
  541.        c -= Av[lb][j]*Av[ji][j]*Av[jk][j];
  542.      Av[ki][k] = c/Av[lb][k];
  543.       }
  544.    }
  545.    
  546.    return A;
  547. }
  548.  
  549. /* solve A*x = b, where A is factorized by 
  550.    Choleski LDL^T factorization */
  551. VEC    *bdLDLsolve(A,b,x)
  552. BAND   *A;
  553. VEC    *b, *x;
  554. {
  555.    int i,j,l,n,n1,lb,ilb;
  556.    Real **Av, *Avlb;
  557.    Real c;
  558.  
  559.    if ( ! A || ! b )
  560.      error(E_NULL,"bdLDLsolve");
  561.    if ( A->mat->n != b->dim )
  562.      error(E_SIZES,"bdLDLsolve");
  563.  
  564.    n = A->mat->n;
  565.    n1 = n-1;
  566.    x = v_resize(x,n);
  567.    lb = A->lb;
  568.    Av = A->mat->me;  
  569.    Avlb = Av[lb];
  570.    
  571.    /* solve L*y = b */
  572.    x->ve[0] = b->ve[0];
  573.    for (i=1; i < n; i++) {
  574.       ilb = i-lb;
  575.       c = b->ve[i];
  576.       for (j=max(0,ilb), l=j-ilb; j < i; j++,l++)
  577.     c -= Av[l][j]*x->ve[j];
  578.       x->ve[i] = c;
  579.    }
  580.  
  581.    /* solve D*z = y */
  582.    for (i=0; i < n; i++) 
  583.      x->ve[i] /= Avlb[i];
  584.  
  585.    /* solve L^T*x = z */
  586.    for (i=n-2; i >= 0; i--) {
  587.       ilb = i+lb;
  588.       c = x->ve[i];
  589.       for (j=min(n1,ilb), l=ilb-j; j > i; j--,l++)
  590.     c -= Av[l][i]*x->ve[j];
  591.       x->ve[i] = c;
  592.    }
  593.  
  594.    return x;
  595. }
  596.  
  597.  
  598.  
  599.