home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / chfactor < prev    next >
Encoding:
Text File  |  1994-01-13  |  4.9 KB  |  218 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. /* CHfactor.c 1.2 11/25/87 */
  32. static    char    rcsid[] = "$Id: chfactor.c,v 1.2 1994/01/13 05:36:36 des Exp $";
  33.  
  34. #include    <stdio.h>
  35. #include    <math.h>
  36. #include    "matrix.h"
  37. #include        "matrix2.h"
  38.  
  39.  
  40. /* Most matrix factorisation routines are in-situ unless otherwise specified */
  41.  
  42. /* CHfactor -- Cholesky L.L' factorisation of A in-situ */
  43. MAT    *CHfactor(A)
  44. MAT    *A;
  45. {
  46.     u_int    i, j, k, n;
  47.     Real    **A_ent, *A_piv, *A_row, sum, tmp;
  48.  
  49.     if ( A==(MAT *)NULL )
  50.         error(E_NULL,"CHfactor");
  51.     if ( A->m != A->n )
  52.         error(E_SQUARE,"CHfactor");
  53.     n = A->n;    A_ent = A->me;
  54.  
  55.     for ( k=0; k<n; k++ )
  56.     {    
  57.         /* do diagonal element */
  58.         sum = A_ent[k][k];
  59.         A_piv = A_ent[k];
  60.         for ( j=0; j<k; j++ )
  61.         {
  62.             /* tmp = A_ent[k][j]; */
  63.             tmp = *A_piv++;
  64.             sum -= tmp*tmp;
  65.         }
  66.         if ( sum <= 0.0 )
  67.             error(E_POSDEF,"CHfactor");
  68.         A_ent[k][k] = sqrt(sum);
  69.  
  70.         /* set values of column k */
  71.         for ( i=k+1; i<n; i++ )
  72.         {
  73.             sum = A_ent[i][k];
  74.             A_piv = A_ent[k];
  75.             A_row = A_ent[i];
  76.             sum -= __ip__(A_row,A_piv,(int)k);
  77.             /************************************************
  78.             for ( j=0; j<k; j++ )
  79.                 sum -= A_ent[i][j]*A_ent[k][j];
  80.                 sum -= (*A_row++)*(*A_piv++);
  81.             ************************************************/
  82.             A_ent[j][i] = A_ent[i][j] = sum/A_ent[k][k];
  83.         }
  84.     }
  85.  
  86.     return (A);
  87. }
  88.  
  89.  
  90. /* CHsolve -- given a CHolesky factorisation in A, solve A.x=b */
  91. VEC    *CHsolve(A,b,x)
  92. MAT    *A;
  93. VEC    *b,*x;
  94. {
  95.     if ( A==(MAT *)NULL || b==(VEC *)NULL )
  96.         error(E_NULL,"CHsolve");
  97.     if ( A->m != A->n || A->n != b->dim )
  98.         error(E_SIZES,"CHsolve");
  99.     x = v_resize(x,b->dim);
  100.     Lsolve(A,b,x,0.0);
  101.     Usolve(A,x,x,0.0);
  102.  
  103.     return (x);
  104. }
  105.  
  106. /* LDLfactor -- L.D.L' factorisation of A in-situ */
  107. MAT    *LDLfactor(A)
  108. MAT    *A;
  109. {
  110.     u_int    i, k, n, p;
  111.     Real    **A_ent;
  112.     Real d, sum;
  113.     static VEC    *r = VNULL;
  114.  
  115.     if ( ! A )
  116.         error(E_NULL,"LDLfactor");
  117.     if ( A->m != A->n )
  118.         error(E_SQUARE,"LDLfactor");
  119.     n = A->n;    A_ent = A->me;
  120.     r = v_resize(r,n);
  121.     MEM_STAT_REG(r,TYPE_VEC);
  122.  
  123.     for ( k = 0; k < n; k++ )
  124.     {
  125.         sum = 0.0;
  126.         for ( p = 0; p < k; p++ )
  127.         {
  128.             r->ve[p] = A_ent[p][p]*A_ent[k][p];
  129.             sum += r->ve[p]*A_ent[k][p];
  130.         }
  131.         d = A_ent[k][k] -= sum;
  132.  
  133.         if ( d == 0.0 )
  134.             error(E_SING,"LDLfactor");
  135.         for ( i = k+1; i < n; i++ )
  136.         {
  137.             sum = __ip__(A_ent[i],r->ve,(int)k);
  138.             /****************************************
  139.             sum = 0.0;
  140.             for ( p = 0; p < k; p++ )
  141.             sum += A_ent[i][p]*r->ve[p];
  142.             ****************************************/
  143.             A_ent[i][k] = (A_ent[i][k] - sum)/d;
  144.         }
  145.     }
  146.  
  147.     return A;
  148. }
  149.  
  150. VEC    *LDLsolve(LDL,b,x)
  151. MAT    *LDL;
  152. VEC    *b, *x;
  153. {
  154.     if ( ! LDL || ! b )
  155.         error(E_NULL,"LDLsolve");
  156.     if ( LDL->m != LDL->n )
  157.         error(E_SQUARE,"LDLsolve");
  158.     if ( LDL->m != b->dim )
  159.         error(E_SIZES,"LDLsolve");
  160.     x = v_resize(x,b->dim);
  161.  
  162.     Lsolve(LDL,b,x,1.0);
  163.     Dsolve(LDL,x,x);
  164.     LTsolve(LDL,x,x,1.0);
  165.  
  166.     return x;
  167. }
  168.  
  169. /* MCHfactor -- Modified Cholesky L.L' factorisation of A in-situ */
  170. MAT    *MCHfactor(A,tol)
  171. MAT    *A;
  172. double  tol;
  173. {
  174.     u_int    i, j, k, n;
  175.     Real    **A_ent, *A_piv, *A_row, sum, tmp;
  176.  
  177.     if ( A==(MAT *)NULL )
  178.         error(E_NULL,"MCHfactor");
  179.     if ( A->m != A->n )
  180.         error(E_SQUARE,"MCHfactor");
  181.     if ( tol <= 0.0 )
  182.             error(E_RANGE,"MCHfactor");
  183.     n = A->n;    A_ent = A->me;
  184.  
  185.     for ( k=0; k<n; k++ )
  186.     {    
  187.         /* do diagonal element */
  188.         sum = A_ent[k][k];
  189.         A_piv = A_ent[k];
  190.         for ( j=0; j<k; j++ )
  191.         {
  192.             /* tmp = A_ent[k][j]; */
  193.             tmp = *A_piv++;
  194.             sum -= tmp*tmp;
  195.         }
  196.         if ( sum <= tol )
  197.             sum = tol;
  198.         A_ent[k][k] = sqrt(sum);
  199.  
  200.         /* set values of column k */
  201.         for ( i=k+1; i<n; i++ )
  202.         {
  203.             sum = A_ent[i][k];
  204.             A_piv = A_ent[k];
  205.             A_row = A_ent[i];
  206.             sum -= __ip__(A_row,A_piv,(int)k);
  207.             /************************************************
  208.             for ( j=0; j<k; j++ )
  209.                 sum -= A_ent[i][j]*A_ent[k][j];
  210.                 sum -= (*A_row++)*(*A_piv++);
  211.             ************************************************/
  212.             A_ent[j][i] = A_ent[i][j] = sum/A_ent[k][k];
  213.         }
  214.     }
  215.  
  216.     return (A);
  217. }
  218.