1 /* adapted from DQRDC of LINPACK */
5 #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
7 static double NORM2(i
, j
, n
, x
)
12 double maxx
, sum
, temp
;
14 for (k
= i
, maxx
= 0.0; k
< n
; k
++) {
16 if (maxx
< temp
) maxx
= temp
;
18 if (maxx
== 0.0) return(0.0);
20 for (k
= i
, sum
= 0.0; k
< n
; k
++) {
21 temp
= x
[k
][j
] / maxx
;
24 return(maxx
* sqrt(sum
));
28 static double DOT(i
, j
, k
, n
, x
)
35 for (l
= i
, sum
= 0.0; l
< n
; l
++) sum
+= x
[l
][j
] * x
[l
][k
];
39 static AXPY(i
, j
, k
, n
, a
, x
)
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
)
52 for (k
= i
; k
< n
; k
++) x
[k
][j
] *= a
;
55 static SWAP(i
, j
, n
, a
)
61 for (k
= 0; k
< n
; k
++) {
68 qrdecomp(x
,n
,p
,v
,jpvt
,pivot
)
73 int i
,j
,k
,jp
,l
,lp1
,lup
,maxj
;
74 double maxnrm
,tt
,*qraux
,*work
;
82 * compute the norms of the free columns.
85 for (j
= 0; j
< p
; j
++) {
87 qraux
[j
] = NORM2(0, j
, n
, x
);
91 * perform the householder reduction of x.
93 lup
= (n
< p
) ? n
: p
;
94 for (l
= 0; l
< lup
; l
++) {
97 * locate the column of largest norm and bring it
98 * into the pivot position.
102 for (j
= l
; j
< p
; j
++)
103 if (qraux
[j
]>maxnrm
) {
109 qraux
[maxj
] = qraux
[l
];
110 work
[maxj
] = work
[l
];
112 jpvt
[maxj
] = jpvt
[l
];
119 * compute the householder transformation for column l.
121 nrmxl
= NORM2(l
, l
, n
, x
);
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.
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
);
137 tt
= 1.0-(fabs(x
[l
][j
])/qraux
[j
])*(fabs(x
[l
][j
])/qraux
[j
]);
138 if (tt
< 0.0) tt
= 0.0;
140 tt
= 1.0+0.05*tt
*(qraux
[j
]/work
[j
])*(qraux
[j
]/work
[j
]);
142 qraux
[j
] = qraux
[j
]*sqrt(t
);
144 qraux
[j
] = NORM2(l
+1, j
, n
, x
);
150 * save the transformation.
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
++) {
166 /* accumulate the Q transformation -- assumes p <= n */
167 for (i
= 0; i
< p
; 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;
174 for (k
= i
; k
< n
; k
++) x
[k
][i
] = -x
[k
][i
];
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
);