home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / solve < prev    next >
Encoding:
Text File  |  1994-01-13  |  6.4 KB  |  278 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. /* solve.c 1.2 11/25/87 */
  32. static    char    rcsid[] = "$Id: solve.c,v 1.3 1994/01/13 05:29:57 des Exp $";
  33.  
  34. #include    <stdio.h>
  35. #include    <math.h>
  36. #include        "matrix2.h"
  37.  
  38.  
  39.  
  40.  
  41.  
  42. /* Most matrix factorisation routines are in-situ unless otherwise specified */
  43.  
  44. /* Usolve -- back substitution with optional over-riding diagonal
  45.         -- can be in-situ but doesn't need to be */
  46. VEC    *Usolve(matrix,b,out,diag)
  47. MAT    *matrix;
  48. VEC    *b, *out;
  49. double    diag;
  50. {
  51.     u_int    dim /* , j */;
  52.     int    i, i_lim;
  53.     Real    **mat_ent, *mat_row, *b_ent, *out_ent, *out_col, sum;
  54.  
  55.     if ( matrix==(MAT *)NULL || b==(VEC *)NULL )
  56.         error(E_NULL,"Usolve");
  57.     dim = min(matrix->m,matrix->n);
  58.     if ( b->dim < dim )
  59.         error(E_SIZES,"Usolve");
  60.     if ( out==(VEC *)NULL || out->dim < dim )
  61.         out = v_resize(out,matrix->n);
  62.     mat_ent = matrix->me;    b_ent = b->ve;    out_ent = out->ve;
  63.  
  64.     for ( i=dim-1; i>=0; i-- )
  65.         if ( b_ent[i] != 0.0 )
  66.             break;
  67.         else
  68.             out_ent[i] = 0.0;
  69.     i_lim = i;
  70.  
  71.     for (    ; i>=0; i-- )
  72.     {
  73.         sum = b_ent[i];
  74.         mat_row = &(mat_ent[i][i+1]);
  75.         out_col = &(out_ent[i+1]);
  76.         sum -= __ip__(mat_row,out_col,i_lim-i);
  77.         /******************************************************
  78.         for ( j=i+1; j<=i_lim; j++ )
  79.             sum -= mat_ent[i][j]*out_ent[j];
  80.             sum -= (*mat_row++)*(*out_col++);
  81.         ******************************************************/
  82.         if ( diag==0.0 )
  83.         {
  84.             if ( mat_ent[i][i]==0.0 )
  85.                 error(E_SING,"Usolve");
  86.             else
  87.                 out_ent[i] = sum/mat_ent[i][i];
  88.         }
  89.         else
  90.             out_ent[i] = sum/diag;
  91.     }
  92.  
  93.     return (out);
  94. }
  95.  
  96. /* Lsolve -- forward elimination with (optional) default diagonal value */
  97. VEC    *Lsolve(matrix,b,out,diag)
  98. MAT    *matrix;
  99. VEC    *b,*out;
  100. double    diag;
  101. {
  102.     u_int    dim, i, i_lim /* , j */;
  103.     Real    **mat_ent, *mat_row, *b_ent, *out_ent, *out_col, sum;
  104.  
  105.     if ( matrix==(MAT *)NULL || b==(VEC *)NULL )
  106.         error(E_NULL,"Lsolve");
  107.     dim = min(matrix->m,matrix->n);
  108.     if ( b->dim < dim )
  109.         error(E_SIZES,"Lsolve");
  110.     if ( out==(VEC *)NULL || out->dim < dim )
  111.         out = v_resize(out,matrix->n);
  112.     mat_ent = matrix->me;    b_ent = b->ve;    out_ent = out->ve;
  113.  
  114.     for ( i=0; i<dim; i++ )
  115.         if ( b_ent[i] != 0.0 )
  116.             break;
  117.         else
  118.             out_ent[i] = 0.0;
  119.     i_lim = i;
  120.  
  121.     for (    ; i<dim; i++ )
  122.     {
  123.         sum = b_ent[i];
  124.         mat_row = &(mat_ent[i][i_lim]);
  125.         out_col = &(out_ent[i_lim]);
  126.         sum -= __ip__(mat_row,out_col,(int)(i-i_lim));
  127.         /*****************************************************
  128.         for ( j=i_lim; j<i; j++ )
  129.             sum -= mat_ent[i][j]*out_ent[j];
  130.             sum -= (*mat_row++)*(*out_col++);
  131.         ******************************************************/
  132.         if ( diag==0.0 )
  133.         {
  134.             if ( mat_ent[i][i]==0.0 )
  135.                 error(E_SING,"Lsolve");
  136.             else
  137.                 out_ent[i] = sum/mat_ent[i][i];
  138.         }
  139.         else
  140.             out_ent[i] = sum/diag;
  141.     }
  142.  
  143.     return (out);
  144. }
  145.  
  146.  
  147. /* UTsolve -- forward elimination with (optional) default diagonal value
  148.         using UPPER triangular part of matrix */
  149. VEC    *UTsolve(U,b,out,diag)
  150. MAT    *U;
  151. VEC    *b,*out;
  152. double    diag;
  153. {
  154.     u_int    dim, i, i_lim;
  155.     Real    **U_me, *b_ve, *out_ve, tmp, invdiag;
  156.     
  157.     if ( ! U || ! b )
  158.     error(E_NULL,"UTsolve");
  159.     dim = min(U->m,U->n);
  160.     if ( b->dim < dim )
  161.     error(E_SIZES,"UTsolve");
  162.     out = v_resize(out,U->n);
  163.     U_me = U->me;    b_ve = b->ve;    out_ve = out->ve;
  164.     
  165.     for ( i=0; i<dim; i++ )
  166.     if ( b_ve[i] != 0.0 )
  167.         break;
  168.     else
  169.         out_ve[i] = 0.0;
  170.     i_lim = i;
  171.     if ( b != out )
  172.     {
  173.     __zero__(out_ve,out->dim);
  174.     MEM_COPY(&(b_ve[i_lim]),&(out_ve[i_lim]),(dim-i_lim)*sizeof(Real));
  175.     }
  176.  
  177.     if ( diag == 0.0 )
  178.     {
  179.     for (    ; i<dim; i++ )
  180.     {
  181.         tmp = U_me[i][i];
  182.         if ( tmp == 0.0 )
  183.         error(E_SING,"UTsolve");
  184.         out_ve[i] /= tmp;
  185.         __mltadd__(&(out_ve[i+1]),&(U_me[i][i+1]),-out_ve[i],dim-i-1);
  186.     }
  187.     }
  188.     else
  189.     {
  190.     invdiag = 1.0/diag;
  191.     for (    ; i<dim; i++ )
  192.     {
  193.         out_ve[i] *= invdiag;
  194.         __mltadd__(&(out_ve[i+1]),&(U_me[i][i+1]),-out_ve[i],dim-i-1);
  195.     }
  196.     }
  197.     return (out);
  198. }
  199.  
  200. /* Dsolve -- solves Dx=b where D is the diagonal of A -- may be in-situ */
  201. VEC    *Dsolve(A,b,x)
  202. MAT    *A;
  203. VEC    *b,*x;
  204. {
  205.     u_int    dim, i;
  206.     
  207.     if ( ! A || ! b )
  208.     error(E_NULL,"Dsolve");
  209.     dim = min(A->m,A->n);
  210.     if ( b->dim < dim )
  211.     error(E_SIZES,"Dsolve");
  212.     x = v_resize(x,A->n);
  213.     
  214.     dim = b->dim;
  215.     for ( i=0; i<dim; i++ )
  216.     if ( A->me[i][i] == 0.0 )
  217.         error(E_SING,"Dsolve");
  218.     else
  219.         x->ve[i] = b->ve[i]/A->me[i][i];
  220.     
  221.     return (x);
  222. }
  223.  
  224. /* LTsolve -- back substitution with optional over-riding diagonal
  225.         using the LOWER triangular part of matrix
  226.         -- can be in-situ but doesn't need to be */
  227. VEC    *LTsolve(L,b,out,diag)
  228. MAT    *L;
  229. VEC    *b, *out;
  230. double    diag;
  231. {
  232.     u_int    dim;
  233.     int        i, i_lim;
  234.     Real    **L_me, *b_ve, *out_ve, tmp, invdiag;
  235.     
  236.     if ( ! L || ! b )
  237.     error(E_NULL,"LTsolve");
  238.     dim = min(L->m,L->n);
  239.     if ( b->dim < dim )
  240.     error(E_SIZES,"LTsolve");
  241.     out = v_resize(out,L->n);
  242.     L_me = L->me;    b_ve = b->ve;    out_ve = out->ve;
  243.     
  244.     for ( i=dim-1; i>=0; i-- )
  245.     if ( b_ve[i] != 0.0 )
  246.         break;
  247.     i_lim = i;
  248.  
  249.     if ( b != out )
  250.     {
  251.     __zero__(out_ve,out->dim);
  252.     MEM_COPY(b_ve,out_ve,(i_lim+1)*sizeof(Real));
  253.     }
  254.  
  255.     if ( diag == 0.0 )
  256.     {
  257.     for (        ; i>=0; i-- )
  258.     {
  259.         tmp = L_me[i][i];
  260.         if ( tmp == 0.0 )
  261.         error(E_SING,"LTsolve");
  262.         out_ve[i] /= tmp;
  263.         __mltadd__(out_ve,L_me[i],-out_ve[i],i);
  264.     }
  265.     }
  266.     else
  267.     {
  268.     invdiag = 1.0/diag;
  269.     for (        ; i>=0; i-- )
  270.     {
  271.         out_ve[i] *= invdiag;
  272.         __mltadd__(out_ve,L_me[i],-out_ve[i],i);
  273.     }
  274.     }
  275.     
  276.     return (out);
  277. }
  278.