lib/qrdecomp.c

   1 /* adapted from DQRDC of LINPACK */
   2
   3 #include "linalg.h"
   4
   5 #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
   6
   7 static double NORM2(i, j, n, x)
   8      int i, j, n;
   9      double **x;
  10 {
  11   int k;
  12   double maxx, sum, temp;
  13
  14   for (k = i, maxx = 0.0; k < n; k++) {
  15     temp = fabs(x[k][j]);
  16     if (maxx < temp) maxx = temp;
  17   }
  18   if (maxx == 0.0) return(0.0);
  19   else {
  20     for (k = i, sum = 0.0; k < n; k++) {
  21       temp = x[k][j] / maxx;
  22       sum += temp * temp;
  23     }
  24     return(maxx * sqrt(sum));
  25   }
  26 }
  27
  28 static double DOT(i, j, k, n, x)
  29      int i, j, k;
  30      double **x;
  31 {
  32   int l;
  33   double sum;
  34
  35   for (l = i, sum = 0.0; l < n; l++) sum += x[l][j] * x[l][k];
  36   return(sum);
  37 }
  38
  39 static AXPY(i, j, k, n, a, x)
  40      int i, j, k, n;
  41      double a, **x;
  42 {
  43   int l;
  44   for (l = i; l < n; l++) x[l][k] = a * x[l][j] + x[l][k];
  45 }
  46
  47 static SCALE(i, j, n, a, x)
  48      int i, j, n;
  49      double a, **x;
  50 {
  51   int k;
  52   for (k = i; k < n; k++) x[k][j] *= a;
  53 }
  54
  55 static SWAP(i, j, n, a)
  56   int i, j, n;
  57   double **a;
  58 {
  59   int k;
  60   double temp;
  61   for (k = 0; k < n; k++) {
  62     temp = a[k][i];
  63     a[k][i] = a[k][j];
  64     a[k][j] = temp;
  65   }
  66 }
  67
  68 qrdecomp(x,n,p,v,jpvt,pivot)
  69      int n, p, pivot;
  70      int *jpvt;
  71      double **x, **v;
  72 {
  73   int i,j,k,jp,l,lp1,lup,maxj;
  74   double maxnrm,tt,*qraux,*work;
  75   double nrmxl,t;
  76
  77   if (n < 0) return;
  78   work = v[0];
  79   qraux = rvector(p);
  80
  81   /*
  82    * compute the norms of the free columns.
  83    */
  84   if (pivot)
  85     for (j = 0; j < p; j++) {
  86       jpvt[j] = j;
  87       qraux[j] = NORM2(0, j, n, x);
  88       work[j] = qraux[j];
  89     }
  90   /*
  91    * perform the householder reduction of x.
  92    */
  93   lup = (n < p) ? n : p;
  94   for (l = 0; l < lup; l++) {
  95     if (pivot) {
  96       /*
  97        * locate the column of largest norm and bring it
  98        * into the pivot position.
  99        */
 100       maxnrm = 0.0;
 101       maxj = l;
 102       for (j = l; j < p; j++)
 103         if (qraux[j]>maxnrm) {
 104           maxnrm = qraux[j];
 105           maxj = j;
 106         }
 107       if (maxj!=l) {
 108         SWAP(l, maxj, n, x);
 109         qraux[maxj] = qraux[l];
 110         work[maxj] = work[l];
 111         jp = jpvt[maxj];
 112         jpvt[maxj] = jpvt[l];
 113         jpvt[l] = jp;
 114       }
 115     }
 116     qraux[l] = 0.0;
 117     if (l != n-1) {
 118       /*
 119        * compute the householder transformation for column l.
 120        */
 121       nrmxl = NORM2(l, l, n, x);
 122       if (nrmxl != 0.0) {
 123         if (x[l][l] != 0.0)
 124           nrmxl = SIGN(nrmxl, x[l][l]);
 125         SCALE(l, l, n, 1.0 / nrmxl, x);
 126         x[l][l] = 1.0+x[l][l];
 127         /*
 128          * apply the transformation to the remaining columns,
 129          * updating the norms.
 130          */
 131         lp1 = l+1;
 132         for (j = lp1; j < p; j++) {
 133           t = -DOT(l, l, j, n, x) / x[l][l];
 134           AXPY(l, l, j, n, t, x);
 135           if (pivot)
 136             if (qraux[j]!=0.0) {
 137               tt = 1.0-(fabs(x[l][j])/qraux[j])*(fabs(x[l][j])/qraux[j]);
 138               if (tt < 0.0) tt = 0.0;
 139               t = tt;
 140               tt = 1.0+0.05*tt*(qraux[j]/work[j])*(qraux[j]/work[j]);
 141               if (tt!=1.0)
 142                 qraux[j] = qraux[j]*sqrt(t);
 143               else {
 144                 qraux[j] = NORM2(l+1, j, n, x);
 145                 work[j] = qraux[j];
 146               }
 147             }
 148         }
 149         /*
 150          * save the transformation.
 151          */
 152         qraux[l] = x[l][l];
 153         x[l][l] = -nrmxl;
 154       }
 155     }
 156   }
 157
 158   /* copy over the upper triangle of a */
 159   for (i = 0; i < p; i++) {
 160     for (j = 0; j < i; j++) v[i][j] = 0.0;
 161     for (j = i; j < p; j++) {
 162       v[i][j] = x[i][j];
 163     }
 164   }
 165
 166   /* accumulate the Q transformation -- assumes p <= n */
 167   for (i = 0; i < p; i++) {
 168     x[i][i] = qraux[i];
 169     for (k = 0; k < i; k++) x[k][i] = 0.0;
 170   }
 171   for (i = p - 1; i >= 0; i--) {
 172     if (i == n - 1) x[i][i] = 1.0;
 173     else {
 174       for (k = i; k < n; k++) x[k][i] = -x[k][i];
 175       x[i][i] += 1.0;
 176     }
 177     for (j = i - 1; j >= 0; j--) {
 178       if (x[j][j] != 0.0) {
 179         t = -DOT(j, j, i, n, x) / x[j][j];
 180         AXPY(j, j, i, n, t, x);
 181       }
 182     }
 183   }
 184
 185   free_vector(qraux);
 186 }