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