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