/* x=A\b where A can be rectangular; b overwritten with solution */
int cs_qrsol (int order, const cs *A, double *b)
{
double *x ;
css *S ;
csn *N ;
cs *AT = NULL ;
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 (double)) ; /* 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 (double)) ; /* 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) ;
}
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 = (css *) 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 = (int *) 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));
}
/* C = alpha*A + beta*B */
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 = cs_calloc (m, sizeof (int)) ; /* get workspace */
values = (A->x != NULL) && (Bx != NULL) ;
x = 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 */
}
/* C = A*B */
cs *cs_multiply (const cs *A, const cs *B)
{
CS_INT p, j, nz = 0, anz, *Cp, *Ci, *Bp, m, n, bnz, *w, values, *Bi ;
CS_ENTRY *x, *Bx, *Cx ;
cs *C ;
if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ; /* check inputs */
if (A->n != B->m) return (NULL) ;
m = A->m ; anz = A->p [A->n] ;
n = B->n ; Bp = B->p ; Bi = B->i ; Bx = B->x ; bnz = Bp [n] ;
w = cs_calloc (m, sizeof (CS_INT)) ; /* get workspace */
values = (A->x != NULL) && (Bx != NULL) ;
x = values ? cs_malloc (m, sizeof (CS_ENTRY)) : 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 ;
for (j = 0 ; j < n ; j++)
{
if (nz + m > C->nzmax && !cs_sprealloc (C, 2*(C->nzmax)+m))
{
return (cs_done (C, w, x, 0)) ; /* out of memory */
}
Ci = C->i ; Cx = C->x ; /* C->i and C->x may be reallocated */
Cp [j] = nz ; /* column j of C starts here */
for (p = Bp [j] ; p < Bp [j+1] ; p++)
{
nz = cs_scatter (A, Bi [p], Bx ? Bx [p] : 1, w, x, j+1, C, nz) ;
}
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 */
}
/* symbolic ordering and analysis for QR or LU */
css *cs_sqr (CS_INT order, const cs *A, CS_INT qr)
{
CS_INT n, k, ok = 1, *post ;
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 */
S->q = cs_amd (order, A) ; /* fill-reducing ordering */
if (order && !S->q) return (cs_sfree (S)) ;
if (qr) /* QR symbolic analysis */
{
cs *C = order ? cs_permute (A, NULL, S->q, 0) : ((cs *) A) ;
S->parent = cs_etree (C, 1) ; /* etree of C'*C, where C=A(:,q) */
post = cs_post (S->parent, n) ;
S->cp = cs_counts (C, S->parent, post, 1) ; /* col counts chol(C'*C) */
cs_free (post) ;
ok = C && S->parent && S->cp && cs_vcount (C, S) ;
if (ok) for (S->unz = 0, k = 0 ; k < n ; k++) S->unz += S->cp [k] ;
if (order) cs_spfree (C) ;
}
else
{
S->unz = 4*(A->p [n]) + n ; /* for LU factorization only, */
S->lnz = S->unz ; /* guess nnz(L) and nnz(U) */
}
return (ok ? S : cs_sfree (S)) ; /* return result S */
}
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 = (int *) 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 */
}
rstatus_t eventmgr_init(eventmanager* eventmgr, int nevents)
{
eventmgr = cs_alloc(sizeof(eventmanager));
if(eventmgr == nil) {
return M_ERR;
}
eventmgr->epfd = epoll_create(1024); /*1024 will be ignored*/
if(eventmgr->epfd == -1) {
cs_free(eventmgr);
return M_ERR;
}
eventmgr->nevent = nevents;
eventmgr->events = cs_calloc(eventmgr->nevent, sizeof(*eventmgr->events));
if(eventmgr->events == nil) {
cs_free(eventmgr);
return M_ERR;
}
eventmgr->evmgr_coro = coro_alloc(&event_loop, eventmgr, DEFAULT_STACK_SIZE);
if(eventmgr->evmgr_coro == nil) {
cs_free(eventmgr);
cs_free(eventmgr->events);
return M_ERR;
}
eventmgr->stop = 0;
return M_OK;
}
cs *cs_compress(const cs *T) {
int m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj;
double *Cx, *Tx;
cs *C;
if (!CS_TRIPLET (T))
return (NULL); /* check inputs */
m = T->m;
n = T->n;
Ti = T->i;
Tj = T->p;
Tx = T->x;
nz = T->nz;
C = cs_spalloc(m, n, nz, Tx != NULL, 0); /* allocate 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 (k = 0; k < nz; k++)
w[Tj[k]]++; /* column counts */
cs_cumsum(Cp, w, n); /* column pointers */
for (k = 0; k < nz; k++) {
Ci[p = w[Tj[k]]++] = Ti[k]; /* A(i,j) is the pth entry in C */
if (Cx)
Cx[p] = Tx[k];
}
return (cs_done(C, w, NULL, 1)); /* success; free w and return C */
}
/* read a problem from a file; use %g for integers to avoid int conflicts */
problem *get_problem (FILE *f, double tol)
{
cs *T, *A, *C ;
int sym, m, n, mn, nz1, nz2 ;
problem *Prob ;
Prob = cs_calloc (1, sizeof (problem)) ;
if (!Prob) return (NULL) ;
T = cs_load (f) ; /* load triplet matrix T from a file */
Prob->A = A = cs_compress (T) ; /* A = compressed-column form of T */
cs_spfree (T) ; /* clear T */
if (!cs_dupl (A)) return (free_problem (Prob)) ; /* sum up duplicates */
Prob->sym = sym = is_sym (A) ; /* determine if A is symmetric */
m = A->m ; n = A->n ;
mn = CS_MAX (m,n) ;
nz1 = A->p [n] ;
cs_dropzeros (A) ; /* drop zero entries */
nz2 = A->p [n] ;
if (tol > 0) cs_droptol (A, tol) ; /* drop tiny entries (just to test) */
Prob->C = C = sym ? make_sym (A) : A ; /* C = A + triu(A,1)', or C=A */
if (!C) return (free_problem (Prob)) ;
printf ("\n--- Matrix: %g-by-%g, nnz: %g (sym: %g: nnz %g), norm: %8.2e\n",
(double) m, (double) n, (double) (A->p [n]), (double) sym,
(double) (sym ? C->p [n] : 0), cs_norm (C)) ;
if (nz1 != nz2) printf ("zero entries dropped: %g\n", (double) (nz1 - nz2));
if (nz2 != A->p [n]) printf ("tiny entries dropped: %g\n",
(double) (nz2 - A->p [n])) ;
Prob->b = cs_malloc (mn, sizeof (double)) ;
Prob->x = cs_malloc (mn, sizeof (double)) ;
Prob->resid = cs_malloc (mn, sizeof (double)) ;
return ((!Prob->b || !Prob->x || !Prob->resid) ? free_problem (Prob) : Prob) ;
}
void mexFunction
(
int nargout,
mxArray *pargout [ ],
int nargin,
const mxArray *pargin [ ]
)
{
cs Lmatrix, Bmatrix, *L, *B ;
double *x ;
int k, i, j, top, *xi, *perm ;
if (nargout > 1 || nargin != 2)
{
mexErrMsgTxt ("Usage: x = cs_reach(L,b)") ;
}
/* get inputs */
L = cs_mex_get_sparse (&Lmatrix, 1, 1, pargin [0]) ;
B = cs_mex_get_sparse (&Bmatrix, 0, 1, pargin [1]) ;
cs_mex_check (0, L->n, 1, 0, 1, 1, pargin [1]) ;
perm = cs_malloc (L->n, sizeof (int)) ;
for (k = 0 ; k < L->n ; k++) perm [k] = k ;
xi = cs_calloc (3*L->n, sizeof (int)) ;
top = cs_reach (L, B, 0, xi, perm) ;
pargout [0] = mxCreateDoubleMatrix (L->n - top, 1, mxREAL) ;
x = mxGetPr (pargout [0]) ;
for (j = 0, i = top ; i < L->n ; i++, j++) x [j] = xi [i] ;
cs_free (xi) ;
cs_free (perm) ;
}
/* Computes: y <- alpha A^T*x + beta y */
int mfiles_dgemv1(double alpha, const mxArray *A, const mxArray *x,
double beta, mxArray *y) {
size_t rA = mxGetM(A);
size_t cA = mxGetN(A);
size_t rx = mxGetM(x);
size_t cx = mxGetN(x);
size_t ry = mxGetM(y);
size_t cy = mxGetN(y);
if (mxIsSparse(x) || mxIsSparse(y)) {
mexErrMsgIdAndTxt("mfiles:BadType",
"Sparse vectors are not supported.");
}
if (mxIsComplex(A) || mxIsComplex(x) || mxIsComplex(y)) {
mexErrMsgIdAndTxt("mfiles:BadType",
"Complex data is not supported.");
}
if ((rA != rx) || (cA != ry) || (cx != 1) || (cy != 1)) {
mexErrMsgIdAndTxt("mfiles:BadDim",
"Dimensions of matrices do not match.");
}
if (mxIsSparse(A)) {
double *px = mxGetPr(x);
double *py = mxGetPr(y);
double *pz = mxCalloc(ry, sizeof (double));
cs *cs_A = cs_calloc(1, sizeof (cs));
mfiles_mx2cs(A, cs_A);
/* Transpose A */
cs *cs_AT = cs_transpose(cs_A, 1);
/* Compute z <- A^T*x */
cs_gaxpy(cs_AT, px, pz);
/* Compute y <- beta y */
cblas_dscal(ry, beta, py, 1);
/* Compute y <- alpha*z+y */
cblas_daxpy(ry, alpha, pz, 1, py, 1);
cs_free(cs_A); /* Check this cs_free and cs_spfree ? */
cs_spfree(cs_AT);
mxFree(pz);
} else {
double *pA = mxGetPr(A);
double *px = mxGetPr(x);
double *py = mxGetPr(y);
cblas_dgemv(CblasRowMajor, CblasTrans,
rA, cA, alpha, pA, rA, px, 1, beta, py, 1);
}
return EXIT_SUCCESS;
}
/* allocate a cs_dmperm or cs_scc result */
csd *cs_dalloc (int m, int n)
{
csd *D ;
D = cs_calloc (1, sizeof (csd)) ;
if (!D) return (NULL) ;
D->p = cs_malloc (m, sizeof (int)) ;
D->r = cs_malloc (m+6, sizeof (int)) ;
D->q = cs_malloc (n, sizeof (int)) ;
D->s = cs_malloc (n+6, sizeof (int)) ;
return ((!D->p || !D->r || !D->q || !D->s) ? cs_dfree (D) : D) ;
}
/* allocate a sparse matrix (triplet form or compressed-column form) */
cs *cs_spalloc (int m, int n, int nzmax, int values, int triplet)
{
cs *A = cs_calloc (1, sizeof (cs)) ; /* allocate the cs struct */
if (!A) return (NULL) ; /* out of memory */
A->m = m ; /* define dimensions and nzmax */
A->n = n ;
A->nzmax = nzmax = CS_MAX (nzmax, 1) ;
A->nz = triplet ? 0 : -1 ; /* allocate triplet or comp.col */
A->p = cs_malloc (triplet ? nzmax : n+1, sizeof (int)) ;
A->i = cs_malloc (nzmax, sizeof (int)) ;
A->x = values ? cs_malloc (nzmax, sizeof (double)) : NULL ;
return ((!A->p || !A->i || (values && !A->x)) ? cs_spfree (A) : A) ;
}
cs *cs_spalloc(scs_int m, scs_int n, scs_int nzmax, scs_int values, scs_int triplet) {
cs *A = (cs *) cs_calloc(1, sizeof(cs)); /* allocate the cs struct */
if (!A)
return (NULL); /* out of memory */
A->m = m; /* define dimensions and nzmax */
A->n = n;
A->nzmax = nzmax = MAX(nzmax, 1);
A->nz = triplet ? 0 : -1; /* allocate triplet or comp.col */
A->p = (scs_int *) cs_malloc(triplet ? nzmax : n + 1, sizeof(scs_int));
A->i = (scs_int *) cs_malloc(nzmax, sizeof(scs_int));
A->x = values ? (scs_float *) cs_malloc(nzmax, sizeof(scs_float)) : (scs_float *) NULL;
return ((!A->p || !A->i || (values && !A->x)) ? cs_spfree(A) : A);
}
/* 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_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 */
}
/* [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 */
}
/* sparse QR factorization [V,beta,pinv,R] = qr (A) */
csn *cs_qr (const cs *A, const css *S)
{
CS_ENTRY *Rx, *Vx, *Ax, *x ;
double *Beta ;
CS_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 (CS_INT)) ; /* get CS_INT workspace */
x = cs_malloc (m2, sizeof (CS_ENTRY)) ; /* get CS_ENTRY 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 */
}
csn *cs_chol(const cs *A, const css *S) {
double d, lki;
double *Lx, *x, *Cx;
int top, i, p, k, n, *Li, *Lp, *cp, *pinv, *s, *c, *parent, *Cp, *Ci;
cs *L, *C, *E;
csn *N;
if (!CS_CSC (A) || !S || !S->cp || !S->parent)
return (NULL);
n = A->n;
N = (csn *) cs_calloc(1, sizeof(csn)); /* allocate result */
c = (int *) cs_malloc(2 * n, sizeof(int)); /* get int workspace */
x = (double *) cs_malloc(n, sizeof(double)); /* get double workspace */
cp = S->cp;
pinv = S->pinv;
parent = S->parent;
C = pinv ? cs_symperm(A, pinv, 1) : ((cs *) A);
E = pinv ? C : NULL; /* E is alias for A, or a copy E=A(p,p) */
if (!N || !c || !x || !C)
return (cs_ndone(N, E, c, x, 0));
s = c + n;
Cp = C->p;
Ci = C->i;
Cx = C->x;
N->L = L = cs_spalloc(n, n, cp[n], 1, 0); /* allocate result */
if (!L)
return (cs_ndone(N, E, c, x, 0));
Lp = L->p;
Li = L->i;
Lx = L->x;
for (k = 0; k < n; k++)
Lp[k] = c[k] = cp[k];
for (k = 0; k < n; k++) /* compute L(k,:) for L*L' = C */
{
/* --- Nonzero pattern of L(k,:) ------------------------------------ */
top = cs_ereach(C, k, parent, s, c); /* find pattern of L(k,:) */
x[k] = 0; /* x (0:k) is now zero */
for (p = Cp[k]; p < Cp[k + 1]; p++) /* x = full(triu(C(:,k))) */
{
if (Ci[p] <= k)
x[Ci[p]] = Cx[p];
}
d = x[k]; /* d = C(k,k) */
x[k] = 0; /* clear x for k+1st iteration */
/* --- Triangular solve --------------------------------------------- */
for (; top < n; top++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */
{
i = s[top]; /* s [top..n-1] is pattern of L(k,:) */
lki = x[i] / Lx[Lp[i]]; /* L(k,i) = x (i) / L(i,i) */
x[i] = 0; /* clear x for k+1st iteration */
for (p = Lp[i] + 1; p < c[i]; p++) {
x[Li[p]] -= Lx[p] * lki;
}
d -= lki * lki; /* d = d - L(k,i)*L(k,i) */
p = c[i]++;
Li[p] = k; /* store L(k,i) in column i */
Lx[p] = lki;
}
/* --- Compute L(k,k) ----------------------------------------------- */
if (d <= 0)
return (cs_ndone(N, E, c, x, 0)); /* not pos def */
p = c[k]++;
Li[p] = k; /* store L(k,k) = sqrt (d) in column k */
Lx[p] = sqrt(d);
}
Lp[n] = cp[n]; /* finalize L */
return (cs_ndone(N, E, c, x, 1)); /* success: free E,s,x; return N */
}
double step(const cs *X, const CS_INT K, const double B,
const double *pZin, const double *pWgZin, const double *pDgZin,
double *pZout, double *pWgZout, double *pDgZout) {
bool debug = 0;
CS_INT p;
CS_INT j;
CS_INT k;
CS_INT nn, mm;
if(debug) printf("About to calloc\n");
double *s = cs_calloc(K, sizeof(double));
if(debug) printf("Calloc'd\n");
double st;
double pZnorm = 0;
double ll = 0;
double lls;
double sum;
double *Xx = X->x; /* non-zero values in X */
CS_INT *Xp = X->p; /* # elements in column */
CS_INT *Xi = X->i; /* row indices of X */
CS_INT n = X->n; /* Number of columns in X */
CS_INT m = X->m; /* Number of rows in X */
/* As a data structure reference, the matrix :
A = [ 4.5 0 3.2 0 ]
[ 3.1 2.9 0 0.9 ]
[ 0 1.7 3.0 0 ]
[ 3.5 0.4 0 1.0 ]
Would be stored as:
int p [] = { 0, 3, 6, 8, 10 };
int i [] = { 0, 1, 3, 1, 2, 3, 0, 2, 1, 3 };
double x [] = { 4.5, 3.1, 3.5, 2.9, 1.7, 0.4, 3.2, 3.0, 0.9, 1.0 };
*/
if(debug) printf("About to enter loop\n");
for ( j = 0; j < n; j++) { /* Column Index */
for ( p = Xp[j]; p < Xp[j+1]; p++) { /* Index into Xx for X(n,m) */
sum = 0;
nn = j;
mm = Xi[p];
for ( k = 0; k < K; k++) {
st = pow(pWgZin[k + K * mm] * pDgZin[k + K * nn], B) * pZin[k];
sum += st;
s[k] = st;
}
ll += log(sum) * Xx[p];
for ( k = 0; k < K; k++) {
st = (s[k] /* Xx[p]*/) / sum;
pWgZout[k + K * mm] += st;
pDgZout[k + K * nn] += st;
pZout[k] += st;
pZnorm += st;
}
}
}
if(debug) printf("About to normalize\n");
CS_INT c = 0;
/* Normalize pWgZ and pDgZ */
for ( j = 0; j < m; j++) {
c = K*j;
for (k = 0; k < K; k++) {
pWgZout[k + c] /= pZout[k];
}
}
if(debug) printf("Normalized pWgZout\n");
for ( j = 0; j < n; j++) {
c = K*j;
for (k = 0; k < K; k++) {
pDgZout[k + c] /= pZout[k];
}
}
if(debug) printf("Normalized pDgZout\n");
/* Normalize pZ */
for ( k = 0; k < K; k++) {
pZout[k] /= pZnorm;
}
if (debug) {
printf("pZ:\n");
for ( k = 0; k < K; k++) {
printf("%d, %f\n", k, pZout[k]);
}
}
cs_free(s);
if(debug) printf("Returning\n");
return ll;
}
