home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 1 / RISC_DISC_1.iso / pd_share / code / meschach / !Meschach / c / zqrfctr < prev    next >
Encoding:
Text File  |  1994-08-04  |  13.5 KB  |  526 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.   This file contains the routines needed to perform QR factorisation
  28.   of matrices, as well as Householder transformations.
  29.   The internal "factored form" of a matrix A is not quite standard.
  30.   The diagonal of A is replaced by the diagonal of R -- not by the 1st non-zero
  31.   entries of the Householder vectors. The 1st non-zero entries are held in
  32.   the diag parameter of QRfactor(). The reason for this non-standard
  33.   representation is that it enables direct use of the Usolve() function
  34.   rather than requiring that  a seperate function be written just for this case.
  35.   See, e.g., QRsolve() below for more details.
  36.  
  37.   Complex version
  38.   
  39. */
  40.  
  41. static    char    rcsid[] = "$Id: zqrfctr.c,v 1.1 1994/01/13 04:21:22 des Exp $";
  42.  
  43. #include    <stdio.h>
  44. #include    <math.h>
  45. #include    "zmatrix.h"
  46. #include    "zmatrix2.h" 
  47.  
  48.  
  49. #define    is_zero(z)    ((z).re == 0.0 && (z).im == 0.0)
  50.  
  51.  
  52. #define        sign(x)    ((x) > 0.0 ? 1 : ((x) < 0.0 ? -1 : 0 ))
  53.  
  54. /* Note: The usual representation of a Householder transformation is taken
  55.    to be:
  56.    P = I - beta.u.u*
  57.    where beta = 2/(u*.u) and u is called the Householder vector
  58.    (u* is the conjugate transposed vector of u
  59. */
  60.  
  61. /* zQRfactor -- forms the QR factorisation of A
  62.     -- factorisation stored in compact form as described above
  63.     (not quite standard format) */
  64. ZMAT    *zQRfactor(A,diag)
  65. ZMAT    *A;
  66. ZVEC    *diag;
  67. {
  68.     u_int    k,limit;
  69.     Real    beta;
  70.     static    ZVEC    *tmp1=ZVNULL;
  71.     
  72.     if ( ! A || ! diag )
  73.     error(E_NULL,"zQRfactor");
  74.     limit = min(A->m,A->n);
  75.     if ( diag->dim < limit )
  76.     error(E_SIZES,"zQRfactor");
  77.     
  78.     tmp1 = zv_resize(tmp1,A->m);
  79.     MEM_STAT_REG(tmp1,TYPE_ZVEC);
  80.     
  81.     for ( k=0; k<limit; k++ )
  82.     {
  83.     /* get H/holder vector for the k-th column */
  84.     zget_col(A,k,tmp1);
  85.     /* hhvec(tmp1,k,&beta->ve[k],tmp1,&A->me[k][k]); */
  86.     zhhvec(tmp1,k,&beta,tmp1,&A->me[k][k]);
  87.     diag->ve[k] = tmp1->ve[k];
  88.     
  89.     /* apply H/holder vector to remaining columns */
  90.     /* hhtrcols(A,k,k+1,tmp1,beta->ve[k]); */
  91.     tracecatch(zhhtrcols(A,k,k+1,tmp1,beta),"zQRfactor");
  92.     }
  93.  
  94.     return (A);
  95. }
  96.  
  97. /* zQRCPfactor -- forms the QR factorisation of A with column pivoting
  98.    -- factorisation stored in compact form as described above
  99.    ( not quite standard format )                */
  100. ZMAT    *zQRCPfactor(A,diag,px)
  101. ZMAT    *A;
  102. ZVEC    *diag;
  103. PERM    *px;
  104. {
  105.     u_int    i, i_max, j, k, limit;
  106.     static    ZVEC    *tmp1=ZVNULL, *tmp2=ZVNULL;
  107.     static    VEC    *gamma=VNULL;
  108.     Real     beta;
  109.     Real    maxgamma, sum, tmp;
  110.     complex    ztmp;
  111.     
  112.     if ( ! A || ! diag || ! px )
  113.     error(E_NULL,"QRCPfactor");
  114.     limit = min(A->m,A->n);
  115.     if ( diag->dim < limit || px->size != A->n )
  116.     error(E_SIZES,"QRCPfactor");
  117.     
  118.     tmp1 = zv_resize(tmp1,A->m);
  119.     tmp2 = zv_resize(tmp2,A->m);
  120.     gamma = v_resize(gamma,A->n);
  121.     MEM_STAT_REG(tmp1,TYPE_ZVEC);
  122.     MEM_STAT_REG(tmp2,TYPE_ZVEC);
  123.     MEM_STAT_REG(gamma,TYPE_VEC);
  124.     
  125.     /* initialise gamma and px */
  126.     for ( j=0; j<A->n; j++ )
  127.     {
  128.     px->pe[j] = j;
  129.     sum = 0.0;
  130.     for ( i=0; i<A->m; i++ )
  131.         sum += square(A->me[i][j].re) + square(A->me[i][j].im);
  132.     gamma->ve[j] = sum;
  133.     }
  134.     
  135.     for ( k=0; k<limit; k++ )
  136.     {
  137.     /* find "best" column to use */
  138.     i_max = k;    maxgamma = gamma->ve[k];
  139.     for ( i=k+1; i<A->n; i++ )
  140.         /* Loop invariant:maxgamma=gamma[i_max]
  141.            >=gamma[l];l=k,...,i-1 */
  142.         if ( gamma->ve[i] > maxgamma )
  143.         {    maxgamma = gamma->ve[i]; i_max = i;    }
  144.     
  145.     /* swap columns if necessary */
  146.     if ( i_max != k )
  147.     {
  148.         /* swap gamma values */
  149.         tmp = gamma->ve[k];
  150.         gamma->ve[k] = gamma->ve[i_max];
  151.         gamma->ve[i_max] = tmp;
  152.         
  153.         /* update column permutation */
  154.         px_transp(px,k,i_max);
  155.         
  156.         /* swap columns of A */
  157.         for ( i=0; i<A->m; i++ )
  158.         {
  159.         ztmp = A->me[i][k];
  160.         A->me[i][k] = A->me[i][i_max];
  161.         A->me[i][i_max] = ztmp;
  162.         }
  163.     }
  164.     
  165.     /* get H/holder vector for the k-th column */
  166.     zget_col(A,k,tmp1);
  167.     /* hhvec(tmp1,k,&beta->ve[k],tmp1,&A->me[k][k]); */
  168.     zhhvec(tmp1,k,&beta,tmp1,&A->me[k][k]);
  169.     diag->ve[k] = tmp1->ve[k];
  170.     
  171.     /* apply H/holder vector to remaining columns */
  172.     /* hhtrcols(A,k,k+1,tmp1,beta->ve[k]); */
  173.     zhhtrcols(A,k,k+1,tmp1,beta);
  174.     
  175.     /* update gamma values */
  176.     for ( j=k+1; j<A->n; j++ )
  177.         gamma->ve[j] -= square(A->me[k][j].re)+square(A->me[k][j].im);
  178.     }
  179.  
  180.     return (A);
  181. }
  182.  
  183. /* zQsolve -- solves Qx = b, Q is an orthogonal matrix stored in compact
  184.     form a la QRfactor()
  185.     -- may be in-situ */
  186. ZVEC    *_zQsolve(QR,diag,b,x,tmp)
  187. ZMAT    *QR;
  188. ZVEC    *diag, *b, *x, *tmp;
  189. {
  190.     u_int    dynamic;
  191.     int        k, limit;
  192.     Real    beta, r_ii, tmp_val;
  193.     
  194.     limit = min(QR->m,QR->n);
  195.     dynamic = FALSE;
  196.     if ( ! QR || ! diag || ! b )
  197.     error(E_NULL,"_zQsolve");
  198.     if ( diag->dim < limit || b->dim != QR->m )
  199.     error(E_SIZES,"_zQsolve");
  200.     x = zv_resize(x,QR->m);
  201.     if ( tmp == ZVNULL )
  202.     dynamic = TRUE;
  203.     tmp = zv_resize(tmp,QR->m);
  204.     
  205.     /* apply H/holder transforms in normal order */
  206.     x = zv_copy(b,x);
  207.     for ( k = 0 ; k < limit ; k++ )
  208.     {
  209.     zget_col(QR,k,tmp);
  210.     r_ii = zabs(tmp->ve[k]);
  211.     tmp->ve[k] = diag->ve[k];
  212.     tmp_val = (r_ii*zabs(diag->ve[k]));
  213.     beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
  214.     /* hhtrvec(tmp,beta->ve[k],k,x,x); */
  215.     zhhtrvec(tmp,beta,k,x,x);
  216.     }
  217.     
  218.     if ( dynamic )
  219.     ZV_FREE(tmp);
  220.     
  221.     return (x);
  222. }
  223.  
  224. /* zmakeQ -- constructs orthogonal matrix from Householder vectors stored in
  225.    compact QR form */
  226. ZMAT    *zmakeQ(QR,diag,Qout)
  227. ZMAT    *QR,*Qout;
  228. ZVEC    *diag;
  229. {
  230.     static    ZVEC    *tmp1=ZVNULL,*tmp2=ZVNULL;
  231.     u_int    i, limit;
  232.     Real    beta, r_ii, tmp_val;
  233.     int    j;
  234.  
  235.     limit = min(QR->m,QR->n);
  236.     if ( ! QR || ! diag )
  237.     error(E_NULL,"zmakeQ");
  238.     if ( diag->dim < limit )
  239.     error(E_SIZES,"zmakeQ");
  240.     Qout = zm_resize(Qout,QR->m,QR->m);
  241.  
  242.     tmp1 = zv_resize(tmp1,QR->m);    /* contains basis vec & columns of Q */
  243.     tmp2 = zv_resize(tmp2,QR->m);    /* contains H/holder vectors */
  244.     MEM_STAT_REG(tmp1,TYPE_ZVEC);
  245.     MEM_STAT_REG(tmp2,TYPE_ZVEC);
  246.  
  247.     for ( i=0; i<QR->m ; i++ )
  248.     {    /* get i-th column of Q */
  249.     /* set up tmp1 as i-th basis vector */
  250.     for ( j=0; j<QR->m ; j++ )
  251.         tmp1->ve[j].re = tmp1->ve[j].im = 0.0;
  252.     tmp1->ve[i].re = 1.0;
  253.     
  254.     /* apply H/h transforms in reverse order */
  255.     for ( j=limit-1; j>=0; j-- )
  256.     {
  257.         zget_col(QR,j,tmp2);
  258.         r_ii = zabs(tmp2->ve[j]);
  259.         tmp2->ve[j] = diag->ve[j];
  260.         tmp_val = (r_ii*zabs(diag->ve[j]));
  261.         beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
  262.         /* hhtrvec(tmp2,beta->ve[j],j,tmp1,tmp1); */
  263.         zhhtrvec(tmp2,beta,j,tmp1,tmp1);
  264.     }
  265.     
  266.     /* insert into Q */
  267.     zset_col(Qout,i,tmp1);
  268.     }
  269.  
  270.     return (Qout);
  271. }
  272.  
  273. /* zmakeR -- constructs upper triangular matrix from QR (compact form)
  274.     -- may be in-situ (all it does is zero the lower 1/2) */
  275. ZMAT    *zmakeR(QR,Rout)
  276. ZMAT    *QR,*Rout;
  277. {
  278.     u_int    i,j;
  279.     
  280.     if ( QR==ZMNULL )
  281.     error(E_NULL,"zmakeR");
  282.     Rout = zm_copy(QR,Rout);
  283.     
  284.     for ( i=1; i<QR->m; i++ )
  285.     for ( j=0; j<QR->n && j<i; j++ )
  286.         Rout->me[i][j].re = Rout->me[i][j].im = 0.0;
  287.     
  288.     return (Rout);
  289. }
  290.  
  291. /* zQRsolve -- solves the system Q.R.x=b where Q & R are stored in compact form
  292.    -- returns x, which is created if necessary */
  293. ZVEC    *zQRsolve(QR,diag,b,x)
  294. ZMAT    *QR;
  295. ZVEC    *diag, *b, *x;
  296. {
  297.     int    limit;
  298.     static    ZVEC    *tmp = ZVNULL;
  299.     
  300.     if ( ! QR || ! diag || ! b )
  301.     error(E_NULL,"zQRsolve");
  302.     limit = min(QR->m,QR->n);
  303.     if ( diag->dim < limit || b->dim != QR->m )
  304.     error(E_SIZES,"zQRsolve");
  305.     tmp = zv_resize(tmp,limit);
  306.     MEM_STAT_REG(tmp,TYPE_ZVEC);
  307.  
  308.     x = zv_resize(x,QR->n);
  309.     _zQsolve(QR,diag,b,x,tmp);
  310.     x = zUsolve(QR,x,x,0.0);
  311.     x = zv_resize(x,QR->n);
  312.  
  313.     return x;
  314. }
  315.  
  316. /* zQRAsolve -- solves the system (Q.R)*.x = b
  317.     -- Q & R are stored in compact form
  318.     -- returns x, which is created if necessary */
  319. ZVEC    *zQRAsolve(QR,diag,b,x)
  320. ZMAT    *QR;
  321. ZVEC    *diag, *b, *x;
  322. {
  323.     int        j, limit;
  324.     Real    beta, r_ii, tmp_val;
  325.     static    ZVEC    *tmp = ZVNULL;
  326.     
  327.     if ( ! QR || ! diag || ! b )
  328.     error(E_NULL,"zQRAsolve");
  329.     limit = min(QR->m,QR->n);
  330.     if ( diag->dim < limit || b->dim != QR->n )
  331.     error(E_SIZES,"zQRAsolve");
  332.  
  333.     x = zv_resize(x,QR->m);
  334.     x = zUAsolve(QR,b,x,0.0);
  335.     x = zv_resize(x,QR->m);
  336.  
  337.     tmp = zv_resize(tmp,x->dim);
  338.     MEM_STAT_REG(tmp,TYPE_ZVEC);
  339.     printf("zQRAsolve: tmp->dim = %d, x->dim = %d\n", tmp->dim, x->dim);
  340.     
  341.     /* apply H/h transforms in reverse order */
  342.     for ( j=limit-1; j>=0; j-- )
  343.     {
  344.     zget_col(QR,j,tmp);
  345.     tmp = zv_resize(tmp,QR->m);
  346.     r_ii = zabs(tmp->ve[j]);
  347.     tmp->ve[j] = diag->ve[j];
  348.     tmp_val = (r_ii*zabs(diag->ve[j]));
  349.     beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
  350.     zhhtrvec(tmp,beta,j,x,x);
  351.     }
  352.  
  353.  
  354.     return x;
  355. }
  356.  
  357. /* zQRCPsolve -- solves A.x = b where A is factored by QRCPfactor()
  358.    -- assumes that A is in the compact factored form */
  359. ZVEC    *zQRCPsolve(QR,diag,pivot,b,x)
  360. ZMAT    *QR;
  361. ZVEC    *diag;
  362. PERM    *pivot;
  363. ZVEC    *b, *x;
  364. {
  365.     if ( ! QR || ! diag || ! pivot || ! b )
  366.     error(E_NULL,"zQRCPsolve");
  367.     if ( (QR->m > diag->dim && QR->n > diag->dim) || QR->n != pivot->size )
  368.     error(E_SIZES,"zQRCPsolve");
  369.     
  370.     x = zQRsolve(QR,diag,b,x);
  371.     x = pxinv_zvec(pivot,x,x);
  372.  
  373.     return x;
  374. }
  375.  
  376. /* zUmlt -- compute out = upper_triang(U).x
  377.     -- may be in situ */
  378. ZVEC    *zUmlt(U,x,out)
  379. ZMAT    *U;
  380. ZVEC    *x, *out;
  381. {
  382.     int        i, limit;
  383.  
  384.     if ( U == ZMNULL || x == ZVNULL )
  385.     error(E_NULL,"zUmlt");
  386.     limit = min(U->m,U->n);
  387.     if ( limit != x->dim )
  388.     error(E_SIZES,"zUmlt");
  389.     if ( out == ZVNULL || out->dim < limit )
  390.     out = zv_resize(out,limit);
  391.  
  392.     for ( i = 0; i < limit; i++ )
  393.     out->ve[i] = __zip__(&(x->ve[i]),&(U->me[i][i]),limit - i,Z_NOCONJ);
  394.     return out;
  395. }
  396.  
  397. /* zUAmlt -- returns out = upper_triang(U)^T.x */
  398. ZVEC    *zUAmlt(U,x,out)
  399. ZMAT    *U;
  400. ZVEC    *x, *out;
  401. {
  402.     /* complex    sum; */
  403.     complex    tmp;
  404.     int        i, limit;
  405.  
  406.     if ( U == ZMNULL || x == ZVNULL )
  407.     error(E_NULL,"zUAmlt");
  408.     limit = min(U->m,U->n);
  409.     if ( out == ZVNULL || out->dim < limit )
  410.     out = zv_resize(out,limit);
  411.  
  412.     for ( i = limit-1; i >= 0; i-- )
  413.     {
  414.     tmp = x->ve[i];
  415.     out->ve[i].re = out->ve[i].im = 0.0;
  416.     __zmltadd__(&(out->ve[i]),&(U->me[i][i]),tmp,limit-i-1,Z_CONJ);
  417.     }
  418.  
  419.     return out;
  420. }
  421.  
  422.  
  423. /* zQRcondest -- returns an estimate of the 2-norm condition number of the
  424.         matrix factorised by QRfactor() or QRCPfactor()
  425.     -- note that as Q does not affect the 2-norm condition number,
  426.         it is not necessary to pass the diag, beta (or pivot) vectors
  427.     -- generates a lower bound on the true condition number
  428.     -- if the matrix is exactly singular, HUGE is returned
  429.     -- note that QRcondest() is likely to be more reliable for
  430.         matrices factored using QRCPfactor() */
  431. double    zQRcondest(QR)
  432. ZMAT    *QR;
  433. {
  434.     static    ZVEC    *y=ZVNULL;
  435.     Real    norm, norm1, norm2, tmp1, tmp2;
  436.     complex    sum, tmp;
  437.     int        i, j, limit;
  438.  
  439.     if ( QR == ZMNULL )
  440.     error(E_NULL,"zQRcondest");
  441.  
  442.     limit = min(QR->m,QR->n);
  443.     for ( i = 0; i < limit; i++ )
  444.     /* if ( QR->me[i][i] == 0.0 ) */
  445.     if ( is_zero(QR->me[i][i]) )
  446.         return HUGE_VAL;
  447.  
  448.     y = zv_resize(y,limit);
  449.     MEM_STAT_REG(y,TYPE_ZVEC);
  450.     /* use the trick for getting a unit vector y with ||R.y||_inf small
  451.        from the LU condition estimator */
  452.     for ( i = 0; i < limit; i++ )
  453.     {
  454.     sum.re = sum.im = 0.0;
  455.     for ( j = 0; j < i; j++ )
  456.         /* sum -= QR->me[j][i]*y->ve[j]; */
  457.         sum = zsub(sum,zmlt(QR->me[j][i],y->ve[j]));
  458.     /* sum -= (sum < 0.0) ? 1.0 : -1.0; */
  459.     norm1 = zabs(sum);
  460.     if ( norm1 == 0.0 )
  461.         sum.re = 1.0;
  462.     else
  463.     {
  464.         sum.re += sum.re / norm1;
  465.         sum.im += sum.im / norm1;
  466.     }
  467.     /* y->ve[i] = sum / QR->me[i][i]; */
  468.     y->ve[i] = zdiv(sum,QR->me[i][i]);
  469.     }
  470.     zUAmlt(QR,y,y);
  471.  
  472.     /* now apply inverse power method to R*.R */
  473.     for ( i = 0; i < 3; i++ )
  474.     {
  475.     tmp1 = zv_norm2(y);
  476.     zv_mlt(zmake(1.0/tmp1,0.0),y,y);
  477.     zUAsolve(QR,y,y,0.0);
  478.     tmp2 = zv_norm2(y);
  479.     zv_mlt(zmake(1.0/tmp2,0.0),y,y);
  480.     zUsolve(QR,y,y,0.0);
  481.     }
  482.     /* now compute approximation for ||R^{-1}||_2 */
  483.     norm1 = sqrt(tmp1)*sqrt(tmp2);
  484.  
  485.     /* now use complementary approach to compute approximation to ||R||_2 */
  486.     for ( i = limit-1; i >= 0; i-- )
  487.     {
  488.     sum.re = sum.im = 0.0;
  489.     for ( j = i+1; j < limit; j++ )
  490.         sum = zadd(sum,zmlt(QR->me[i][j],y->ve[j]));
  491.     if ( is_zero(QR->me[i][i]) )
  492.         return HUGE_VAL;
  493.     tmp = zdiv(sum,QR->me[i][i]);
  494.     if ( is_zero(tmp) )
  495.     {
  496.         y->ve[i].re = 1.0;
  497.         y->ve[i].im = 0.0;
  498.     }
  499.     else
  500.     {
  501.         norm = zabs(tmp);
  502.         y->ve[i].re = sum.re / norm;
  503.         y->ve[i].im = sum.im / norm;
  504.     }
  505.     /* y->ve[i] = (sum >= 0.0) ? 1.0 : -1.0; */
  506.     /* y->ve[i] = (QR->me[i][i] >= 0.0) ? y->ve[i] : - y->ve[i]; */
  507.     }
  508.  
  509.     /* now apply power method to R*.R */
  510.     for ( i = 0; i < 3; i++ )
  511.     {
  512.     tmp1 = zv_norm2(y);
  513.     zv_mlt(zmake(1.0/tmp1,0.0),y,y);
  514.     zUmlt(QR,y,y);
  515.     tmp2 = zv_norm2(y);
  516.     zv_mlt(zmake(1.0/tmp2,0.0),y,y);
  517.     zUAmlt(QR,y,y);
  518.     }
  519.     norm2 = sqrt(tmp1)*sqrt(tmp2);
  520.  
  521.     /* printf("QRcondest: norm1 = %g, norm2 = %g\n",norm1,norm2); */
  522.  
  523.     return norm1*norm2;
  524. }
  525.  
  526.