74 lines
2.9 KiB
C
74 lines
2.9 KiB
C
#include "cs.h"
|
|
/* sparse QR factorization [V,beta,pinv,R] = qr (A) */
|
|
csn *cs_qr (const cs *A, const css *S)
|
|
{
|
|
double *Rx, *Vx, *Ax, *x, *Beta ;
|
|
int i, k, p, m, n, vnz, p1, top, m2, len, col, rnz, *s, *leftmost, *Ap, *Ai,
|
|
*parent, *Rp, *Ri, *Vp, *Vi, *w, *pinv, *q ;
|
|
cs *R, *V ;
|
|
csn *N ;
|
|
if (!CS_CSC (A) || !S) return (NULL) ;
|
|
m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
|
|
q = S->q ; parent = S->parent ; pinv = S->pinv ; m2 = S->m2 ;
|
|
vnz = S->lnz ; rnz = S->unz ; leftmost = S->leftmost ;
|
|
w = cs_malloc (m2+n, sizeof (int)) ; /* get int workspace */
|
|
x = cs_malloc (m2, sizeof (double)) ; /* get double workspace */
|
|
N = cs_calloc (1, sizeof (csn)) ; /* allocate result */
|
|
if (!w || !x || !N) return (cs_ndone (N, NULL, w, x, 0)) ;
|
|
s = w + m2 ; /* s is size n */
|
|
for (k = 0 ; k < m2 ; k++) x [k] = 0 ; /* clear workspace x */
|
|
N->L = V = cs_spalloc (m2, n, vnz, 1, 0) ; /* allocate result V */
|
|
N->U = R = cs_spalloc (m2, n, rnz, 1, 0) ; /* allocate result R */
|
|
N->B = Beta = cs_malloc (n, sizeof (double)) ; /* allocate result Beta */
|
|
if (!R || !V || !Beta) return (cs_ndone (N, NULL, w, x, 0)) ;
|
|
Rp = R->p ; Ri = R->i ; Rx = R->x ;
|
|
Vp = V->p ; Vi = V->i ; Vx = V->x ;
|
|
for (i = 0 ; i < m2 ; i++) w [i] = -1 ; /* clear w, to mark nodes */
|
|
rnz = 0 ; vnz = 0 ;
|
|
for (k = 0 ; k < n ; k++) /* compute V and R */
|
|
{
|
|
Rp [k] = rnz ; /* R(:,k) starts here */
|
|
Vp [k] = p1 = vnz ; /* V(:,k) starts here */
|
|
w [k] = k ; /* add V(k,k) to pattern of V */
|
|
Vi [vnz++] = k ;
|
|
top = n ;
|
|
col = q ? q [k] : k ;
|
|
for (p = Ap [col] ; p < Ap [col+1] ; p++) /* find R(:,k) pattern */
|
|
{
|
|
i = leftmost [Ai [p]] ; /* i = min(find(A(i,q))) */
|
|
for (len = 0 ; w [i] != k ; i = parent [i]) /* traverse up to k */
|
|
{
|
|
s [len++] = i ;
|
|
w [i] = k ;
|
|
}
|
|
while (len > 0) s [--top] = s [--len] ; /* push path on stack */
|
|
i = pinv [Ai [p]] ; /* i = permuted row of A(:,col) */
|
|
x [i] = Ax [p] ; /* x (i) = A(:,col) */
|
|
if (i > k && w [i] < k) /* pattern of V(:,k) = x (k+1:m) */
|
|
{
|
|
Vi [vnz++] = i ; /* add i to pattern of V(:,k) */
|
|
w [i] = k ;
|
|
}
|
|
}
|
|
for (p = top ; p < n ; p++) /* for each i in pattern of R(:,k) */
|
|
{
|
|
i = s [p] ; /* R(i,k) is nonzero */
|
|
cs_happly (V, i, Beta [i], x) ; /* apply (V(i),Beta(i)) to x */
|
|
Ri [rnz] = i ; /* R(i,k) = x(i) */
|
|
Rx [rnz++] = x [i] ;
|
|
x [i] = 0 ;
|
|
if (parent [i] == k) vnz = cs_scatter (V, i, 0, w, NULL, k, V, vnz);
|
|
}
|
|
for (p = p1 ; p < vnz ; p++) /* gather V(:,k) = x */
|
|
{
|
|
Vx [p] = x [Vi [p]] ;
|
|
x [Vi [p]] = 0 ;
|
|
}
|
|
Ri [rnz] = k ; /* R(k,k) = norm (x) */
|
|
Rx [rnz++] = cs_house (Vx+p1, Beta+k, vnz-p1) ; /* [v,beta]=house(x) */
|
|
}
|
|
Rp [n] = rnz ; /* finalize R */
|
|
Vp [n] = vnz ; /* finalize V */
|
|
return (cs_ndone (N, NULL, w, x, 1)) ; /* success */
|
|
}
|