update lift to 29.11.2007 version
[CommonLispStat.git] / lib / qrdecomp.c
blob8e04db86ade37df80e984423f994fec49f77f1cb
1 /* adapted from DQRDC of LINPACK */
3 #include "linalg.h"
5 #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
7 static double
8 NORM2(int i, int j, int n, double **x)
10   int k;
11   double maxx, sum, temp;
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   }
27 static double
28 DOT(int i, int j, int k, int n, double **x)
30   int l;
31   double sum;
33   for (l = i, sum = 0.0; l < n; l++) sum += x[l][j] * x[l][k];
34   return(sum);
37 static void
38 AXPY(int i, int j, int k, int n,
39      double a, double **x)
41   int l;
42   for (l = i; l < n; l++) {
43     x[l][k] = a * x[l][j] + x[l][k];
44   }
47 static void
48 SCALE(int i, int j, int n, double a, double **x)
50   int k;
51   for (k = i; k < n; k++) {
52     x[k][j] *= a;
53   }
56 static void
57 SWAP(int i, int j, int n, double **a)
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   }
68 void
69 qrdecomp(double **x, int n, int p,
70          double **v, int *jpvt, int pivot)
72   int i,j,k,jp,l,lp1,lup,maxj;
73   double maxnrm,tt,*qraux,*work;
74   double nrmxl,t;
76   if (n < 0) {
77     return;
78   }
79   work = v[0];
80   qraux = rvector(p);
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   }
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);