clean up of consistent-dataframe-p generic/methods. They were subtly wrong.
[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;
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;
23 return(maxx * sqrt(sum));
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];
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;
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;
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;
79 work = v[0];
80 qraux = rvector(p);
83 * compute the norms of the free columns.
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];
92 * perform the householder reduction of x.
94 lup = (n < p) ? n : p;
95 for (l = 0; l < lup; l++) {
96 if (pivot) {
98 * locate the column of largest norm and bring it
99 * into the pivot position.
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;
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;
117 qraux[l] = 0.0;
118 if (l != n-1) {
120 * compute the householder transformation for column l.
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];
129 * apply the transformation to the remaining columns,
130 * updating the norms.
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];
151 * save the transformation.
153 qraux[l] = x[l][l];
154 x[l][l] = -nrmxl;
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];
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;
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;
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);
185 free_vector(qraux);