/* x=A\b where A can be rectangular; b overwritten with solution */
CS_INT cs_qrsol (CS_INT order, const cs *A, CS_ENTRY *b)
{
CS_ENTRY *x ;
css *S ;
csn *N ;
cs *AT = NULL ;
CS_INT k, m, n, ok ;
if (!CS_CSC (A) || !b) return (0) ; /* check inputs */
n = A->n ;
m = A->m ;
if (m >= n)
{
S = cs_sqr (order, A, 1) ; /* ordering and symbolic analysis */
N = cs_qr (A, S) ; /* numeric QR factorization */
x = cs_calloc (S ? S->m2 : 1, sizeof (CS_ENTRY)) ; /* get workspace */
ok = (S && N && x) ;
if (ok)
{
cs_ipvec (S->pinv, b, x, m) ; /* x(0:m-1) = b(p(0:m-1) */
for (k = 0 ; k < n ; k++) /* apply Householder refl. to x */
{
cs_happly (N->L, k, N->B [k], x) ;
}
cs_usolve (N->U, x) ; /* x = R\x */
cs_ipvec (S->q, x, b, n) ; /* b(q(0:n-1)) = x(0:n-1) */
}
}
else
{
AT = cs_transpose (A, 1) ; /* Ax=b is underdetermined */
S = cs_sqr (order, AT, 1) ; /* ordering and symbolic analysis */
N = cs_qr (AT, S) ; /* numeric QR factorization of A' */
x = cs_calloc (S ? S->m2 : 1, sizeof (CS_ENTRY)) ; /* get workspace */
ok = (AT && S && N && x) ;
if (ok)
{
cs_pvec (S->q, b, x, m) ; /* x(q(0:m-1)) = b(0:m-1) */
cs_utsolve (N->U, x) ; /* x = R'\x */
for (k = m-1 ; k >= 0 ; k--) /* apply Householder refl. to x */
{
cs_happly (N->L, k, N->B [k], x) ;
}
cs_pvec (S->pinv, x, b, n) ; /* b(0:n-1) = x(p(0:n-1)) */
}
}
cs_free (x) ;
cs_sfree (S) ;
cs_nfree (N) ;
cs_spfree (AT) ;
return (ok) ;
}
/* x = x + beta * A(:,j), where x is a dense vector and A(:,j) is sparse */
int cs_scatter (const cs *A, int j, double beta, int *w, double *x, int mark,
cs *C, int nz)
{
int i, p, *Ap, *Ai, *Ci ;
double *Ax ;
if (!CS_CSC (A) || !w || !CS_CSC (C)) return (-1) ; /* check inputs */
Ap = A->p ; Ai = A->i ; Ax = A->x ; Ci = C->i ;
// JLBC: Modification for MRPT: optimized case for beta==1:
if (beta==1)
{
for (p = Ap [j] ; p < Ap [j+1] ; p++)
{
i = Ai [p] ; /* A(i,j) is nonzero */
if (w [i] < mark)
{
w [i] = mark ; /* i is new entry in column j */
Ci [nz++] = i ; /* add i to pattern of C(:,j) */
if (x) x [i] = /* beta * */ Ax [p] ; /* x(i) = beta*A(i,j) */
}
else if (x) x [i] += /*beta * */ Ax [p] ; /* i exists in C(:,j) already */
}
}
else
{
for (p = Ap [j] ; p < Ap [j+1] ; p++)
{
i = Ai [p] ; /* A(i,j) is nonzero */
if (w [i] < mark)
{
w [i] = mark ; /* i is new entry in column j */
Ci [nz++] = i ; /* add i to pattern of C(:,j) */
if (x) x [i] = beta * Ax [p] ; /* x(i) = beta*A(i,j) */
}
else if (x) x [i] += beta * Ax [p] ; /* i exists in C(:,j) already */
}
}
return (nz) ;
}
/* find a maximum transveral */
int *cs_maxtrans (const cs *A, int seed) /*[jmatch [0..m-1]; imatch [0..n-1]]*/
{
int i, j, k, n, m, p, n2 = 0, m2 = 0, *Ap, *jimatch, *w, *cheap, *js, *is,
*ps, *Ai, *Cp, *jmatch, *imatch, *q ;
cs *C ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
n = A->n ; m = A->m ; Ap = A->p ; Ai = A->i ;
w = jimatch = cs_calloc (m+n, sizeof (int)) ; /* allocate result */
if (!jimatch) return (NULL) ;
for (k = 0, j = 0 ; j < n ; j++) /* count nonempty rows and columns */
{
n2 += (Ap [j] < Ap [j+1]) ;
for (p = Ap [j] ; p < Ap [j+1] ; p++)
{
w [Ai [p]] = 1 ;
k += (j == Ai [p]) ; /* count entries already on diagonal */
}
}
if (k == CS_MIN (m,n)) /* quick return if diagonal zero-free */
{
jmatch = jimatch ; imatch = jimatch + m ;
for (i = 0 ; i < k ; i++) jmatch [i] = i ;
for ( ; i < m ; i++) jmatch [i] = -1 ;
for (j = 0 ; j < k ; j++) imatch [j] = j ;
for ( ; j < n ; j++) imatch [j] = -1 ;
return (cs_idone (jimatch, NULL, NULL, 1)) ;
}
for (i = 0 ; i < m ; i++) m2 += w [i] ;
C = (m2 < n2) ? cs_transpose (A,0) : ((cs *) A) ; /* transpose if needed */
if (!C) return (cs_idone (jimatch, (m2 < n2) ? C : NULL, NULL, 0)) ;
n = C->n ; m = C->m ; Cp = C->p ;
jmatch = (m2 < n2) ? jimatch + n : jimatch ;
imatch = (m2 < n2) ? jimatch : jimatch + m ;
w = cs_malloc (5*n, sizeof (int)) ; /* get workspace */
if (!w) return (cs_idone (jimatch, (m2 < n2) ? C : NULL, w, 0)) ;
cheap = w + n ; js = w + 2*n ; is = w + 3*n ; ps = w + 4*n ;
for (j = 0 ; j < n ; j++) cheap [j] = Cp [j] ; /* for cheap assignment */
for (j = 0 ; j < n ; j++) w [j] = -1 ; /* all columns unflagged */
for (i = 0 ; i < m ; i++) jmatch [i] = -1 ; /* nothing matched yet */
q = cs_randperm (n, seed) ; /* q = random permutation */
for (k = 0 ; k < n ; k++) /* augment, starting at column q[k] */
{
cs_augment (q ? q [k]: k, C, jmatch, cheap, w, js, is, ps) ;
}
cs_free (q) ;
for (j = 0 ; j < n ; j++) imatch [j] = -1 ; /* find row match */
for (i = 0 ; i < m ; i++) if (jmatch [i] >= 0) imatch [jmatch [i]] = i ;
return (cs_idone (jimatch, (m2 < n2) ? C : NULL, w, 1)) ;
}
cs *cs_add(const cs *A, const cs *B, double alpha, double beta) {
int p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values;
double *x, *Bx, *Cx;
cs *C;
if (!CS_CSC (A) || !CS_CSC (B))
return (NULL); /* check inputs */
if (A->m != B->m || A->n != B->n)
return (NULL);
m = A->m;
anz = A->p[A->n];
n = B->n;
Bp = B->p;
Bx = B->x;
bnz = Bp[n];
w = (int *) cs_calloc(m, sizeof(int)); /* get workspace */
values = (A->x != NULL) && (Bx != NULL);
x = (double *) (values ? cs_malloc(m, sizeof(double)) : NULL); /* get workspace */
C = cs_spalloc(m, n, anz + bnz, values, 0); /* allocate result*/
if (!C || !w || (values && !x))
return (cs_done(C, w, x, 0));
Cp = C->p;
Ci = C->i;
Cx = C->x;
for (j = 0; j < n; j++) {
Cp[j] = nz; /* column j of C starts here */
nz = cs_scatter(A, j, alpha, w, x, j + 1, C, nz); /* alpha*A(:,j)*/
nz = cs_scatter(B, j, beta, w, x, j + 1, C, nz); /* beta*B(:,j) */
if (values)
for (p = Cp[j]; p < nz; p++)
Cx[p] = x[Ci[p]];
}
Cp[n] = nz; /* finalize the last column of C */
cs_sprealloc(C, 0); /* remove extra space from C */
return (cs_done(C, w, x, 1)); /* success; free workspace, return C */
}
int cs_gaxpy (const cs *A, const double *x, double *y)
{
int p, j, n, *Ap, *Ai ;
double *Ax ;
if (!CS_CSC (A) || !x || !y) return (0) ; /* check inputs */
n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
for (j = 0 ; j < n ; j++)
{
for (p = Ap [j] ; p < Ap [j+1] ; p++)
{
y [Ai [p]] += Ax [p] * x [j] ;
}
}
return (1) ;
}
/* 1-norm of a sparse matrix = max (sum (abs (A))), largest column sum */
double cs_norm (const cs *A)
{
int p, j, n, *Ap ;
double *Ax, norm = 0, s ;
if (!CS_CSC (A) || !A->x) return (-1) ; /* check inputs */
n = A->n ;
Ap = A->p ;
Ax = A->x ;
for (j = 0 ; j < n ; j++)
{
for (s = 0, p = Ap [j] ; p < Ap [j+1] ; p++) s += fabs (Ax [p]) ;
norm = CS_MAX (norm, s) ;
}
return (norm) ;
}
/* solve xG=b(k,:), where G is either upper (up=1) or lower (up=0) triangular */
int csr_spsolve (csr *G, const csr *B, int k, int *xi, double *x, const int *pinv, int up)
{
int i, I, p, q, px, top, n, *Gp, *Gj, *Bp, *Bj ;
int debug = 0;
double *Gx, *Bx ;
if (!CS_CSC (G) || !CS_CSC (B) || !xi || !x) return (-1) ;
Gp = G->p ; Gj = G->j ; Gx = G->x ; n = G->n ;
Bp = B->p ; Bj = B->j ; Bx = B->x ;
top = csr_reach (G, B, k, xi, pinv) ; /* xi[top..n-1]=Reach(B(:,k)) */
for (p = top ; p < n ; p++) x [xi [p]] = 0 ; /* clear x */
for (p = Bp [k] ; p < Bp [k+1] ; p++) x [Bj [p]] = Bx [p] ; /* scatter B */
if( debug )
printf("solve k=%d x= %g %g %g %g \n", k, x[0], x[1], x[2], x[3]);
if( debug )
printf("top=%d xi= %d %d %d %d \n", top , xi[0], xi[1], xi[2], xi[3]);
for (px = top ; px < n ; px++)
{
i = xi [px] ; /* x(i) is nonzero */
I = pinv ? (pinv [i]) : i ; /* i maps to col I of G */
if (I < 0) continue ; /* row I is empty */
/* dmd */
x [i] /= Gx [up ? (Gp [I]) : (Gp [I+1]-1)] ;/* x(i) /= G(i,i) */
p = up ? (Gp [I]+1) : (Gp [I]) ; /* up: L(i,i) 1st entry */
q = up ? (Gp [I+1]) : (Gp [I+1]-1) ; /* up: U(i,i) last entry */
for ( ; p < q ; p++)
{
if( debug )
printf("%d %d solve %d %g %g \n", px, i ,p, Gx [p] , x [Gj [p]] );
x [Gj[p]] -= Gx [p] * x [i] ; /* x(i) -= G(i,j) * x(j) */
}
if( debug )
printf(" x= %g %g %g %g \n", x[0], x[1], x[2], x[3]);
}
return (top) ; /* return top of stack */
}
/* solve Ux=b where x and b are dense. x=b on input, solution on output. */
CS_INT cs_usolve (const cs *U, CS_ENTRY *x)
{
CS_INT p, j, n, *Up, *Ui ;
CS_ENTRY *Ux ;
if (!CS_CSC (U) || !x) return (0) ; /* check inputs */
n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ;
for (j = n-1 ; j >= 0 ; j--)
{
x [j] /= Ux [Up [j+1]-1] ;
for (p = Up [j] ; p < Up [j+1]-1 ; p++)
{
x [Ui [p]] -= Ux [p] * x [j] ;
}
}
return (1) ;
}
/* solve L'x=b where x and b are dense. x=b on input, solution on output. */
CS_INT cs_ltsolve (const cs *L, CS_ENTRY *x)
{
CS_INT p, j, n, *Lp, *Li ;
CS_ENTRY *Lx ;
if (!CS_CSC (L) || !x) return (0) ; /* check inputs */
n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ;
for (j = n-1 ; j >= 0 ; j--)
{
for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
{
x [j] -= CS_CONJ (Lx [p]) * x [Li [p]] ;
}
x [j] /= CS_CONJ (Lx [Lp [j]]) ;
}
return (1) ;
}
/* solve L'x=b where x and b are dense. x=b on input, solution on output. */
int cs_ltsolve (const cs *L, double *x)
{
int p, j, n, *Lp, *Li ;
double *Lx ;
if (!CS_CSC (L) || !x) return (0) ; /* check inputs */
n = L->n ; Lp = L->p ; Li = L->i ; Lx = L->x ;
for (j = n-1 ; j >= 0 ; j--)
{
for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
{
x [j] -= Lx [p] * x [Li [p]] ;
}
x [j] /= Lx [Lp [j]] ;
}
return (1) ;
}
/* solve U'x=b where x and b are dense. x=b on input, solution on output. */
int cs_utsolve (const cs *U, double *x)
{
int p, j, n, *Up, *Ui ;
double *Ux ;
if (!CS_CSC (U) || !x) return (0) ; /* check inputs */
n = U->n ; Up = U->p ; Ui = U->i ; Ux = U->x ;
for (j = 0 ; j < n ; j++)
{
for (p = Up [j] ; p < Up [j+1]-1 ; p++)
{
x [j] -= Ux [p] * x [Ui [p]] ;
}
x [j] /= Ux [Up [j+1]-1] ;
}
return (1) ;
}
int cs_sprealloc(cs *A, int nzmax) {
int ok, oki, okj = 1, okx = 1;
if (!A)
return (0);
if (nzmax <= 0)
nzmax = (CS_CSC (A)) ? (A->p[A->n]) : A->nz;
A->i = (int *) cs_realloc(A->i, nzmax, sizeof(int), &oki);
if (CS_TRIPLET (A))
A->p = (int *) cs_realloc(A->p, nzmax, sizeof(int), &okj);
if (A->x)
A->x = (double *) cs_realloc(A->x, nzmax, sizeof(double), &okx);
ok = (oki && okj && okx);
if (ok)
A->nzmax = nzmax;
return (ok);
}
/* apply the ith Householder vector to x */
CS_INT cs_happly (const cs *V, CS_INT i, double beta, CS_ENTRY *x)
{
CS_INT p, *Vp, *Vi ;
CS_ENTRY *Vx, tau = 0 ;
if (!CS_CSC (V) || !x) return (0) ; /* check inputs */
Vp = V->p ; Vi = V->i ; Vx = V->x ;
for (p = Vp [i] ; p < Vp [i+1] ; p++) /* tau = v'*x */
{
tau += CS_CONJ (Vx [p]) * x [Vi [p]] ;
}
tau *= beta ; /* tau = beta*(v'*x) */
for (p = Vp [i] ; p < Vp [i+1] ; p++) /* x = x - v*tau */
{
x [Vi [p]] -= Vx [p] * tau ;
}
return (1) ;
}
/* apply the ith Householder vector to x */
int cs_happly (const cs *V, int i, double beta, double *x)
{
int p, *Vp, *Vi ;
double *Vx, tau = 0 ;
if (!CS_CSC (V) || !x) return (0) ; /* check inputs */
Vp = V->p ; Vi = V->i ; Vx = V->x ;
for (p = Vp [i] ; p < Vp [i+1] ; p++) /* tau = v'*x */
{
tau += Vx [p] * x [Vi [p]] ;
}
tau *= beta ; /* tau = beta*(v'*x) */
for (p = Vp [i] ; p < Vp [i+1] ; p++) /* x = x - v*tau */
{
x [Vi [p]] -= Vx [p] * tau ;
}
return (1) ;
}
int *cs_counts (const cs *A, const int *parent, const int *post, int ata)
{
int i, j, k, n, m, J, s, p, q, jleaf, *ATp, *ATi, *maxfirst, *prevleaf,
*ancestor, *head = NULL, *next = NULL, *colcount, *w, *first, *delta ;
cs *AT ;
if (!CS_CSC (A) || !parent || !post) return (NULL) ; /* check inputs */
m = A->m ; n = A->n ;
s = 4*n + (ata ? (n+m+1) : 0) ;
delta = colcount = cs_malloc (n, sizeof (int)) ; /* allocate result */
w = cs_malloc (s, sizeof (int)) ; /* get workspace */
AT = cs_transpose (A, 0) ; /* AT = A' */
if (!AT || !colcount || !w) return (cs_idone (colcount, AT, w, 0)) ;
ancestor = w ; maxfirst = w+n ; prevleaf = w+2*n ; first = w+3*n ;
for (k = 0 ; k < s ; k++) w [k] = -1 ; /* clear workspace w [0..s-1] */
for (k = 0 ; k < n ; k++) /* find first [j] */
{
j = post [k] ;
delta [j] = (first [j] == -1) ? 1 : 0 ; /* delta[j]=1 if j is a leaf */
for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ;
}
ATp = AT->p ; ATi = AT->i ;
if (ata) init_ata (AT, post, w, &head, &next) ;
for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */
for (k = 0 ; k < n ; k++)
{
j = post [k] ; /* j is the kth node in postordered etree */
if (parent [j] != -1) delta [parent [j]]-- ; /* j is not a root */
for (J = HEAD (k,j) ; J != -1 ; J = NEXT (J)) /* J=j for LL'=A case */
{
for (p = ATp [J] ; p < ATp [J+1] ; p++)
{
i = ATi [p] ;
q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf);
if (jleaf >= 1) delta [j]++ ; /* A(i,j) is in skeleton */
if (jleaf == 2) delta [q]-- ; /* account for overlap in q */
}
}
if (parent [j] != -1) ancestor [j] = parent [j] ;
}
for (j = 0 ; j < n ; j++) /* sum up delta's of each child */
{
if (parent [j] != -1) colcount [parent [j]] += colcount [j] ;
}
return (cs_idone (colcount, AT, w, 1)) ; /* success: free workspace */
}
cs *cs_symperm(const cs *A, const int *pinv, int values) {
int i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w;
double *Cx, *Ax;
cs *C;
if (!CS_CSC (A))
return (NULL); /* check inputs */
n = A->n;
Ap = A->p;
Ai = A->i;
Ax = A->x;
C = cs_spalloc(n, n, Ap[n], values && (Ax != NULL), 0); /* alloc result*/
w = (int *) cs_calloc(n, sizeof(int)); /* get workspace */
if (!C || !w)
return (cs_done(C, w, NULL, 0)); /* out of memory */
Cp = C->p;
Ci = C->i;
Cx = C->x;
for (j = 0; j < n; j++) /* count entries in each column of C */
{
j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */
for (p = Ap[j]; p < Ap[j + 1]; p++) {
i = Ai[p];
if (i > j)
continue; /* skip lower triangular part of A */
i2 = pinv ? pinv[i] : i; /* row i of A is row i2 of C */
w[CS_MAX (i2, j2)]++; /* column count of C */
}
}
cs_cumsum(Cp, w, n); /* compute column pointers of C */
for (j = 0; j < n; j++) {
j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */
for (p = Ap[j]; p < Ap[j + 1]; p++) {
i = Ai[p];
if (i > j)
continue; /* skip lower triangular part of A*/
i2 = pinv ? pinv[i] : i; /* row i of A is row i2 of C */
Ci[q = w[CS_MAX (i2, j2)]++] = CS_MIN (i2, j2);
if (Cx)
Cx[q] = Ax[p];
}
}
return (cs_done(C, w, NULL, 1)); /* success; free workspace, return C */
}
/* symbolic ordering and analysis for LU */
css *csr_sqr (int order, const csr *A )
{
int n, ok = 1;
css *S ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
n = A->n ;
S = (css*)calloc(1, sizeof (css)) ; /* allocate result S */
if (!S) return (NULL) ; /* out of memory */
S->q = csr_amd (order, A) ; /* fill-reducing ordering */
if (!S->q)
{
printf(" csr_sqr error no permutation\n");
}
if (order && !S->q) return (csr_sfree (S)) ;
/* LU factorization */
S->unz = (double) CS_MIN(4*(A->p [n]) + n, n * n );
S->lnz = S->unz ; /* guess nnz(L) and nnz(U) */
return (ok ? S : csr_sfree (S)) ; /* return result S */
}
/* find nonzero pattern of Cholesky L(k,1:k-1) using etree and triu(A(:,k)) */
CS_INT cs_ereach (const cs *A, CS_INT k, const CS_INT *parent, CS_INT *s, CS_INT *w)
{
CS_INT i, p, n, len, top, *Ap, *Ai ;
if (!CS_CSC (A) || !parent || !s || !w) return (-1) ; /* check inputs */
top = n = A->n ; Ap = A->p ; Ai = A->i ;
CS_MARK (w, k) ; /* mark node k as visited */
for (p = Ap [k] ; p < Ap [k+1] ; p++)
{
i = Ai [p] ; /* A(i,k) is nonzero */
if (i > k) continue ; /* only use upper triangular part of A */
for (len = 0 ; !CS_MARKED (w,i) ; i = parent [i]) /* traverse up etree*/
{
s [len++] = i ; /* L(k,i) is nonzero */
CS_MARK (w, i) ; /* mark i as visited */
}
while (len > 0) s [--top] = s [--len] ; /* push path onto stack */
}
for (p = top ; p < n ; p++) CS_MARK (w, s [p]) ; /* unmark all nodes */
CS_MARK (w, k) ; /* unmark node k */
return (top) ; /* s [top..n-1] contains pattern of L(k,:)*/
}
/* find the strongly connected components of a square matrix */
csd *cs_scc (cs *A) /* matrix A temporarily modified, then restored */
{
int n, i, k, b, nb = 0, top, *xi, *pstack, *p, *r, *Ap, *ATp, *rcopy, *Blk ;
cs *AT ;
csd *D ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
n = A->n ; Ap = A->p ;
D = cs_dalloc (n, 0) ; /* allocate result */
AT = cs_transpose (A, 0) ; /* AT = A' */
xi = cs_malloc (2*n+1, sizeof (int)) ; /* get workspace */
if (!D || !AT || !xi) return (cs_ddone (D, AT, xi, 0)) ;
Blk = xi ; rcopy = pstack = xi + n ;
p = D->p ; r = D->r ; ATp = AT->p ;
top = n ;
for (i = 0 ; i < n ; i++) /* first dfs(A) to find finish times (xi) */
{
if (!CS_MARKED (Ap, i)) top = cs_dfs (i, A, top, xi, pstack, NULL) ;
}
for (i = 0 ; i < n ; i++) CS_MARK (Ap, i) ; /* restore A; unmark all nodes*/
top = n ;
nb = n ;
for (k = 0 ; k < n ; k++) /* dfs(A') to find strongly connnected comp */
{
i = xi [k] ; /* get i in reverse order of finish times */
if (CS_MARKED (ATp, i)) continue ; /* skip node i if already ordered */
r [nb--] = top ; /* node i is the start of a component in p */
top = cs_dfs (i, AT, top, p, pstack, NULL) ;
}
r [nb] = 0 ; /* first block starts at zero; shift r up */
for (k = nb ; k <= n ; k++) r [k-nb] = r [k] ;
D->nb = nb = n-nb ; /* nb = # of strongly connected components */
for (b = 0 ; b < nb ; b++) /* sort each block in natural order */
{
for (k = r [b] ; k < r [b+1] ; k++) Blk [p [k]] = b ;
}
for (b = 0 ; b <= nb ; b++) rcopy [b] = r [b] ;
for (i = 0 ; i < n ; i++) p [rcopy [Blk [i]]++] = i ;
return (cs_ddone (D, AT, xi, 1)) ;
}
/* drop entries for which fkeep(A(i,j)) is false; return nz if OK, else -1 */
csi cs_fkeep (cs *A, csi (*fkeep) (csi, csi, double, void *), void *other)
{
csi j, p, nz = 0, n, *Ap, *Ai ;
double *Ax ;
if (!CS_CSC (A) || !fkeep) return (-1) ; /* check inputs */
n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
for (j = 0 ; j < n ; j++)
{
p = Ap [j] ; /* get current location of col j */
Ap [j] = nz ; /* record new location of col j */
for ( ; p < Ap [j+1] ; p++)
{
if (fkeep (Ai [p], j, Ax ? Ax [p] : 1, other))
{
if (Ax) Ax [nz] = Ax [p] ; /* keep A(i,j) */
Ai [nz++] = Ai [p] ;
}
}
}
Ap [n] = nz ; /* finalize A */
cs_sprealloc (A, 0) ; /* remove extra space from A */
return (nz) ;
}
cs *cs_permute (const cs *A, const int *pinv, const int *q, int values)
{
int t, j, k, nz = 0, m, n, *Ap, *Ai, *Cp, *Ci ;
double *Cx, *Ax ;
cs *C ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
C = cs_spalloc (m, n, Ap [n], values && Ax != NULL, 0) ; /* alloc result */
if (!C) return (cs_done (C, NULL, NULL, 0)) ; /* out of memory */
Cp = C->p ; Ci = C->i ; Cx = C->x ;
for (k = 0 ; k < n ; k++)
{
Cp [k] = nz ; /* column k of C is column q[k] of A */
j = q ? (q [k]) : k ;
for (t = Ap [j] ; t < Ap [j+1] ; t++)
{
if (Cx) Cx [nz] = Ax [t] ; /* row i of A is row pinv[i] of C */
Ci [nz++] = pinv ? (pinv [Ai [t]]) : Ai [t] ;
}
}
Cp [n] = nz ; /* finalize the last column of C */
return (cs_done (C, NULL, NULL, 1)) ;
}
/* drop entries for which fkeep(A(i,j)) is false; return nz if OK, else -1 */
int csr_fkeep (csr *A, int (*fkeep) (int, int, double, void *), void *other)
{
int j, p, nz = 0, m, *Ap, *Aj ;
double *Ax ;
if (!CS_CSC (A) || !fkeep) return (-1) ; /* check inputs */
m = A->m ; Ap = A->p ; Aj = A->j ; Ax = A->x ;
for (j = 0 ; j < m ; j++)
{
p = Ap [j] ; /* get current location of col j */
Ap [j] = nz ; /* record new location of col j */
for ( ; p < Ap [j+1] ; p++)
{
if (fkeep (Aj [p], j, Ax ? Ax [p] : 1, other))
{
if (Ax) Ax [nz] = Ax [p] ; /* keep A(i,j) */
Aj [nz++] = Aj [p] ;
}
}
}
Ap [m] = nz ; /* finalize A */
csr_sprealloc (A, 0) ; /* remove extra space from A */
return (nz) ;
}
/* C = A' */
cs *cs_transpose (const cs *A, int values)
{
int p, q, j, *Cp, *Ci, n, m, *Ap, *Ai, *w ;
double *Cx, *Ax ;
cs *C ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
m = A->m ; n = A->n ; Ap = A->p ; Ai = A->i ; Ax = A->x ;
C = cs_spalloc (n, m, Ap [n], values && Ax, 0) ; /* allocate result */
w = cs_calloc (m, sizeof (int)) ; /* get workspace */
if (!C || !w) return (cs_done (C, w, NULL, 0)) ; /* out of memory */
Cp = C->p ; Ci = C->i ; Cx = C->x ;
for (p = 0 ; p < Ap [n] ; p++) w [Ai [p]]++ ; /* row counts */
cs_cumsum (Cp, w, m) ; /* row pointers */
for (j = 0 ; j < n ; j++)
{
for (p = Ap [j] ; p < Ap [j+1] ; p++)
{
Ci [q = w [Ai [p]]++] = j ; /* place A(i,j) as entry C(j,i) */
if (Cx) Cx [q] = Ax [p] ;
}
}
return (cs_done (C, w, NULL, 1)) ; /* success; free w and return C */
}
/* C = A' */
csr *csr_transpose (const csr *A, int values)
{
int p, q, i, *Cp, *Cj, n, m, *Ap, *Aj, *w ;
double *Cx, *Ax ;
csr *C ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
m = A->m ; n = A->n ; Ap = A->p ; Aj = A->j ; Ax = A->x ;
C = csr_spalloc (n, m, Ap [m], values && Ax, 0) ; /* allocate result */
w = (int*)calloc (CS_MAX(n,1), sizeof (int)) ; /* get workspace */
if (!C || !w) return (csr_done (C, w, NULL, 0)) ; /* out of memory */
Cp = C->p ; Cj = C->j ; Cx = C->x ;
for (p = 0 ; p < Ap [m] ; p++) w [Aj [p]]++ ; /* col counts */
csr_cumsum (Cp, w, n) ; /* col pointers */
for (i = 0 ; i < m ; i++)
{
for (p = Ap [i] ; p < Ap [i+1] ; p++)
{
Cj [q = w [Aj [p]]++] = i ; /* place A(i,j) as entry C(j,i) */
if (Cx) Cx [q] = Ax [p] ;
}
}
return (csr_done (C, w, NULL, 1)) ; /* success; free w and return C */
}
/* ordering and symbolic analysis for a Cholesky factorization */
css *cs_schol (int order, const cs *A)
{
int n, *c, *post, *P ;
cs *C ;
css *S ;
if (!CS_CSC (A)) return (NULL) ; /* check inputs */
n = A->n ;
S = cs_calloc (1, sizeof (css)) ; /* allocate result S */
if (!S) return (NULL) ; /* out of memory */
P = cs_amd (order, A) ; /* P = amd(A+A'), or natural */
S->pinv = cs_pinv (P, n) ; /* find inverse permutation */
cs_free (P) ;
if (order && !S->pinv) return (cs_sfree (S)) ;
C = cs_symperm (A, S->pinv, 0) ; /* C = spones(triu(A(P,P))) */
S->parent = cs_etree (C, 0) ; /* find etree of C */
post = cs_post (S->parent, n) ; /* postorder the etree */
c = cs_counts (C, S->parent, post, 0) ; /* find column counts of chol(C) */
cs_free (post) ;
cs_spfree (C) ;
S->cp = cs_malloc (n+1, sizeof (int)) ; /* allocate result S->cp */
S->unz = S->lnz = cs_cumsum (S->cp, c, n) ; /* find column pointers for L */
cs_free (c) ;
return ((S->lnz >= 0) ? S : cs_sfree (S)) ;
}
/* x=A\b where A is unsymmetric; b overwritten with solution */
int cs_lusol (int order, const cs *A, double *b, double tol)
{
double *x ;
css *S ;
csn *N ;
int n, ok ;
if (!CS_CSC (A) || !b) return (0) ; /* check inputs */
n = A->n ;
S = cs_sqr (order, A, 0) ; /* ordering and symbolic analysis */
N = cs_lu (A, S, tol) ; /* numeric LU factorization */
x = cs_malloc (n, sizeof (double)) ; /* get workspace */
ok = (S && N && x) ;
if (ok)
{
cs_ipvec (N->pinv, b, x, n) ; /* x = b(p) */
cs_lsolve (N->L, x) ; /* x = L\x */
cs_usolve (N->U, x) ; /* x = U\x */
cs_ipvec (S->q, x, b, n) ; /* b(q) = x */
}
cs_free (x) ;
cs_sfree (S) ;
cs_nfree (N) ;
return (ok) ;
}
/* x=A\b where A is symmetric positive definite; b overwritten with solution */
CS_INT cs_cholsol (CS_INT order, const cs *A, CS_ENTRY *b)
{
CS_ENTRY *x ;
css *S ;
csn *N ;
CS_INT n, ok ;
if (!CS_CSC (A) || !b) return (0) ; /* check inputs */
n = A->n ;
S = cs_schol (order, A) ; /* ordering and symbolic analysis */
N = cs_chol (A, S) ; /* numeric Cholesky factorization */
x = cs_malloc (n, sizeof (CS_ENTRY)) ; /* get workspace */
ok = (S && N && x) ;
if (ok)
{
cs_ipvec (S->pinv, b, x, n) ; /* x = P*b */
cs_lsolve (N->L, x) ; /* x = L\x */
cs_ltsolve (N->L, x) ; /* x = L'\x */
cs_pvec (S->pinv, x, b, n) ; /* b = P'*x */
}
cs_free (x) ;
cs_sfree (S) ;
cs_nfree (N) ;
return (ok) ;
}
int *cs_etree(const cs *A, int ata) {
int i, k, p, m, n, inext, *Ap, *Ai, *w, *parent, *ancestor, *prev;
if (!CS_CSC (A))
return (NULL); /* check inputs */
m = A->m;
n = A->n;
Ap = A->p;
Ai = A->i;
parent = (int *) cs_malloc(n, sizeof(int)); /* allocate result */
w = (int *) cs_malloc(n + (ata ? m : 0), sizeof(int)); /* get workspace */
if (!w || !parent)
return (cs_idone(parent, NULL, w, 0));
ancestor = w;
prev = w + n;
if (ata)
for (i = 0; i < m; i++)
prev[i] = -1;
for (k = 0; k < n; k++) {
parent[k] = -1; /* node k has no parent yet */
ancestor[k] = -1; /* nor does k have an ancestor */
for (p = Ap[k]; p < Ap[k + 1]; p++) {
i = ata ? (prev[Ai[p]]) : (Ai[p]);
for (; i != -1 && i < k; i = inext) /* traverse from i to k */
{
inext = ancestor[i]; /* inext = ancestor of i */
ancestor[i] = k; /* path compression */
if (inext == -1)
parent[i] = k; /* no anc., parent is k */
}
if (ata)
prev[Ai[p]] = k;
}
}
return (cs_idone(parent, NULL, w, 1));
}
/* [L,U,pinv]=lu(A, [q lnz unz]). lnz and unz can be guess */
csn *cs_lu (const cs *A, const css *S, double tol)
{
cs *L, *U ;
csn *N ;
double pivot, *Lx, *Ux, *x, a, t ;
int *Lp, *Li, *Up, *Ui, *pinv, *xi, *q, n, ipiv, k, top, p, i, col, lnz,unz;
if (!CS_CSC (A) || !S) return (NULL) ; /* check inputs */
n = A->n ;
q = S->q ; lnz = S->lnz ; unz = S->unz ;
x = cs_malloc (n, sizeof (double)) ; /* get double workspace */
xi = cs_malloc (2*n, sizeof (int)) ; /* get int workspace */
N = cs_calloc (1, sizeof (csn)) ; /* allocate result */
if (!x || !xi || !N) return (cs_ndone (N, NULL, xi, x, 0)) ;
N->L = L = cs_spalloc (n, n, lnz, 1, 0) ; /* allocate result L */
N->U = U = cs_spalloc (n, n, unz, 1, 0) ; /* allocate result U */
N->pinv = pinv = cs_malloc (n, sizeof (int)) ; /* allocate result pinv */
if (!L || !U || !pinv) return (cs_ndone (N, NULL, xi, x, 0)) ;
Lp = L->p ; Up = U->p ;
for (i = 0 ; i < n ; i++) x [i] = 0 ; /* clear workspace */
for (i = 0 ; i < n ; i++) pinv [i] = -1 ; /* no rows pivotal yet */
for (k = 0 ; k <= n ; k++) Lp [k] = 0 ; /* no cols of L yet */
lnz = unz = 0 ;
for (k = 0 ; k < n ; k++) /* compute L(:,k) and U(:,k) */
{
/* --- Triangular solve --------------------------------------------- */
Lp [k] = lnz ; /* L(:,k) starts here */
Up [k] = unz ; /* U(:,k) starts here */
if ((lnz + n > L->nzmax && !cs_sprealloc (L, 2*L->nzmax + n)) ||
(unz + n > U->nzmax && !cs_sprealloc (U, 2*U->nzmax + n)))
{
return (cs_ndone (N, NULL, xi, x, 0)) ;
}
Li = L->i ; Lx = L->x ; Ui = U->i ; Ux = U->x ;
col = q ? (q [k]) : k ;
top = cs_spsolve (L, A, col, xi, x, pinv, 1) ; /* x = L\A(:,col) */
/* --- Find pivot --------------------------------------------------- */
ipiv = -1 ;
a = -1 ;
for (p = top ; p < n ; p++)
{
i = xi [p] ; /* x(i) is nonzero */
if (pinv [i] < 0) /* row i is not yet pivotal */
{
if ((t = fabs (x [i])) > a)
{
a = t ; /* largest pivot candidate so far */
ipiv = i ;
}
}
else /* x(i) is the entry U(pinv[i],k) */
{
Ui [unz] = pinv [i] ;
Ux [unz++] = x [i] ;
}
}
if (ipiv == -1 || a <= 0)
return (cs_ndone (N, NULL, xi, x, 0)) ;
if (pinv [col] < 0 && fabs (x [col]) >= a*tol) ipiv = col ;
/* --- Divide by pivot ---------------------------------------------- */
pivot = x [ipiv] ; /* the chosen pivot */
Ui [unz] = k ; /* last entry in U(:,k) is U(k,k) */
Ux [unz++] = pivot ;
pinv [ipiv] = k ; /* ipiv is the kth pivot row */
Li [lnz] = ipiv ; /* first entry in L(:,k) is L(k,k) = 1 */
Lx [lnz++] = 1 ;
for (p = top ; p < n ; p++) /* L(k+1:n,k) = x / pivot */
{
i = xi [p] ;
if (pinv [i] < 0) /* x(i) is an entry in L(:,k) */
{
Li [lnz] = i ; /* save unpermuted row in L */
Lx [lnz++] = x [i] / pivot ; /* scale pivot column */
}
x [i] = 0 ; /* x [0..n-1] = 0 for next k */
}
}
/* --- Finalize L and U ------------------------------------------------- */
Lp [n] = lnz ;
Up [n] = unz ;
Li = L->i ; /* fix row indices of L for final pinv */
for (p = 0 ; p < lnz ; p++) Li [p] = pinv [Li [p]] ;
cs_sprealloc (L, 0) ; /* remove extra space from L and U */
cs_sprealloc (U, 0) ;
return (cs_ndone (N, NULL, xi, x, 1)) ; /* success */
}
/* p = amd(A+A') if symmetric is true, or amd(A'A) otherwise */
CS_INT *cs_amd (CS_INT order, const cs *A) /* order 0:natural, 1:Chol, 2:LU, 3:QR */
{
cs *C, *A2, *AT ;
CS_INT *Cp, *Ci, *last, *W, *len, *nv, *next, *P, *head, *elen, *degree, *w,
*hhead, *ATp, *ATi, d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
ok, cnz, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, n, m, t ;
unsigned CS_INT h ;
/* --- Construct matrix C ----------------------------------------------- */
if (!CS_CSC (A) || order <= 0 || order > 3) return (NULL) ; /* check */
AT = cs_transpose (A, 0) ; /* compute A' */
if (!AT) return (NULL) ;
m = A->m ; n = A->n ;
dense = CS_MAX (16, 10 * sqrt ((double) n)) ; /* find dense threshold */
dense = CS_MIN (n-2, dense) ;
if (order == 1 && n == m)
{
C = cs_add (A, AT, 0, 0) ; /* C = A+A' */
}
else if (order == 2)
{
ATp = AT->p ; /* drop dense columns from AT */
ATi = AT->i ;
for (p2 = 0, j = 0 ; j < m ; j++)
{
p = ATp [j] ; /* column j of AT starts here */
ATp [j] = p2 ; /* new column j starts here */
if (ATp [j+1] - p > dense) continue ; /* skip dense col j */
for ( ; p < ATp [j+1] ; p++) ATi [p2++] = ATi [p] ;
}
ATp [m] = p2 ; /* finalize AT */
A2 = cs_transpose (AT, 0) ; /* A2 = AT' */
C = A2 ? cs_multiply (AT, A2) : NULL ; /* C=A'*A with no dense rows */
cs_spfree (A2) ;
}
else
{
C = cs_multiply (AT, A) ; /* C=A'*A */
}
cs_spfree (AT) ;
if (!C) return (NULL) ;
cs_fkeep (C, &cs_diag, NULL) ; /* drop diagonal entries */
Cp = C->p ;
cnz = Cp [n] ;
P = cs_malloc (n+1, sizeof (CS_INT)) ; /* allocate result */
W = cs_malloc (8*(n+1), sizeof (CS_INT)) ; /* get workspace */
t = cnz + cnz/5 + 2*n ; /* add elbow room to C */
if (!P || !W || !cs_sprealloc (C, t)) return (cs_idone (P, C, W, 0)) ;
len = W ; nv = W + (n+1) ; next = W + 2*(n+1) ;
head = W + 3*(n+1) ; elen = W + 4*(n+1) ; degree = W + 5*(n+1) ;
w = W + 6*(n+1) ; hhead = W + 7*(n+1) ;
last = P ; /* use P as workspace for last */
/* --- Initialize quotient graph ---------------------------------------- */
for (k = 0 ; k < n ; k++) len [k] = Cp [k+1] - Cp [k] ;
len [n] = 0 ;
nzmax = C->nzmax ;
Ci = C->i ;
for (i = 0 ; i <= n ; i++)
{
head [i] = -1 ; /* degree list i is empty */
last [i] = -1 ;
next [i] = -1 ;
hhead [i] = -1 ; /* hash list i is empty */
nv [i] = 1 ; /* node i is just one node */
w [i] = 1 ; /* node i is alive */
elen [i] = 0 ; /* Ek of node i is empty */
degree [i] = len [i] ; /* degree of node i */
}
mark = cs_wclear (0, 0, w, n) ; /* clear w */
elen [n] = -2 ; /* n is a dead element */
Cp [n] = -1 ; /* n is a root of assembly tree */
w [n] = 0 ; /* n is a dead element */
/* --- Initialize degree lists ------------------------------------------ */
for (i = 0 ; i < n ; i++)
{
d = degree [i] ;
if (d == 0) /* node i is empty */
{
elen [i] = -2 ; /* element i is dead */
nel++ ;
Cp [i] = -1 ; /* i is a root of assembly tree */
w [i] = 0 ;
}
else if (d > dense) /* node i is dense */
{
nv [i] = 0 ; /* absorb i into element n */
elen [i] = -1 ; /* node i is dead */
nel++ ;
Cp [i] = CS_FLIP (n) ;
nv [n]++ ;
}
else
{
if (head [d] != -1) last [head [d]] = i ;
next [i] = head [d] ; /* put node i in degree list d */
head [d] = i ;
}
}
while (nel < n) /* while (selecting pivots) do */
{
/* --- Select node of minimum approximate degree -------------------- */
for (k = -1 ; mindeg < n && (k = head [mindeg]) == -1 ; mindeg++) ;
if (next [k] != -1) last [next [k]] = -1 ;
head [mindeg] = next [k] ; /* remove k from degree list */
elenk = elen [k] ; /* elenk = |Ek| */
nvk = nv [k] ; /* # of nodes k represents */
nel += nvk ; /* nv[k] nodes of A eliminated */
/* --- Garbage collection ------------------------------------------- */
if (elenk > 0 && cnz + mindeg >= nzmax)
{
for (j = 0 ; j < n ; j++)
{
if ((p = Cp [j]) >= 0) /* j is a live node or element */
{
Cp [j] = Ci [p] ; /* save first entry of object */
Ci [p] = CS_FLIP (j) ; /* first entry is now CS_FLIP(j) */
}
}
for (q = 0, p = 0 ; p < cnz ; ) /* scan all of memory */
{
if ((j = CS_FLIP (Ci [p++])) >= 0) /* found object j */
{
Ci [q] = Cp [j] ; /* restore first entry of object */
Cp [j] = q++ ; /* new pointer to object j */
for (k3 = 0 ; k3 < len [j]-1 ; k3++) Ci [q++] = Ci [p++] ;
}
}
cnz = q ; /* Ci [cnz...nzmax-1] now free */
}
/* --- Construct new element ---------------------------------------- */
dk = 0 ;
nv [k] = -nvk ; /* flag k as in Lk */
p = Cp [k] ;
pk1 = (elenk == 0) ? p : cnz ; /* do in place if elen[k] == 0 */
pk2 = pk1 ;
for (k1 = 1 ; k1 <= elenk + 1 ; k1++)
{
if (k1 > elenk)
{
e = k ; /* search the nodes in k */
pj = p ; /* list of nodes starts at Ci[pj]*/
ln = len [k] - elenk ; /* length of list of nodes in k */
}
else
{
e = Ci [p++] ; /* search the nodes in e */
pj = Cp [e] ;
ln = len [e] ; /* length of list of nodes in e */
}
for (k2 = 1 ; k2 <= ln ; k2++)
{
i = Ci [pj++] ;
if ((nvi = nv [i]) <= 0) continue ; /* node i dead, or seen */
dk += nvi ; /* degree[Lk] += size of node i */
nv [i] = -nvi ; /* negate nv[i] to denote i in Lk*/
Ci [pk2++] = i ; /* place i in Lk */
if (next [i] != -1) last [next [i]] = last [i] ;
if (last [i] != -1) /* remove i from degree list */
{
next [last [i]] = next [i] ;
}
else
{
head [degree [i]] = next [i] ;
}
}
if (e != k)
{
Cp [e] = CS_FLIP (k) ; /* absorb e into k */
w [e] = 0 ; /* e is now a dead element */
}
}
if (elenk != 0) cnz = pk2 ; /* Ci [cnz...nzmax] is free */
degree [k] = dk ; /* external degree of k - |Lk\i| */
Cp [k] = pk1 ; /* element k is in Ci[pk1..pk2-1] */
len [k] = pk2 - pk1 ;
elen [k] = -2 ; /* k is now an element */
/* --- Find set differences ----------------------------------------- */
mark = cs_wclear (mark, lemax, w, n) ; /* clear w if necessary */
for (pk = pk1 ; pk < pk2 ; pk++) /* scan 1: find |Le\Lk| */
{
i = Ci [pk] ;
if ((eln = elen [i]) <= 0) continue ;/* skip if elen[i] empty */
nvi = -nv [i] ; /* nv [i] was negated */
wnvi = mark - nvi ;
for (p = Cp [i] ; p <= Cp [i] + eln - 1 ; p++) /* scan Ei */
{
e = Ci [p] ;
if (w [e] >= mark)
{
w [e] -= nvi ; /* decrement |Le\Lk| */
}
else if (w [e] != 0) /* ensure e is a live element */
{
w [e] = degree [e] + wnvi ; /* 1st time e seen in scan 1 */
}
}
}
/* --- Degree update ------------------------------------------------ */
for (pk = pk1 ; pk < pk2 ; pk++) /* scan2: degree update */
{
i = Ci [pk] ; /* consider node i in Lk */
p1 = Cp [i] ;
p2 = p1 + elen [i] - 1 ;
pn = p1 ;
for (h = 0, d = 0, p = p1 ; p <= p2 ; p++) /* scan Ei */
{
e = Ci [p] ;
if (w [e] != 0) /* e is an unabsorbed element */
{
dext = w [e] - mark ; /* dext = |Le\Lk| */
if (dext > 0)
{
d += dext ; /* sum up the set differences */
Ci [pn++] = e ; /* keep e in Ei */
h += e ; /* compute the hash of node i */
}
else
{
Cp [e] = CS_FLIP (k) ; /* aggressive absorb. e->k */
w [e] = 0 ; /* e is a dead element */
}
}
}
elen [i] = pn - p1 + 1 ; /* elen[i] = |Ei| */
p3 = pn ;
p4 = p1 + len [i] ;
for (p = p2 + 1 ; p < p4 ; p++) /* prune edges in Ai */
{
j = Ci [p] ;
if ((nvj = nv [j]) <= 0) continue ; /* node j dead or in Lk */
d += nvj ; /* degree(i) += |j| */
Ci [pn++] = j ; /* place j in node list of i */
h += j ; /* compute hash for node i */
}
if (d == 0) /* check for mass elimination */
{
Cp [i] = CS_FLIP (k) ; /* absorb i into k */
nvi = -nv [i] ;
dk -= nvi ; /* |Lk| -= |i| */
nvk += nvi ; /* |k| += nv[i] */
nel += nvi ;
nv [i] = 0 ;
elen [i] = -1 ; /* node i is dead */
}
else
{
degree [i] = CS_MIN (degree [i], d) ; /* update degree(i) */
Ci [pn] = Ci [p3] ; /* move first node to end */
Ci [p3] = Ci [p1] ; /* move 1st el. to end of Ei */
Ci [p1] = k ; /* add k as 1st element in of Ei */
len [i] = pn - p1 + 1 ; /* new len of adj. list of node i */
h %= n ; /* finalize hash of i */
next [i] = hhead [h] ; /* place i in hash bucket */
hhead [h] = i ;
last [i] = h ; /* save hash of i in last[i] */
}
} /* scan2 is done */
degree [k] = dk ; /* finalize |Lk| */
lemax = CS_MAX (lemax, dk) ;
mark = cs_wclear (mark+lemax, lemax, w, n) ; /* clear w */
/* --- Supernode detection ------------------------------------------ */
for (pk = pk1 ; pk < pk2 ; pk++)
{
i = Ci [pk] ;
if (nv [i] >= 0) continue ; /* skip if i is dead */
h = last [i] ; /* scan hash bucket of node i */
i = hhead [h] ;
hhead [h] = -1 ; /* hash bucket will be empty */
for ( ; i != -1 && next [i] != -1 ; i = next [i], mark++)
{
ln = len [i] ;
eln = elen [i] ;
for (p = Cp [i]+1 ; p <= Cp [i] + ln-1 ; p++) w [Ci [p]] = mark;
jlast = i ;
for (j = next [i] ; j != -1 ; ) /* compare i with all j */
{
ok = (len [j] == ln) && (elen [j] == eln) ;
for (p = Cp [j] + 1 ; ok && p <= Cp [j] + ln - 1 ; p++)
{
if (w [Ci [p]] != mark) ok = 0 ; /* compare i and j*/
}
if (ok) /* i and j are identical */
{
Cp [j] = CS_FLIP (i) ; /* absorb j into i */
nv [i] += nv [j] ;
nv [j] = 0 ;
elen [j] = -1 ; /* node j is dead */
j = next [j] ; /* delete j from hash bucket */
next [jlast] = j ;
}
else
{
jlast = j ; /* j and i are different */
j = next [j] ;
}
}
}
}
/* --- Finalize new element------------------------------------------ */
for (p = pk1, pk = pk1 ; pk < pk2 ; pk++) /* finalize Lk */
{
i = Ci [pk] ;
if ((nvi = -nv [i]) <= 0) continue ;/* skip if i is dead */
nv [i] = nvi ; /* restore nv[i] */
d = degree [i] + dk - nvi ; /* compute external degree(i) */
d = CS_MIN (d, n - nel - nvi) ;
if (head [d] != -1) last [head [d]] = i ;
next [i] = head [d] ; /* put i back in degree list */
last [i] = -1 ;
head [d] = i ;
mindeg = CS_MIN (mindeg, d) ; /* find new minimum degree */
degree [i] = d ;
Ci [p++] = i ; /* place i in Lk */
}
nv [k] = nvk ; /* # nodes absorbed into k */
if ((len [k] = p-pk1) == 0) /* length of adj list of element k*/
{
Cp [k] = -1 ; /* k is a root of the tree */
w [k] = 0 ; /* k is now a dead element */
}
if (elenk != 0) cnz = p ; /* free unused space in Lk */
}
/* --- Postordering ----------------------------------------------------- */
for (i = 0 ; i < n ; i++) Cp [i] = CS_FLIP (Cp [i]) ;/* fix assembly tree */
for (j = 0 ; j <= n ; j++) head [j] = -1 ;
for (j = n ; j >= 0 ; j--) /* place unordered nodes in lists */
{
if (nv [j] > 0) continue ; /* skip if j is an element */
next [j] = head [Cp [j]] ; /* place j in list of its parent */
head [Cp [j]] = j ;
}
for (e = n ; e >= 0 ; e--) /* place elements in lists */
{
if (nv [e] <= 0) continue ; /* skip unless e is an element */
if (Cp [e] != -1)
{
next [e] = head [Cp [e]] ; /* place e in list of its parent */
head [Cp [e]] = e ;
}
}
for (k = 0, i = 0 ; i <= n ; i++) /* postorder the assembly tree */
{
if (Cp [i] == -1) k = cs_tdfs (i, k, head, next, P, w) ;
}
return (cs_idone (P, C, W, 1)) ;
}