1 #include "cminpack.h"
2 #include <math.h>
3 #ifdef USE_LAPACK
4 #include <stdlib.h>
5 #include <string.h>
6 #include <assert.h>
7 #endif
8 #include "cminpackP.h"
9
10 __cminpack_attr__
__cminpack_func__(qrfac)11 void __cminpack_func__(qrfac)(int m, int n, real *a, int
12 lda, int pivot, int *ipvt, int lipvt, real *rdiag,
13 real *acnorm, real *wa)
14 {
15 #ifdef USE_LAPACK
16 __CLPK_integer m_ = m;
17 __CLPK_integer n_ = n;
18 __CLPK_integer lda_ = lda;
19 __CLPK_integer *jpvt;
20
21 int i, j, k;
22 double t;
23 double* tau = wa;
24 const __CLPK_integer ltau = m > n ? n : m;
25 __CLPK_integer lwork = -1;
26 __CLPK_integer info = 0;
27 double* work;
28
29 if (pivot) {
30 assert( lipvt >= n );
31 if (sizeof(__CLPK_integer) != sizeof(ipvt[0])) {
32 jpvt = malloc(n*sizeof(__CLPK_integer));
33 } else {
34 /* __CLPK_integer is actually an int, just do a cast */
35 jpvt = (__CLPK_integer *)ipvt;
36 }
37 /* set all columns free */
38 memset(jpvt, 0, sizeof(int)*n);
39 }
40
41 /* query optimal size of work */
42 lwork = -1;
43 if (pivot) {
44 dgeqp3_(&m_,&n_,a,&lda_,jpvt,tau,tau,&lwork,&info);
45 lwork = (int)tau[0];
46 assert( lwork >= 3*n+1 );
47 } else {
48 dgeqrf_(&m_,&n_,a,&lda_,tau,tau,&lwork,&info);
49 lwork = (int)tau[0];
50 assert( lwork >= 1 && lwork >= n );
51 }
52
53 assert( info == 0 );
54
55 /* alloc work area */
56 work = (double *)malloc(sizeof(double)*lwork);
57 assert(work != NULL);
58
59 /* set acnorm first (from the doc of qrfac, acnorm may point to the same area as rdiag) */
60 if (acnorm != rdiag) {
61 for (j = 0; j < n; ++j) {
62 acnorm[j] = __cminpack_enorm__(m, &a[j * lda]);
63 }
64 }
65
66 /* QR decomposition */
67 if (pivot) {
68 dgeqp3_(&m_,&n_,a,&lda_,jpvt,tau,work,&lwork,&info);
69 } else {
70 dgeqrf_(&m_,&n_,a,&lda_,tau,work,&lwork,&info);
71 }
72 assert(info == 0);
73
74 /* set rdiag, before the diagonal is replaced */
75 memset(rdiag, 0, sizeof(double)*n);
76 for(i=0 ; i<n ; ++i) {
77 rdiag[i] = a[i*lda+i];
78 }
79
80 /* modify lower trinagular part to look like qrfac's output */
81 for(i=0 ; i<ltau ; ++i) {
82 k = i*lda+i;
83 t = tau[i];
84 a[k] = t;
85 for(j=i+1 ; j<m ; j++) {
86 k++;
87 a[k] *= t;
88 }
89 }
90
91 free(work);
92 if (pivot) {
93 /* convert back jpvt to ipvt */
94 if (sizeof(__CLPK_integer) != sizeof(ipvt[0])) {
95 for(i=0; i<n; ++i) {
96 ipvt[i] = jpvt[i];
97 }
98 free(jpvt);
99 }
100 }
101 #else /* !USE_LAPACK */
102 /* Initialized data */
103
104 #define p05 .05
105
106 /* System generated locals */
107 real d1;
108
109 /* Local variables */
110 int i, j, k, jp1;
111 real sum;
112 real temp;
113 int minmn;
114 real epsmch;
115 real ajnorm;
116
117 /* ********** */
118
119 /* subroutine qrfac */
120
121 /* this subroutine uses householder transformations with column */
122 /* pivoting (optional) to compute a qr factorization of the */
123 /* m by n matrix a. that is, qrfac determines an orthogonal */
124 /* matrix q, a permutation matrix p, and an upper trapezoidal */
125 /* matrix r with diagonal elements of nonincreasing magnitude, */
126 /* such that a*p = q*r. the householder transformation for */
127 /* column k, k = 1,2,...,min(m,n), is of the form */
128
129 /* t */
130 /* i - (1/u(k))*u*u */
131
132 /* where u has zeros in the first k-1 positions. the form of */
133 /* this transformation and the method of pivoting first */
134 /* appeared in the corresponding linpack subroutine. */
135
136 /* the subroutine statement is */
137
138 /* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) */
139
140 /* where */
141
142 /* m is a positive integer input variable set to the number */
143 /* of rows of a. */
144
145 /* n is a positive integer input variable set to the number */
146 /* of columns of a. */
147
148 /* a is an m by n array. on input a contains the matrix for */
149 /* which the qr factorization is to be computed. on output */
150 /* the strict upper trapezoidal part of a contains the strict */
151 /* upper trapezoidal part of r, and the lower trapezoidal */
152 /* part of a contains a factored form of q (the non-trivial */
153 /* elements of the u vectors described above). */
154
155 /* lda is a positive integer input variable not less than m */
156 /* which specifies the leading dimension of the array a. */
157
158 /* pivot is a logical input variable. if pivot is set true, */
159 /* then column pivoting is enforced. if pivot is set false, */
160 /* then no column pivoting is done. */
161
162 /* ipvt is an integer output array of length lipvt. ipvt */
163 /* defines the permutation matrix p such that a*p = q*r. */
164 /* column j of p is column ipvt(j) of the identity matrix. */
165 /* if pivot is false, ipvt is not referenced. */
166
167 /* lipvt is a positive integer input variable. if pivot is false, */
168 /* then lipvt may be as small as 1. if pivot is true, then */
169 /* lipvt must be at least n. */
170
171 /* rdiag is an output array of length n which contains the */
172 /* diagonal elements of r. */
173
174 /* acnorm is an output array of length n which contains the */
175 /* norms of the corresponding columns of the input matrix a. */
176 /* if this information is not needed, then acnorm can coincide */
177 /* with rdiag. */
178
179 /* wa is a work array of length n. if pivot is false, then wa */
180 /* can coincide with rdiag. */
181
182 /* subprograms called */
183
184 /* minpack-supplied ... dpmpar,enorm */
185
186 /* fortran-supplied ... dmax1,dsqrt,min0 */
187
188 /* argonne national laboratory. minpack project. march 1980. */
189 /* burton s. garbow, kenneth e. hillstrom, jorge j. more */
190
191 /* ********** */
192 (void)lipvt;
193
194 /* epsmch is the machine precision. */
195
196 epsmch = __cminpack_func__(dpmpar)(1);
197
198 /* compute the initial column norms and initialize several arrays. */
199
200 for (j = 0; j < n; ++j) {
201 acnorm[j] = __cminpack_enorm__(m, &a[j * lda + 0]);
202 rdiag[j] = acnorm[j];
203 wa[j] = rdiag[j];
204 if (pivot) {
205 ipvt[j] = j+1;
206 }
207 }
208
209 /* reduce a to r with householder transformations. */
210
211 minmn = min(m,n);
212 for (j = 0; j < minmn; ++j) {
213 if (pivot) {
214
215 /* bring the column of largest norm into the pivot position. */
216
217 int kmax = j;
218 for (k = j; k < n; ++k) {
219 if (rdiag[k] > rdiag[kmax]) {
220 kmax = k;
221 }
222 }
223 if (kmax != j) {
224 for (i = 0; i < m; ++i) {
225 temp = a[i + j * lda];
226 a[i + j * lda] = a[i + kmax * lda];
227 a[i + kmax * lda] = temp;
228 }
229 rdiag[kmax] = rdiag[j];
230 wa[kmax] = wa[j];
231 k = ipvt[j];
232 ipvt[j] = ipvt[kmax];
233 ipvt[kmax] = k;
234 }
235 }
236
237 /* compute the householder transformation to reduce the */
238 /* j-th column of a to a multiple of the j-th unit vector. */
239
240 ajnorm = __cminpack_enorm__(m - (j+1) + 1, &a[j + j * lda]);
241 if (ajnorm != 0.) {
242 if (a[j + j * lda] < 0.) {
243 ajnorm = -ajnorm;
244 }
245 for (i = j; i < m; ++i) {
246 a[i + j * lda] /= ajnorm;
247 }
248 a[j + j * lda] += 1.;
249
250 /* apply the transformation to the remaining columns */
251 /* and update the norms. */
252
253 jp1 = j + 1;
254 if (n > jp1) {
255 for (k = jp1; k < n; ++k) {
256 sum = 0.;
257 for (i = j; i < m; ++i) {
258 sum += a[i + j * lda] * a[i + k * lda];
259 }
260 temp = sum / a[j + j * lda];
261 for (i = j; i < m; ++i) {
262 a[i + k * lda] -= temp * a[i + j * lda];
263 }
264 if (pivot && rdiag[k] != 0.) {
265 temp = a[j + k * lda] / rdiag[k];
266 /* Computing MAX */
267 d1 = 1. - temp * temp;
268 rdiag[k] *= sqrt((max((real)0.,d1)));
269 /* Computing 2nd power */
270 d1 = rdiag[k] / wa[k];
271 if (p05 * (d1 * d1) <= epsmch) {
272 rdiag[k] = __cminpack_enorm__(m - (j+1), &a[jp1 + k * lda]);
273 wa[k] = rdiag[k];
274 }
275 }
276 }
277 }
278 }
279 rdiag[j] = -ajnorm;
280 }
281
282 /* last card of subroutine qrfac. */
283 #endif /* !USE_LAPACK */
284 } /* qrfac_ */
285
286