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)); }
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 */ }