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));
}
/* cs_symperm: symmetric permutation of a symmetric sparse matrix. */
void mexFunction
(
int nargout,
mxArray *pargout [ ],
int nargin,
const mxArray *pargin [ ]
)
{
cs Amatrix, *A, *C, *D ;
csi ignore, n, *P, *Pinv ;
if (nargout > 1 || nargin != 2)
{
mexErrMsgTxt ("Usage: C = cs_symperm(A,p)") ;
}
A = cs_mex_get_sparse (&Amatrix, 1, 1, pargin [0]) ;
n = A->n ;
P = cs_mex_get_int (n, pargin [1], &ignore, 1) ; /* get P */
Pinv = cs_pinv (P, n) ; /* P=Pinv' */
C = cs_symperm (A, Pinv, 1) ; /* C = A(p,p) */
D = cs_transpose (C, 1) ; /* sort C */
cs_spfree (C) ;
C = cs_transpose (D, 1) ;
cs_spfree (D) ;
pargout [0] = cs_mex_put_sparse (&C) ; /* return C */
cs_free (P) ;
cs_free (Pinv) ;
}
Sparsity CSparseCholeskyInterface::linsol_cholesky_sparsity(void* mem, bool tr) const {
auto m = static_cast<CsparseCholMemory*>(mem);
casadi_assert(m->S);
int n = m->A.n;
int nzmax = m->S->cp[n];
std::vector< int > row(n+1);
std::copy(m->S->cp, m->S->cp+n+1, row.begin());
std::vector< int > colind(nzmax);
int *Li = &colind.front();
int *Lp = &row.front();
const cs* C;
C = m->S->pinv ? cs_symperm(&m->A, m->S->pinv, 1) : &m->A;
std::vector< int > temp(2*n);
int *c = &temp.front();
int *s = c+n;
for (int k = 0 ; k < n ; k++) c[k] = m->S->cp[k] ;
for (int k = 0 ; k < n ; k++) { /* compute L(k, :) for L*L' = C */
int top = cs_ereach(C, k, m->S->parent, s, c) ;
for ( ; top < n ; top++) { /* solve L(0:k-1, 0:k-1) * x = C(:, k) */
int i = s[top] ; /* s[top..n-1] is pattern of L(k, :) */
int p = c[i]++ ;
Li[p] = k ; /* store L(k, i) in row i */
}
int p = c[k]++ ;
Li[p] = k ;
}
Lp[n] = m->S->cp[n] ;
Sparsity ret(n, n, row, colind); // BUG?
return tr ? ret.T() : ret;
}
CRSSparsity CSparseCholeskyInternal::getFactorizationSparsity() const {
casadi_assert(S_);
int n = AT_.n;
int nzmax = S_->cp[n];
std::vector< int > col(n+1);
std::copy(S_->cp,S_->cp+n+1,col.begin());
std::vector< int > rowind(nzmax);
int *Li = &rowind.front();
int *Lp = &col.front();
const cs* C;
C = S_->pinv ? cs_symperm (&AT_, S_->pinv, 1) : &AT_;
std::vector< int > temp(2*n);
int *c = & temp.front();
int *s = c+n;
for (int k = 0 ; k < n ; k++) c [k] = S_->cp [k] ;
for (int k = 0 ; k < n ; k++) { /* compute L(k,:) for L*L' = C */
int top = cs_ereach (C, k, S_->parent, s, c) ;
for ( ; top < n ; top++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */
{
int i = s [top] ; /* s [top..n-1] is pattern of L(k,:) */
int p = c [i]++ ;
Li [p] = k ; /* store L(k,i) in column i */
}
int p = c [k]++ ;
Li [p] = k ;
}
Lp [n] = S_->cp [n] ;
return trans(CRSSparsity(n, n, rowind, col));
}
scs_int factorize(const AMatrix * A, const Settings * stgs, Priv * p) {
scs_float *info;
scs_int *Pinv, amd_status, ldl_status;
cs *C, *K = formKKT(A, stgs);
if (!K) {
return -1;
}
amd_status = LDLInit(K, p->P, &info);
if (amd_status < 0)
return (amd_status);
#if EXTRAVERBOSE > 0
if(stgs->verbose) {
scs_printf("Matrix factorization info:\n");
#ifdef DLONG
amd_l_info(info);
#else
amd_info(info);
#endif
}
#endif
Pinv = cs_pinv(p->P, A->n + A->m);
C = cs_symperm(K, Pinv, 1);
ldl_status = LDLFactor(C, NULL, NULL, &p->L, &p->D);
cs_spfree(C);
cs_spfree(K);
scs_free(Pinv);
scs_free(info);
return (ldl_status);
}
int cs_rechol(const cs *A, const csn *N, int *pinv, int *c, double *x) {
double d, lki;
double *Lx, *Cx;
int t, i, p, k, n, *Li, *Lp, *Ui, *Up, *Cp, *Ci;
cs *C, *E;
if (!CS_CSC (A) || !N || !N->L || !N->U || !c || !x)
return (0);
n = A->n;
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 (!C) {
cs_spfree(E);
return (0);
}
Cp = C->p;
Ci = C->i;
Cx = C->x;
Lp = N->L->p;
Li = N->L->i;
Lx = N->L->x;
Up = N->U->p;
Ui = N->U->i;
for (k = 0; k < n; k++) /* compute L(k,:) for L*L' = C */
{
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 */
for (t = Up[k]; t < Up[k + 1] - 1; t++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */
/* the loop does not include the diagonal element of row k of L - or column k of U - */
/* (which is placed at the end of column k for a sorted matrix - after its transposition), */
/* because it is treated outside the loop */
{
i = Ui[t]; /* 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]++;
Lx[p] = lki;
}
if (d <= 0) {
cs_spfree(E);
return (0);
} /* not pos def */
p = c[k]++;
Lx[p] = sqrt(d);
}
cs_spfree(E);
return (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 */
}