more ANSI cleanups, SBCL compile note fixes
[tsl.git] / lib / qrdecomp.c
blobb7c97080561ead2cf954023cf313b3a35aabaebb
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 NORM2(i, j, n, x)
8 int i, j, n;
9 double **x;
11 int k;
12 double maxx, sum, temp;
14 for (k = i, maxx = 0.0; k < n; k++) {
15 temp = fabs(x[k][j]);
16 if (maxx < temp) maxx = temp;
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;
24 return(maxx * sqrt(sum));
28 static double DOT(i, j, k, n, x)
29 int i, j, k;
30 double **x;
32 int l;
33 double sum;
35 for (l = i, sum = 0.0; l < n; l++) sum += x[l][j] * x[l][k];
36 return(sum);
39 static AXPY(i, j, k, n, a, x)
40 int i, j, k, n;
41 double a, **x;
43 int l;
44 for (l = i; l < n; l++) x[l][k] = a * x[l][j] + x[l][k];
47 static SCALE(i, j, n, a, x)
48 int i, j, n;
49 double a, **x;
51 int k;
52 for (k = i; k < n; k++) x[k][j] *= a;
55 static SWAP(i, j, n, a)
56 int i, j, n;
57 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;
68 qrdecomp(x,n,p,v,jpvt,pivot)
69 int n, p, pivot;
70 int *jpvt;
71 double **x, **v;
73 int i,j,k,jp,l,lp1,lup,maxj;
74 double maxnrm,tt,*qraux,*work;
75 double nrmxl,t;
77 if (n < 0) return;
78 work = v[0];
79 qraux = rvector(p);
82 * compute the norms of the free columns.
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];
91 * perform the householder reduction of x.
93 lup = (n < p) ? n : p;
94 for (l = 0; l < lup; l++) {
95 if (pivot) {
97 * locate the column of largest norm and bring it
98 * into the pivot position.
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;
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;
116 qraux[l] = 0.0;
117 if (l != n-1) {
119 * compute the householder transformation for column l.
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];
128 * apply the transformation to the remaining columns,
129 * updating the norms.
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];
150 * save the transformation.
152 qraux[l] = x[l][l];
153 x[l][l] = -nrmxl;
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];
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;
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;
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);
185 free_vector(qraux);