void SparseCholeskySolver<TMatrix,TVector>::invert(Matrix& M) {
int order = -1; //?????
if (S) cs_sfree(S);
if (N) cs_nfree(N);
//if (tmp) cs_free(tmp);
M.compress();
A.nzmax = M.getColsValue().size(); // maximum number of entries
A_p = (int *) &(M.getRowBegin()[0]);
A_i = (int *) &(M.getColsIndex()[0]);
A_x.resize(A.nzmax);
for (int i=0;i<A.nzmax;i++) A_x[i] = (double) M.getColsValue()[i];
//remplir A avec M
A.m = M.rowBSize(); // number of rows
A.n = M.colBSize(); // number of columns
A.p = A_p; // column pointers (size n+1) or col indices (size nzmax)
A.i = A_i; // row indices, size nzmax
A.x = (double*) &(A_x[0]); // numerical values, size nzmax
A.nz = -1; // # of entries in triplet matrix, -1 for compressed-col
cs_dropzeros( &A );
//M.check_matrix();
//CompressedRowSparseMatrix<double>::check_matrix(-1 /*A.nzmax*/,A.m,A.n,A.p,A.i,A.x);
//sout << "diag =";
//for (int i=0;i<A.n;++i) sout << " " << M.element(i,i);
//sout << sendl;
//sout << "SparseCholeskySolver: start factorization, n = " << A.n << " nnz = " << A.p[A.n] << sendl;
//tmp = (double *) cs_malloc (A.n, sizeof (double)) ;
tmp.resize(A.n);
S = cs_schol (&A, order) ; /* ordering and symbolic analysis */
N = cs_chol (&A, S) ; /* numeric Cholesky factorization */
//sout << "SparseCholeskySolver: factorization complete, nnz = " << N->L->p[N->L->n] << sendl;
}
void CSparseCholeskyInternal::prepare() {
prepared_ = false;
// Get a reference to the nonzeros of the linear system
const vector<double>& linsys_nz = input().data();
// Make sure that all entries of the linear system are valid
for (int k=0; k<linsys_nz.size(); ++k) {
casadi_assert_message(!isnan(linsys_nz[k]), "Nonzero " << k << " is not-a-number");
casadi_assert_message(!isinf(linsys_nz[k]), "Nonzero " << k << " is infinite");
}
if (verbose()) {
userOut() << "CSparseCholeskyInternal::prepare: numeric factorization" << endl;
userOut() << "linear system to be factorized = " << endl;
input(0).printSparse();
}
if (L_) cs_nfree(L_);
L_ = cs_chol(&AT_, S_) ; // numeric Cholesky factorization
if (L_==0) {
DMatrix temp = input();
temp.makeSparse();
if (temp.sparsity().issingular()) {
stringstream ss;
ss << "CSparseCholeskyInternal::prepare: factorization failed due "
"to matrix being singular. Matrix contains numerical zeros which are"
" structurally non-zero. Promoting these zeros to be structural "
"zeros, the matrix was found to be structurally rank deficient. "
"sprank: " << sprank(temp.sparsity()) << " <-> " << temp.size2() << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
} else {
stringstream ss;
ss << "CSparseCholeskyInternal::prepare: factorization failed, "
"check if Jacobian is singular" << endl;
if (verbose()) {
ss << "Sparsity of the linear system: " << endl;
input(LINSOL_A).sparsity().print(ss); // print detailed
}
throw CasadiException(ss.str());
}
}
casadi_assert(L_!=0);
prepared_ = true;
}
void CSparseCholeskyInterface::factorize(void* mem, const double* A) const {
auto m = static_cast<CsparseCholMemory*>(mem);
// Set the nonzeros of the matrix
m->A.x = const_cast<double*>(A);
// Make sure that all entries of the linear system are valid
int nnz = m->nnz();
for (int k=0; k<nnz; ++k) {
casadi_assert_message(!isnan(A[k]), "Nonzero " << k << " is not-a-number");
casadi_assert_message(!isinf(A[k]), "Nonzero " << k << " is infinite");
}
if (m->L) cs_nfree(m->L);
m->L = cs_chol(&m->A, m->S) ; // numeric Cholesky factorization
casadi_assert(m->L!=0);
}
cs *cs_rinvwishart(const cs *A, double nu, const css *As){
int m, i, j, cnt;
cs *T, *IW, *C, *W, *tC;
csn *U;
m = A->n;
T = cs_spalloc (m, m, m*(m+1)/2, 1, 0) ;
if (!T ) return (cs_done (T, NULL, NULL, 0));
double df = nu;
cnt = 0;
for(i = 0; i<m; i++){
T->p[i] = cnt;
T->i[cnt] = i;
T->x[cnt] = sqrt(rchisq(df));
cnt++;
for(j = i+1; j<m; j++){
T->i[cnt] = j;
T->x[cnt] = rnorm(0.0,1.0);
cnt++;
}
df--;
}
T->p[m] = m*(m+1)/2;
U = cs_chol(A, As);
if(U==NULL){
PutRNGstate();
error("ill-conditioned cross-product: can't form Cholesky factor\n");
}
C = cs_multiply(U->L,T); // t(T)%*%chol(A)
tC = cs_transpose(C, TRUE); // t(CI)
W = cs_multiply(C,tC);
IW = cs_inv(W); // crossprod(t(CI))
cs_spfree(T);
cs_nfree(U);
cs_spfree(C);
cs_spfree(tC);
cs_spfree(W);
return (cs_done (IW, NULL, NULL, 1)) ; /* success; free workspace, return C */
}
/**
* @brief Performs Cholesky factorization on a matrix.
*
* @param[in,out] chold Cholesky factorization of the matrix.
* @param[in] A Matrix to factorize.
* @return 0 on success.
*/
static int cs_fact_init_cholesky( cs_fact_t *chold, const cs *A )
{
int order = 1; /* order 0:natural, 1:Chol, 2:LU, 3:QR */
chold->S = cs_schol( order, A );
if (chold->S == NULL)
goto err_S;
chold->N = cs_chol( A, chold->S );
if (chold->N == NULL)
goto err_N;
chold->x = cs_malloc( A->n, sizeof(double) );
if (chold->x == NULL)
goto err_x;
chold->type = CS_FACT_CHOLESKY;
return 0;
err_x:
cs_nfree( chold->N );
err_N:
cs_sfree( chold->S );
err_S:
return -1;
}
/* 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) ;
}
void choleskyDecomp_sparse(cs* matrixA, double* matrixB, double* matrixX,int size)
{
int i;
css *S;
csn *N;
S=cs_schol(1, matrixA);
N=cs_chol(matrixA, S);
cs_spfree(matrixA);
if(found_dc_sweep==0){
cs_ipvec(S->pinv, matrixB, matrixX, size);
cs_lsolve(N->L, matrixX);
cs_ltsolve(N->L, matrixX);
cs_pvec(S->pinv, matrixX, matrixB, size);
printf("X vector \n");
for(i=0;i<size;i++){
printf(" %.6lf ",matrixX[i]);
}
printf("\n");
}
else{
double value;
double *matrixB_temp = (double *)calloc(size,sizeof(double));
if (source > -1) {
for(value=start_value;value<=end_value;value=value+step){
matrixB[source-1]=value;
cs_ipvec(S->pinv, matrixB, matrixX, size);
cs_lsolve(N->L, matrixX);
cs_ltsolve(N->L, matrixX);
cs_pvec(S->pinv, matrixX, matrixB_temp, size);
printf("value= %lf Matrix X: \n",value);
for(i=0;i<size;i++){
printf(" %.6lf ",matrixX[i]);
}
printf("\n");
}
}
else {
if (sweep_node1!=0){
matrixB[sweep_node1-1]+=sweep_value-start_value;
}
if(sweep_node2!=0){
matrixB[sweep_node2-1]-=sweep_value-start_value;
}
for(value=start_value;value<=end_value;value=value+step) {
cs_ipvec(S->pinv, matrixB, matrixX, size);
cs_lsolve(N->L, matrixX);
cs_ltsolve(N->L, matrixX);
cs_pvec(S->pinv, matrixX, matrixB_temp, size);
if (sweep_node1!=0){
matrixB[sweep_node1-1]-=step;
}
if(sweep_node2!=0){
matrixB[sweep_node2-1]+=step;
}
printf("value= %lf Matrix X: \n",value);
for(i=0;i<size;i++){
printf(" %.6lf ",matrixX[i]);
}
printf("\n");
}
}
}
printf("\n");
}
/* Cholesky update/downdate */
int demo3 (problem *Prob)
{
cs *A, *C, *W = NULL, *WW, *WT, *E = NULL, *W2 ;
int n, k, *Li, *Lp, *Wi, *Wp, p1, p2, *p = NULL, ok ;
double *b, *x, *resid, *y = NULL, *Lx, *Wx, s, t, t1 ;
css *S = NULL ;
csn *N = NULL ;
if (!Prob || !Prob->sym || Prob->A->n == 0) return (0) ;
A = Prob->A ; C = Prob->C ; b = Prob->b ; x = Prob->x ; resid = Prob->resid;
n = A->n ;
if (!Prob->sym || n == 0) return (1) ;
rhs (x, b, n) ; /* compute right-hand side */
printf ("\nchol then update/downdate ") ;
print_order (1) ;
y = cs_malloc (n, sizeof (double)) ;
t = tic () ;
S = cs_schol (1, C) ; /* symbolic Chol, amd(A+A') */
printf ("\nsymbolic chol time %8.2f\n", toc (t)) ;
t = tic () ;
N = cs_chol (C, S) ; /* numeric Cholesky */
printf ("numeric chol time %8.2f\n", toc (t)) ;
if (!S || !N || !y) return (done3 (0, S, N, y, W, E, p)) ;
t = tic () ;
cs_ipvec (S->pinv, b, y, n) ; /* y = P*b */
cs_lsolve (N->L, y) ; /* y = L\y */
cs_ltsolve (N->L, y) ; /* y = L'\y */
cs_pvec (S->pinv, y, x, n) ; /* x = P'*y */
printf ("solve chol time %8.2f\n", toc (t)) ;
printf ("original: ") ;
print_resid (1, C, x, b, resid) ; /* print residual */
k = n/2 ; /* construct W */
W = cs_spalloc (n, 1, n, 1, 0) ;
if (!W) return (done3 (0, S, N, y, W, E, p)) ;
Lp = N->L->p ; Li = N->L->i ; Lx = N->L->x ;
Wp = W->p ; Wi = W->i ; Wx = W->x ;
Wp [0] = 0 ;
p1 = Lp [k] ;
Wp [1] = Lp [k+1] - p1 ;
s = Lx [p1] ;
srand (1) ;
for ( ; p1 < Lp [k+1] ; p1++)
{
p2 = p1 - Lp [k] ;
Wi [p2] = Li [p1] ;
Wx [p2] = s * rand () / ((double) RAND_MAX) ;
}
t = tic () ;
ok = cs_updown (N->L, +1, W, S->parent) ; /* update: L*L'+W*W' */
t1 = toc (t) ;
printf ("update: time: %8.2f\n", t1) ;
if (!ok) return (done3 (0, S, N, y, W, E, p)) ;
t = tic () ;
cs_ipvec (S->pinv, b, y, n) ; /* y = P*b */
cs_lsolve (N->L, y) ; /* y = L\y */
cs_ltsolve (N->L, y) ; /* y = L'\y */
cs_pvec (S->pinv, y, x, n) ; /* x = P'*y */
t = toc (t) ;
p = cs_pinv (S->pinv, n) ;
W2 = cs_permute (W, p, NULL, 1) ; /* E = C + (P'W)*(P'W)' */
WT = cs_transpose (W2,1) ;
WW = cs_multiply (W2, WT) ;
cs_spfree (WT) ;
cs_spfree (W2) ;
E = cs_add (C, WW, 1, 1) ;
cs_spfree (WW) ;
if (!E || !p) return (done3 (0, S, N, y, W, E, p)) ;
printf ("update: time: %8.2f (incl solve) ", t1+t) ;
print_resid (1, E, x, b, resid) ; /* print residual */
cs_nfree (N) ; /* clear N */
t = tic () ;
N = cs_chol (E, S) ; /* numeric Cholesky */
if (!N) return (done3 (0, S, N, y, W, E, p)) ;
cs_ipvec (S->pinv, b, y, n) ; /* y = P*b */
cs_lsolve (N->L, y) ; /* y = L\y */
cs_ltsolve (N->L, y) ; /* y = L'\y */
cs_pvec (S->pinv, y, x, n) ; /* x = P'*y */
t = toc (t) ;
printf ("rechol: time: %8.2f (incl solve) ", t) ;
print_resid (1, E, x, b, resid) ; /* print residual */
t = tic () ;
ok = cs_updown (N->L, -1, W, S->parent) ; /* downdate: L*L'-W*W' */
t1 = toc (t) ;
if (!ok) return (done3 (0, S, N, y, W, E, p)) ;
printf ("downdate: time: %8.2f\n", t1) ;
t = tic () ;
cs_ipvec (S->pinv, b, y, n) ; /* y = P*b */
cs_lsolve (N->L, y) ; /* y = L\y */
cs_ltsolve (N->L, y) ; /* y = L'\y */
cs_pvec (S->pinv, y, x, n) ; /* x = P'*y */
t = toc (t) ;
printf ("downdate: time: %8.2f (incl solve) ", t1+t) ;
print_resid (1, C, x, b, resid) ; /* print residual */
return (done3 (1, S, N, y, W, E, p)) ;
}
void solve_spdSparse(double time){
int i;
double current_value;
double *B_sparse_temp;
//Adeiasma twn arxeiwn sta opoia tha apothikeutoun ta apotelesmata tis analysis gia tous komvous PLOT
if(TRAN==0)initPlotFiles("Sparse");
if(ITER==0){
//cholesky decomposition
S=cs_schol(1, C_sparse);
N=cs_chol(C_sparse, S);
//cs_spfree(C_sparse);
}
if(dc_sweep==0){ //An den exoume sweep
if(ITER==0){
cs_ipvec(S->pinv, B_sparse, x_sparse, sizeB);
cs_lsolve(N->L, x_sparse); //seg fault gia choleskyNetlist!LA8OS TO N!ARA ANADROMIKA LA8OS TO C_sparse.Omws C_sparce swsto vash ths CG.
cs_ltsolve(N->L, x_sparse);
cs_pvec(S->pinv, x_sparse, B_sparse, sizeB); //solve cholesky
for(i=0;i<sizeB;i++){
x_sparse[i]=B_sparse[i];
}
//printf("\n ----- SPARSE Cholesky decomposition ----- \n");
}
else{
conjugate_gradient_sparse(C_sparse,B_sparse, sizeB, x_sparse, itol_value); //solve with Conjugated Gradient
//printf("\n");
//printf("---- CG SPARSE ----\n");
}
/*
printf("X = \n");
for(i=0;i<sizeB;i++){
printf(" %.6lf \n",x_sparse[i]);
}
printf("----------------------\n");
printf("\n");
*/
if(TRAN==0){
plotFiles("Sparse", x_sparse, -1.0, "Analysis: DC");
}else{
plotFiles("Sparse", x_sparse, time, "Analysis: TRAN");
}
}else{ //An exoume sweep
B_sparse_temp = (double *)calloc(sizeB,sizeof(double)); //Proswrini apothikeusi tou apotelesmatos anamesa sta loop
if(sweep_source!=-1){ //pigi tasis ginetai sweep
for(current_value=start_value;current_value<=end_value+EPS;current_value+=sweep_step){
B_sparse[sweep_source-1]=current_value;
if(ITER==0){
cs_ipvec(S->pinv, B_sparse, x_sparse, sizeB);
cs_lsolve(N->L, x_sparse);
cs_ltsolve(N->L, x_sparse);
cs_pvec(S->pinv, x_sparse, B_sparse_temp, sizeB); //solve cholesky
}
else{
conjugate_gradient_sparse(C_sparse,B_sparse, sizeB, x_sparse, itol_value);
}
//Apothikeusi twn apotelesmatwn tis analysis gia tous komvous PLOT se arxeia
plotFiles("Sparse", (ITER ? x_sparse:B_sparse_temp), current_value, "Sweep source voltage at");
}
}else{ //pigi reumatos ginetai sweep
//Anairesi twn praksewn + kai - apo tin arxiki timi tis pigis ston pinaka B
//kai praksi + kai - me to start_value
if(sweep_posNode!=0){
B_sparse[sweep_posNode-1]+=sweep_value_I-start_value;
}
if(sweep_negNode!=0){
B_sparse[sweep_negNode-1]-=sweep_value_I-start_value;
}
for(current_value=start_value;current_value<=end_value+EPS;current_value+=sweep_step){
if(ITER==0){
cs_ipvec(S->pinv, B_sparse, x_sparse, sizeB);
cs_lsolve(N->L, x_sparse);
cs_ltsolve(N->L, x_sparse);
cs_pvec(S->pinv, x_sparse, B_sparse_temp, sizeB); //solve cholesky
}
else{
conjugate_gradient_sparse(C_sparse,B_sparse, sizeB, x_sparse, itol_value);
//Arxiki proseggish h lush ths prohgoumenhs
}
//Allagi twn timwn ston pinaka B gia to epomeno vima tou sweep
if(sweep_posNode!=0){
B_sparse[sweep_posNode-1]-=sweep_step;
}
if(sweep_negNode!=0){
B_sparse[sweep_negNode-1]+=sweep_step;
}
//Apothikeusi twn apotelesmatwn tis analysis gia tous komvous PLOT se arxeia
plotFiles("Sparse", (ITER ? x_sparse:B_sparse_temp), current_value, "Sweep source current at");
}
printf("\n");
}
}
}